From 2f1ae29a9cc27f3fd23c45fbc9b2881e1e25f9bc Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 22 Oct 2020 19:55:50 +0900
Subject: [PATCH] AP_NavEKF3: extnav data gets corrected boolean

CorrectExtNavForSensorOffset and CorrectExtNavVelForSensorOffset use
corrected boolean to avoid correcting twice
---
 .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp    |  3 +-
 .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp    | 28 +++++++++++++------
 libraries/AP_NavEKF3/AP_NavEKF3_core.h        |  6 ++--
 3 files changed, 26 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index e64ef734d99..50be0928d2b 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -1054,7 +1054,8 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
     const ext_nav_vel_elements extNavVelNew {
         vel,
         err,
-        timeStamp_ms
+        timeStamp_ms,
+        false
     };
     storedExtNavVel.push(extNavVelNew);
 }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index 0614d13fadb..0b5a99c924e 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -345,8 +345,14 @@ void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
 }
 
 // correct external navigation earth-frame position using sensor body-frame offset
-void NavEKF3_core::CorrectExtNavForSensorOffset(Vector3f &ext_position)
+void NavEKF3_core::CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data)
 {
+    // return immediately if already corrected
+    if (ext_nav_data.corrected) {
+        return;
+    }
+    ext_nav_data.corrected = true;
+
 #if HAL_VISUALODOM_ENABLED
     AP_VisualOdom *visual_odom = AP::visualodom();
     if (visual_odom == nullptr) {
@@ -357,15 +363,21 @@ void NavEKF3_core::CorrectExtNavForSensorOffset(Vector3f &ext_position)
         return;
     }
     Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
-    ext_position.x -= posOffsetEarth.x;
-    ext_position.y -= posOffsetEarth.y;
-    ext_position.z -= posOffsetEarth.z;
+    ext_nav_data.pos.x -= posOffsetEarth.x;
+    ext_nav_data.pos.y -= posOffsetEarth.y;
+    ext_nav_data.pos.z -= posOffsetEarth.z;
 #endif
 }
 
 // correct external navigation earth-frame velocity using sensor body-frame offset
-void NavEKF3_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
+void NavEKF3_core::CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav_vel_data) const
 {
+    // return immediately if already corrected
+    if (ext_nav_vel_data.corrected) {
+        return;
+    }
+    ext_nav_vel_data.corrected = true;
+
 #if HAL_VISUALODOM_ENABLED
     AP_VisualOdom *visual_odom = AP::visualodom();
     if (visual_odom == nullptr) {
@@ -377,7 +389,7 @@ void NavEKF3_core::CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const
     }
     // TODO use a filtered angular rate with a group delay that matches the sensor delay
     const Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
-    ext_velocity += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
+    ext_nav_vel_data.vel += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
 #endif
 }
 
@@ -401,11 +413,11 @@ void NavEKF3_core::SelectVelPosFusion()
     // Check for data at the fusion time horizon
     extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
     if (extNavDataToFuse) {
-        CorrectExtNavForSensorOffset(extNavDataDelayed.pos);
+        CorrectExtNavForSensorOffset(extNavDataDelayed);
     }
     extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
     if (extNavVelToFuse) {
-        CorrectExtNavVelForSensorOffset(extNavVelDelayed.vel);
+        CorrectExtNavVelForSensorOffset(extNavVelDelayed);
     }
 
     // Read GPS data from the sensor
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index b1552a7e3ab..e10e5f0130d 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -628,12 +628,14 @@ private:
         float           posErr;     // spherical poition measurement error 1-std (m)
         uint32_t        time_ms;    // measurement timestamp (msec)
         bool            posReset;   // true when the position measurement has been reset
+        bool            corrected;  // true when the position has been corrected for sensor position
     };
 
     struct ext_nav_vel_elements {
         Vector3f vel;               // velocity in NED (m/s)
         float err;                  // velocity measurement error (m/s)
         uint32_t time_ms;           // measurement timestamp (msec)
+        bool corrected;             // true when the velocity has been corrected for sensor position
     };
 
     // bias estimates for the IMUs that are enabled but not being used
@@ -945,10 +947,10 @@ private:
     void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
 
     // correct external navigation earth-frame position using sensor body-frame offset
-    void CorrectExtNavForSensorOffset(Vector3f &ext_position);
+    void CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data);
 
     // correct external navigation earth-frame velocity using sensor body-frame offset
-    void CorrectExtNavVelForSensorOffset(Vector3f &ext_velocity) const;
+    void CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav_vel_data) const;
 
     // Runs the IMU prediction step for an independent GSF yaw estimator algorithm
     // that uses IMU, GPS horizontal velocity and optionally true airspeed data.