From 1789dc08a36f68e57369f8a1eea419a5dc58df9c Mon Sep 17 00:00:00 2001 From: priseborough <p_riseborough@live.com.au> Date: Tue, 6 Jan 2015 15:19:33 +1100 Subject: [PATCH] AP_NavEKF: Correctly report position timeout when GPS is lost --- libraries/AP_NavEKF/AP_NavEKF.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 6a6c9691c..582506d87 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1928,7 +1928,10 @@ void NavEKF::FuseVelPosNED() // use position data if healthy, timed out, or in constant position mode if (posHealth || posTimeout || constPosMode) { posHealth = true; - posFailTime = imuSampleTime_ms; + // We don't reset the failed time if we are in constant position mode + if (!constPosMode) { + posFailTime = imuSampleTime_ms; + } // if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) { gpsPosGlitchOffsetNE.x += posInnov[0]; -- GitLab