AP_NavEKF3: allow switch to ExtNav from optflow

This commit is contained in:
Randy Mackay 2020-08-22 10:40:30 +09:00
parent 10eeea31e8
commit 886a7d44a7
1 changed files with 1 additions and 1 deletions

View File

@ -225,7 +225,7 @@ void NavEKF3_core::setAidingMode()
bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS or range beacon data is available
// If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
if(readyToUseGPS() || readyToUseRangeBeacon()) {
if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (flowFusionTimeout && bodyOdmFusionTimeout) {
PV_AidingMode = AID_NONE;