AP_NavEKF2: external nav pos offsets from AP_VisualOdom library

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -5,6 +5,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
 #include <AP_GPS/AP_GPS.h>
+#include <AP_VisualOdom/AP_VisualOdom.h>
 #include <new>
 
 /*
@@ -1627,12 +1628,14 @@ void NavEKF2::getTimingStatistics(int8_t instance, struct ekf_timing &timing) co
  * 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 &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
+void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
 {
     if (core) {
         for (uint8_t i=0; i<num_cores; i++) {
-            core[i].writeExtNavData(sensOffset, pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms);
+            core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, resetTime_ms);
         }
     }
 }

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -337,8 +337,10 @@ public:
      * 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 writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
+    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
 
     /*
       check if switching lanes will reduce the normalised

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -893,7 +893,7 @@ void NavEKF2_core::getTimingStatistics(struct ekf_timing &_timing)
     memset(&timing, 0, sizeof(timing));
 }
 
-void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
+void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
 {
     // limit update rate to maximum allowed by sensor buffers and fusion process
     // don't try to write to buffer until the filter has been initialised
@@ -918,7 +918,6 @@ void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &p
         extNavDataNew.posErr = frontend->_gpsHorizPosNoise;
     }
     extNavDataNew.angErr = angErr;
-    extNavDataNew.body_offset = &sensOffset;
     timeStamp_ms = timeStamp_ms - frontend->_extnavDelay_ms;
     // Correct for the average intersampling delay due to the filter updaterate
     timeStamp_ms -= localFilterTimeStep_ms/2;

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -120,10 +120,12 @@ void NavEKF2_core::ResetPosition(void)
             lastRngBcnPassTime_ms = imuSampleTime_ms;
         } else if (imuSampleTime_ms - extNavDataDelayed.time_ms < 250) {
             // use external nav data as the third preference
-            stateStruct.position.x = extNavDataDelayed.pos.x;
-            stateStruct.position.y = extNavDataDelayed.pos.y;
+            ext_nav_elements extNavCorrected = extNavDataDelayed;
+            CorrectExtNavForSensorOffset(extNavCorrected.pos);
+            stateStruct.position.x = extNavCorrected.pos.x;
+            stateStruct.position.y = extNavCorrected.pos.y;
             // set the variances from the external nav filter
-            P[7][7] = P[6][6] = sq(extNavDataDelayed.posErr);
+            P[7][7] = P[6][6] = sq(extNavCorrected.posErr);
         }
     }
     for (uint8_t i=0; i<imu_buffer_length; i++) {
@@ -275,6 +277,25 @@ void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data)
     gps_data.hgt += posOffsetEarth.z;
 }
 
+// correct external navigation earth-frame position using sensor body-frame offset
+void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3f &ext_position)
+{
+#if HAL_VISUALODOM_ENABLED
+    AP_VisualOdom *visual_odom = AP::visualodom();
+    if (visual_odom == nullptr) {
+        return;
+    }
+    const Vector3f &posOffsetBody = visual_odom->get_pos_offset() - accelPosOffset;
+    if (posOffsetBody.is_zero()) {
+        return;
+    }
+    Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+    ext_position.x -= posOffsetEarth.x;
+    ext_position.y -= posOffsetEarth.y;
+    ext_position.z -= posOffsetEarth.z;
+#endif
+}
+
 /********************************************************
 *                   FUSE MEASURED_DATA                  *
 ********************************************************/
@@ -327,6 +348,10 @@ void NavEKF2_core::SelectVelPosFusion()
         fuseVelData = false;
         fuseHgtData = true;
         fusePosData = true;
+
+        // correct for external navigation sensor position
+        CorrectExtNavForSensorOffset(extNavDataDelayed.pos);
+
         velPosObs[3] = extNavDataDelayed.pos.x;
         velPosObs[4] = extNavDataDelayed.pos.y;
         velPosObs[5] = extNavDataDelayed.pos.z;

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -314,7 +314,6 @@ public:
     /*
      * Write position and quaternion data from an external navigation system
      *
-     * sensOffset : position of the external navigation sensor in body frame (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)
@@ -322,8 +321,10 @@ public:
      * 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 writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
+    void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms);
 
     // return true when external nav data is also being used as a yaw observation
     bool isExtNavUsedForYaw(void);
@@ -475,7 +476,6 @@ private:
         Quaternion      quat;       // quaternion describing the rotation from navigation to body frame
         float           posErr;     // spherical poition measurement error 1-std (m)
         float           angErr;     // spherical angular measurement error 1-std (rad)
-        const Vector3f *body_offset;// pointer to XYZ position of the sensor in body frame (m)
         uint32_t        time_ms;    // measurement timestamp (msec)
         bool            posReset;   // true when the position measurement has been reset
     };
@@ -779,6 +779,9 @@ private:
     // correct gps data for antenna position
     void CorrectGPSForAntennaOffset(gps_elements &gps_data);
 
+    // correct external navigation earth-frame position using sensor body-frame offset
+    void CorrectExtNavForSensorOffset(Vector3f &ext_position);
+
     // Length of FIFO buffers used for non-IMU sensor data.
     // Must be larger than the time period defined by IMU_BUFFER_LENGTH
     static const uint32_t OBS_BUFFER_LENGTH = 5;