AP_NavEKF3: ext nav vel corrected for sensor position when recalled from buffer

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -47,10 +47,8 @@ void NavEKF3_core::ResetVelocity(void)
             lastVelPassTime_ms = imuSampleTime_ms;
         } else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && posResetSource == DEFAULT) || velResetSource == EXTNAV) {
             // use external nav data as the 2nd preference
-            // correct for sensor position
+            // already corrected for sensor position
-            ext_nav_vel_elements extNavVelCorrected = extNavVelDelayed;
+            stateStruct.velocity = extNavVelDelayed.vel;
-            CorrectExtNavVelForSensorOffset(extNavVelCorrected.vel);
-            stateStruct.velocity = extNavVelCorrected.vel;
             P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err);
             velTimeout = false;
             lastVelPassTime_ms = imuSampleTime_ms;
@@ -405,6 +403,9 @@ void NavEKF3_core::SelectVelPosFusion()
     // Check for data at the fusion time horizon
     extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
     extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
+    if (extNavVelToFuse) {
+        CorrectExtNavVelForSensorOffset(extNavVelDelayed.vel);
+    }
 
     // Read GPS data from the sensor
     readGpsData();
@@ -454,15 +455,12 @@ void NavEKF3_core::SelectVelPosFusion()
     }
 
     // fuse external navigation velocity data if available
+    // extNavVelDelayed is already corrected for sensor position
     if (extNavVelToFuse && (frontend->_fusionModeGPS == 3)) {
         fuseVelData = true;
-
+        velPosObs[0] = extNavVelDelayed.vel.x;
-        // correct for external navigation sensor position
+        velPosObs[1] = extNavVelDelayed.vel.y;
-        Vector3f vel_corrected = extNavVelDelayed.vel;
+        velPosObs[2] = extNavVelDelayed.vel.z;
-        CorrectExtNavVelForSensorOffset(vel_corrected);
-        velPosObs[0] = vel_corrected.x;
-        velPosObs[1] = vel_corrected.y;
-        velPosObs[2] = vel_corrected.z;
     }
 
     // we have GPS data to fuse and a request to align the yaw using the GPS course

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1357,7 +1357,7 @@ private:
     bool extNavUsedForPos;              // true when the external nav data is being used as a position reference.
     obs_ring_buffer_t<ext_nav_vel_elements> storedExtNavVel;    // external navigation velocity data buffer
     ext_nav_vel_elements extNavVelNew;  // external navigation velocity data at the current time horizon
-    ext_nav_vel_elements extNavVelDelayed;  // external navigation velocity data at the fusion time horizon
+    ext_nav_vel_elements extNavVelDelayed;  // external navigation velocity data at the fusion time horizon.  Already corrected for sensor position
     uint32_t extNavVelMeasTime_ms;      // time external navigation velocity measurements were accepted for input to the data buffer (msec)
     bool extNavVelToFuse;               // true when there is new external navigation velocity to fuse
     bool useExtNavVel;                  // true if external nav velocity should be used