Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: setAidingMode uses lastExtNavPassTime_ms

Browse Source
This commit is contained in:
Randy Mackay 2020-04-16 20:04:53 +09:00
parent d763597065
commit cbb25311bb
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -255,7 +255,7 @@ void NavEKF3_core::setAidingMode()
bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms); bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms);
// Check if attitude drift has been constrained by a measurement source // 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 // check if velocity drift has been constrained by a measurement source
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed; bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed;
@ -270,6 +270,7 @@ void NavEKF3_core::setAidingMode()
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastPosPassTime_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); (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
} }
@ -284,9 +285,11 @@ void NavEKF3_core::setAidingMode()
maxLossTime_ms = frontend->posRetryTimeUseVel_ms; maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
} }
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_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) && 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) { if (attAidLossCritical) {
@ -389,6 +392,7 @@ void NavEKF3_core::setAidingMode()
lastPosPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
lastRngBcnPassTime_ms = imuSampleTime_ms; lastRngBcnPassTime_ms = imuSampleTime_ms;
lastExtNavPassTime_ms = imuSampleTime_ms;
break; break;
} }