wip

libraries/AP_AHRS/AP_AHRS.cpp

@@ -2413,13 +2413,13 @@ void  AP_AHRS::writeBodyFrameOdom(float quality, const Vector3f &delPos, const V
 }
 
 // Write position and quaternion data from an external navigation system
-void AP_AHRS::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
+void AP_AHRS::writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
 {
 #if HAL_NAVEKF2_AVAILABLE
-    EKF2.writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
+    EKF2.writeExtNavData(pos, quat, posCov, angErr, timeStamp_ms-delay_ms, resetTime_ms);
 #endif
 #if HAL_NAVEKF3_AVAILABLE
-    EKF3.writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
+    EKF3.writeExtNavData(pos, quat, posCov, angErr, timeStamp_ms-delay_ms, resetTime_ms);
 #endif
 }
 

libraries/AP_AHRS/AP_AHRS.h

@@ -303,7 +303,7 @@ class AP_AHRS {
     void writeDefaultAirSpeed(float airspeed, float uncertainty);
 
     // Write position and quaternion data from an external navigation system
-    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
+    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
 
     // Write velocity data from an external navigation system
     void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);

libraries/AP_DAL/AP_DAL.cpp

@@ -355,17 +355,21 @@ void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawF
 }
 
 // log external navigation data
-void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
+void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
 {
     end_frame();
 
     const log_REPH old = _REPH;
     _REPH.pos = pos;
     _REPH.quat = quat;
-    _REPH.posErr = posErr;
+    _REPH.posCov_0 = posCov[0];
+    _REPH.posCov_1 = posCov[1];
+    _REPH.posCov_2 = posCov[2];
+    _REPH.posCov_3 = posCov[3];
+    _REPH.posCov_4 = posCov[4];
+    _REPH.posCov_5 = posCov[5];
     _REPH.angErr = angErr;
     _REPH.timeStamp_ms = timeStamp_ms;
-    _REPH.delay_ms = delay_ms;
     _REPH.resetTime_ms = resetTime_ms;
     WRITE_REPLAY_BLOCK_IFCHANGED(REPH, _REPH, old);
 }

libraries/AP_DAL/AP_DAL.h

@@ -216,7 +216,7 @@ class AP_DAL {
     void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
 
     // log external nav data
-    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
+    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
     void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
 
     // log wheel odometry data

libraries/AP_DAL/LogStructure.h

@@ -332,7 +332,12 @@ struct log_ROFH {
 struct log_REPH {
     Vector3f pos;
     Quaternion quat;
-    float posErr;
+    float posCov_0;
+    float posCov_1;
+    float posCov_2;
+    float posCov_3;
+    float posCov_4;
+    float posCov_5;
     float angErr;
     uint32_t timeStamp_ms;
     uint32_t resetTime_ms;
@@ -442,7 +447,7 @@ struct log_RBOH {
     { LOG_ROFH_MSG, RLOG_SIZE(ROFH),                                   \
       "ROFH", "ffffIffffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,HgtOvr,Qual", "----------", "----------" }, \
     { LOG_REPH_MSG, RLOG_SIZE(REPH),                                   \
-      "REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
+      "REPH", "ffffffffffffffIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,NN,NE,ND,EE,ED,DD,AEr,TS,D", "----------------", "----------------" }, \
     { LOG_RSLL_MSG, RLOG_SIZE(RSLL),                         \
       "RSLL", "IIfI", "Lat,Lng,PosAccSD,TS", "DU--", "GG--" }, \
     { LOG_REVH_MSG, RLOG_SIZE(REVH),                                   \

libraries/AP_DDS/AP_DDS_External_Odom.cpp

@@ -31,16 +31,17 @@ void AP_DDS_External_Odom::handle_external_odom(const tf2_msgs_msg_TFMessage& ms
         // TODO what if the quaternion is NaN?
         ap_rotation.normalize();
 
-        // No error is available in TF, trust the data as-is
-        const float posErr {0.0};
-        const float angErr {0.0};
+        // No error is available in TF, setting these to NAN will cause SW to use parameter defined accuracy
+        float posCov[6];
+        posCov[0] = NAN;
+        const float angErr = NAN;
         // The odom to base_link transform used is locally consistent per ROS REP-105.
         // https://www.ros.org/reps/rep-0105.html#id16
         // Thus, there will not be any resets.
         const uint8_t reset_counter {0};
         // TODO implement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg));
         const uint32_t time_ms {static_cast<uint32_t>(remote_time_us * 1E-3)};
-        visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter, 0);
+        visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posCov, angErr, reset_counter, 0);
 
     }
 }

libraries/AP_NavEKF/AP_NavEKF_Source.cpp

@@ -295,6 +295,7 @@ void AP_NavEKF_Source::align_inactive_sources()
 bool AP_NavEKF_Source::usingGPS() const
 {
     return getPosXYSource() == SourceXY::GPS ||
+           getPosXYSource() == SourceXY::GPSANDEXTNAV ||
            getPosZSource() == SourceZ::GPS ||
            getVelXYSource() == SourceXY::GPS ||
            getVelZSource() == SourceZ::GPS ||
@@ -352,6 +353,10 @@ bool AP_NavEKF_Source::pre_arm_check(bool requires_position, char *failure_msg,
             case SourceXY::EXTNAV:
                 visualodom_required = true;
                 break;
+            case SourceXY::GPSANDEXTNAV:
+                gps_required = true;
+                visualodom_required = true;
+                break;
             case SourceXY::OPTFLOW:
             case SourceXY::WHEEL_ENCODER:
             default:
@@ -524,7 +529,7 @@ bool AP_NavEKF_Source::ext_nav_enabled(void) const
 {
     for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
         const auto &src = _source_set[i];
-        if (src.posxy == SourceXY::EXTNAV) {
+        if (src.posxy == SourceXY::EXTNAV || src.posxy == SourceXY::GPSANDEXTNAV) {
             return true;
         }
         if (src.posz == SourceZ::EXTNAV) {

libraries/AP_NavEKF/AP_NavEKF_Source.h

@@ -22,7 +22,8 @@ class AP_NavEKF_Source
         BEACON = 4,
         OPTFLOW = 5,
         EXTNAV = 6,
-        WHEEL_ENCODER = 7
+        WHEEL_ENCODER = 7,
+        GPSANDEXTNAV = 8,
     };
 
     enum class SourceZ : uint8_t {

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -1512,23 +1512,22 @@ void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
 /*
  * Write position and quaternion data from an external navigation system
  *
- * pos        : XYZ position (m) in a RH navigation frame with the Z axis pointing down and XY axes horizontal. Frame must be aligned with NED if the magnetomer is being used for yaw.
- * quat       : quaternion describing the rotation from navigation frame to body frame
- * posErr     : 1-sigma spherical position error (m)
- * angErr     : 1-sigma spherical angle error (rad)
- * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
- * delay_ms   : average delay of external nav system measurements relative to inertial measurements
- * resetTime_ms : system time of the last position reset request (mSec)
+     * pos        : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
+     * quat       : quaternion desribing the rotation from navigation frame to body frame
+     * posCov     ; Row-major representation of position 3x3 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global; first three entries are the first ROW, next 2 entries are the second ROW, etc.). If position variances are unknown, assign NaN value to element [0].
+     * angErr     : 1-sigma spherical angle error (rad). Assign NaN value if not known.
+     * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
+     * resetTime_ms : system time of the last position reset request (mSec)     *
  *
  * Sensor offsets are pulled directly from the AP_VisualOdom library
  *
 */
-void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
+void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
 {
-    AP::dal().writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
+    AP::dal().writeExtNavData(pos, quat, posCov, angErr, timeStamp_ms, resetTime_ms);
     if (!option_is_set(Option::DisableExternalNav) && core) {
         for (uint8_t i=0; i<num_cores; i++) {
-            core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
+            core[i].writeExtNavData(pos, quat, posCov, angErr, timeStamp_ms, resetTime_ms);
         }
     }
 }

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -250,18 +250,17 @@ class NavEKF2 {
     /*
      * Write position and quaternion data from an external navigation system
      *
-     * pos        : position in the RH navigation frame. Frame is assumed to be NED (m)
+     * pos        : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
      * quat       : quaternion desribing the rotation from navigation frame to body frame
-     * posErr     : 1-sigma spherical position error (m)
-     * angErr     : 1-sigma spherical angle error (rad)
+     * posCov     ; Row-major representation of position 3x3 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global; first three entries are the first ROW, next 2 entries are the second ROW, etc.). If position variances are unknown, assign NaN value to element [0].
+     * angErr     : 1-sigma spherical angle error (rad). Assign NaN value if not known.
      * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
-     * delay_ms   : average delay of external nav system measurements relative to inertial measurements
-     * resetTime_ms : system time of the last position reset request (mSec)
+     * resetTime_ms : system time of the last position reset request (mSec)     *
      *
      * Sensor offsets are pulled directly from the AP_VisualOdom library
      *
     */
-    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
+    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
 
     /*
      * Write velocity data from an external navigation system

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -935,10 +935,10 @@ void NavEKF2_core::updateTimingStatistics(void)
     timing.count++;
 }
 
-void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
+void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
 {
     // protect against NaN
-    if (pos.is_nan() || isnan(posErr) || quat.is_nan() || isnan(angErr)) {
+    if (pos.is_nan() || isnan(posCov[0]) || isnan(posCov[3]) || isnan(posCov[5]) || quat.is_nan() || isnan(angErr)) {
         return;
     }
 
@@ -959,9 +959,9 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
 
     extNavDataNew.pos = pos.toftype();
     extNavDataNew.quat = quat.toftype();
-    extNavDataNew.posErr = posErr;
+    extNavDataNew.posErr = sqrtf((posCov[0]+posCov[3]+posCov[5]) / 3.0f);
     extNavDataNew.angErr = angErr;
-    timeStamp_ms = timeStamp_ms - delay_ms;
+    timeStamp_ms = timeStamp_ms;
     // Correct for the average intersampling delay due to the filter updaterate
     timeStamp_ms -= localFilterTimeStep_ms/2;
     // Prevent time delay exceeding age of oldest IMU data in the buffer

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -294,18 +294,17 @@ class NavEKF2_core : public NavEKF_core_common
     /*
      * Write position and quaternion data from an external navigation system
      *
-     * pos        : position in the RH navigation frame. Frame is assumed to be NED (m)
+     * pos        : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
      * quat       : quaternion desribing the rotation from navigation frame to body frame
-     * posErr     : 1-sigma spherical position error (m)
-     * angErr     : 1-sigma spherical angle error (rad)
+     * posCov     ; Row-major representation of position 3x3 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global; first three entries are the first ROW, next 2 entries are the second ROW, etc.). If position variances are unknown, assign NaN value to element [0].
+     * angErr     : 1-sigma spherical angle error (rad). Assign NaN value if not known.
      * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
-     * delay_ms   : average delay of external nav system measurements relative to inertial measurements
-     * resetTime_ms : system time of the last position reset request (mSec)
+     * resetTime_ms : system time of the last position reset request (mSec)     *
      *
      * Sensor offsets are pulled directly from the AP_VisualOdom library
      *
     */
-    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
+    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
 
     /*
      * Write velocity data from an external navigation system

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -743,6 +743,30 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
     // @User: Advanced
     AP_GROUPINFO("OPTIONS",  11, NavEKF3, _options, 0),
 
+    // @Param: EXT_TCONST
+    // @DisplayName: External navigation origin time constant (sec)
+    // @Description: This sets the time constant used to align the external navigation source NED origin with the EKF origin when a global position reference source such as GPS is available.
+    // @Range: 1.0 100.0
+    // @User: Advanced
+    // @Units: s
+    AP_GROUPINFO("EXT_TCONST", 12, NavEKF3, _extNavOriginTconst, 10.0f),
+
+    // @Param: EXT_M_NSE
+    // @DisplayName: External navigation error growth minimum (m)
+    // @Description: When set to a positive value, the external naivigation position data will be assumed to be from an odometry source that accumulates error over time and reports the uncertainty growth in the covariance. The EKF will then use the increase in position variance to set the observation variance, but with a lower bound set by this parameter.
+    // @Range: 0.0 10.0
+    // @User: Advanced
+    // @Units: m
+    AP_GROUPINFO("EXT_M_NSE", 13, NavEKF3, _extNavPosNoiseMin, 0.0f),
+
+    // @Param: EXT_TSHIFT
+    // @DisplayName: External navigsation time shift limit (sec)
+    // @Description: This sets the maximum number of seconds the external navigation data will be time shifted using current accelerationo and velocity estimates when it is time stamped behind the EKF fusion time horizon.
+    // @Range: 0.0 10.0
+    // @User: Advanced
+    // @Units: s
+    AP_GROUPINFO("EXT_TSHIFT", 14, NavEKF3, _extNavMaxTshift, 5.0f),
+
     AP_GROUPEND
 };
 
@@ -1402,10 +1426,12 @@ bool NavEKF3::setOriginLLH(const Location &loc)
     if (!core) {
         return false;
     }
-    if (common_origin_valid) {
-        // we don't allow setting the EKF origin if it has already been set
-        // this is to prevent causing upsets from a shifting origin.
-        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3: origin already set");
+    if ((sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS || sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPSANDEXTNAV) || common_origin_valid) {
+        // we don't allow setting of the EKF origin if using GPS
+        // or if the EKF origin has already been set.
+        // This is to prevent accidental setting of EKF origin with an
+        // invalid position or height or causing upsets from a shifting origin.
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 refusing set origin");
         return false;
     }
     bool ret = false;
@@ -1543,7 +1569,7 @@ bool NavEKF3::using_extnav_for_yaw() const
 bool NavEKF3::configuredToUseGPSForPosXY(void) const
 {
     // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity, 3 = do not use GPS
-    return  (sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS);
+    return  (sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS || sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPSANDEXTNAV);
 }
 
 // write the raw optical flow measurements
@@ -1590,24 +1616,21 @@ void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t tim
 }
 
 /*
- * Write position and quaternion data from an external navigation system
- *
- * pos        : XYZ position (m) in a RH navigation frame with the Z axis pointing down and XY axes horizontal. Frame must be aligned with NED if the magnetomer is being used for yaw.
- * quat       : quaternion describing the rotation from navigation frame to body frame
- * posErr     : 1-sigma spherical position error (m)
- * angErr     : 1-sigma spherical angle error (rad)
- * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
- * delay_ms   : average delay of external nav system measurements relative to inertial measurements
- * resetTime_ms : system time of the last position reset request (mSec)
- *
+    * Write pose and covariance data from an external navigation system     *
+    * pos        : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
+    * quat       : quaternion desribing the rotation from navigation frame to body frame
+    * posCov     ; Row-major representation of position 3x3 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global; first three entries are the first ROW, next 2 entries are the second ROW, etc.). If position variances are unknown, assign NaN value to element [0].
+    * angErr     : 1-sigma spherical angle error (rad). Assign NaN value if not known.
+    * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
+    * resetTime_ms : system time of the last position reset request (mSec)     *
 */
-void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
+void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, const float posCov[6], float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
 {
-    dal.writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
+    AP::dal().writeExtNavData(pos, quat, posCov, angErr, timeStamp_ms, resetTime_ms);
 
     if (core) {
         for (uint8_t i=0; i<num_cores; i++) {
-            core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
+            core[i].writeExtNavData(pos, quat, posCov, angErr, timeStamp_ms, resetTime_ms);
         }
     }
 }