diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index df39da9edd..5c7e318302 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -178,7 +178,11 @@ void NavEKF2_core::setAidingMode() bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000); // Check if the fusion has timed out (flow measurements have been rejected for too long) bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); - if (flowSensorTimeout || flowFusionTimeout) { + // Enable switch to absolute position mode if GPS is available + // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding + if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) { + PV_AidingMode = AID_ABSOLUTE; + } else if (flowSensorTimeout || flowFusionTimeout) { PV_AidingMode = AID_NONE; } } else if (PV_AidingMode == AID_ABSOLUTE) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 8a2795a4fe..2126faa0ba 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -59,8 +59,8 @@ void NavEKF2_core::SelectFlowFusion() EstimateTerrainOffset(); } - // Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode - if (flowDataToFuse && tiltOK && PV_AidingMode == AID_RELATIVE) + // Fuse optical flow data into the main filter + if (flowDataToFuse && tiltOK) { // Set the flow noise used by the fusion processes R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));