diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 33769f27bb5bc..152f250e18b65 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -153,6 +153,64 @@ void NavEKF3_core::ResetPosition(void) } +// reset the stateStruct's NE position to the specified position +// posResetNE is updated to hold the change in position +// storedOutput, outputDataNew and outputDataDelayed are updated with the change in position +// lastPosReset_ms is updated with the time of the reset +void NavEKF3_core::ResetPositionNE(float posN, float posE) +{ + // Store the position before the reset so that we can record the reset delta + const Vector3f posOrig = stateStruct.position; + + // Set the position states to the new position + stateStruct.position.x = posN; + stateStruct.position.y = posE; + + // Calculate the position offset due to the reset + posResetNE.x = stateStruct.position.x - posOrig.x; + posResetNE.y = stateStruct.position.y - posOrig.y; + + // Add the offset to the output observer states + for (uint8_t i=0; i_fusionModeGPS == 3) && extNavDataDelayed.posReset) { + ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y); + if (activeHgtSource == HGT_SOURCE_EXTNAV) { + ResetPositionD(-hgtMea); + } + } // If we are operating without any aiding, fuse in the last known position // to constrain tilt drift. This assumes a non-manoeuvring vehicle diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index de79e4007635f..a873bdc5e3e6e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -771,6 +771,12 @@ class NavEKF3_core : public NavEKF_core_common // reset the horizontal position states uing the last GPS measurement void ResetPosition(void); + // reset the stateStruct's NE position to the specified position + void ResetPositionNE(float posN, float posE); + + // reset the stateStruct's D position + void ResetPositionD(float posD); + // reset velocity states using the last GPS measurement void ResetVelocity(void);