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

AP_NavEKF2: setAidingMode uses lastExtNavPassTime_ms

Browse Source
This commit is contained in:
Randy Mackay 2020-04-18 10:52:51 +09:00
parent 3b20623a31
commit 014c889ceb
2 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -209,14 +209,17 @@ void NavEKF2_core::setAidingMode()
bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
// Check if external nav is being used
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 = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed; bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || 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; bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
// check if position drift has been constrained by a measurement source // check if position drift has been constrained by a measurement source
bool posAiding = posUsed || rngBcnUsed; bool posAiding = posUsed || rngBcnUsed || extNavUsed;
// Check if the loss of attitude aiding has become critical // Check if the loss of attitude aiding has become critical
bool attAidLossCritical = false; bool attAidLossCritical = false;
@ -225,6 +228,7 @@ void NavEKF2_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);
} }
@ -239,9 +243,11 @@ void NavEKF2_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) {
@ -335,6 +341,7 @@ void NavEKF2_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;
} }

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -1160,6 +1160,7 @@ private:
ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon
uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec) uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec)
uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec) uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec)
uint32_t lastExtNavPassTime_ms; // time stamp when external nav position measurement last passed innovation consistency check (msec)
bool extNavDataToFuse; // true when there is new external nav data to fuse bool extNavDataToFuse; // true when there is new external nav data to fuse
bool extNavUsedForYaw; // true when the external nav data is also being used as a yaw observation bool extNavUsedForYaw; // true when the external nav data is also being used as a yaw observation
bool extNavUsedForPos; // true when the external nav data is being used as a position reference. bool extNavUsedForPos; // true when the external nav data is being used as a position reference.