diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 2f6ba935c7..5018e4d29c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/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 - ext_nav_vel_elements extNavVelCorrected = extNavVelDelayed; - CorrectExtNavVelForSensorOffset(extNavVelCorrected.vel); - stateStruct.velocity = extNavVelCorrected.vel; + // already corrected for sensor position + stateStruct.velocity = extNavVelDelayed.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; - - // correct for external navigation sensor position - Vector3f vel_corrected = extNavVelDelayed.vel; - CorrectExtNavVelForSensorOffset(vel_corrected); - velPosObs[0] = vel_corrected.x; - velPosObs[1] = vel_corrected.y; - velPosObs[2] = vel_corrected.z; + velPosObs[0] = extNavVelDelayed.vel.x; + velPosObs[1] = extNavVelDelayed.vel.y; + velPosObs[2] = extNavVelDelayed.vel.z; } // we have GPS data to fuse and a request to align the yaw using the GPS course diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index edaef69ff2..156af418d1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/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 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