From cbb25311bb9b6d401d6107d13dbf7ed90342447a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 16 Apr 2020 20:04:53 +0900 Subject: [PATCH] AP_NavEKF3: setAidingMode uses lastExtNavPassTime_ms --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 3539f58020..600e490799 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -255,7 +255,7 @@ void NavEKF3_core::setAidingMode() bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms); // Check if attitude drift has been constrained by a measurement source - bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed; + bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed || extNavUsed; // check if velocity drift has been constrained by a measurement source bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed; @@ -270,6 +270,7 @@ void NavEKF3_core::setAidingMode() (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) && + (imuSampleTime_ms - lastExtNavPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms); } @@ -284,9 +285,11 @@ void NavEKF3_core::setAidingMode() maxLossTime_ms = frontend->posRetryTimeUseVel_ms; } posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) && - (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); + (imuSampleTime_ms - lastExtNavPassTime_ms > maxLossTime_ms) && + (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); posAidLossPending = (imuSampleTime_ms - lastRngBcnPassTime_ms > (uint32_t)frontend->_gsfResetDelay) && - (imuSampleTime_ms - lastPosPassTime_ms > (uint32_t)frontend->_gsfResetDelay); + (imuSampleTime_ms - lastExtNavPassTime_ms > (uint32_t)frontend->_gsfResetDelay) && + (imuSampleTime_ms - lastPosPassTime_ms > (uint32_t)frontend->_gsfResetDelay); } if (attAidLossCritical) { @@ -389,6 +392,7 @@ void NavEKF3_core::setAidingMode() lastPosPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms; lastRngBcnPassTime_ms = imuSampleTime_ms; + lastExtNavPassTime_ms = imuSampleTime_ms; break; }