From c505a458de4dda85315a399a2588205429ec403d Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 9 Jan 2015 23:41:15 +1100 Subject: [PATCH] AP_NavEKF: Always declare a position measurement timeout if aiding not used If position and velocity aiding is turned off, then the position measurement should always be reported as timed out. --- 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 7e015aa447..22fbeb6dee 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1931,8 +1931,8 @@ void NavEKF::FuseVelPosNED() float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime))); posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); - // declare a timeout condition if we have been too long without data - posTimeout = ((imuSampleTime_ms - posFailTime) > gpsRetryTime); + // declare a timeout condition if we have been too long without data or not aiding + posTimeout = (((imuSampleTime_ms - posFailTime) > gpsRetryTime) || PV_AidingMode == AID_NONE); // use position data if healthy, timed out, or in constant position mode if (posHealth || posTimeout || constPosMode) { posHealth = true;