diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 793c71f5e2..5f0f7080ca 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -205,7 +205,7 @@ bool NavEKF2_core::optFlowDataPresent(void) const // return true if the filter to be ready to use gps bool NavEKF2_core::readyToUseGPS(void) const { - return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign; + return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3); } // return true if we should use the compass