diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 477eea0b99..a5e90dc489 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -190,17 +190,17 @@ void NavEKF2_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;