From aa94ff629d84986ac45937689b516be5bfddef4d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 31 Jan 2015 10:56:06 +1100 Subject: [PATCH] AP_NavEKF: Prevent bad GPS pre-arming casuing initial position errors If the vehicle moves significantly or the GPS changes position significantly pre-armed, then the GPS glitch logic was being invoked when the first GPs measurements were fused. This patch resets the position to the GPS when the vehicle arms. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 8b9d780222..6c5edc4be9 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4621,6 +4621,8 @@ void NavEKF::performArmingChecks() PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states constPosMode = false; constVelMode = false; + // we reset the position in case the initial GPS setting the origin was off or the vehicle has been moved a significant distance + ResetPosition(); } } // Reset filter positon, height and velocity states on arming or disarming