From 886a7d44a790338eeeabcd9c3e9dde947e35e9db Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 22 Aug 2020 10:40:30 +0900 Subject: [PATCH] AP_NavEKF3: allow switch to ExtNav from optflow --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index e33cb54e64..99a04592c1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -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;