From 5c8e71a8d1d99829de89a36fd3235400481d5282 Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 9 Jan 2015 23:47:22 +1100 Subject: [PATCH] AP_NavEKF: Don't reset the position measurement timeout if not aiding When PV aiding is disabled, then the timeout time reference should not be reset becasue we want the position measurement timeout status to remain true the whole time the measurement is not being used. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 22fbeb6dee..76d899a2eb 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1936,8 +1936,8 @@ void NavEKF::FuseVelPosNED() // use position data if healthy, timed out, or in constant position mode if (posHealth || posTimeout || constPosMode) { posHealth = true; - // We don't reset the failed time if we are in constant position mode - if (!constPosMode) { + // only reset the failed time and do glitch timeout checks if we are doing full aiding + if (PV_AidingMode == AID_ABSOLUTE) { 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