From 71eb46dd04eb720bf91867e8369d7700d8794eaa Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 21 Dec 2016 16:35:37 +1100 Subject: [PATCH] AP_NavEKF3: Fix bug in loss of aiding fallback Fixes a bug introduced by the introduction of range beacon fusion to the fallback test. --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index e873bd1e44..84727c6727 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -213,17 +213,17 @@ void NavEKF3_core::setAidingMode() uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms)); // Check if optical flow data is being used - bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms > minTestTime_ms); + bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms); // Check if airspeed data is being used - bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms > minTestTime_ms); + bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms); // Check if range beacon data is being used - bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms > minTestTime_ms); + bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms); // Check if GPS is being used - bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms > minTestTime_ms); - bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms > minTestTime_ms); + bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); + bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); // Check if attitude drift has been constrained by a measurement source bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;