diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index b8f1412d61..f7819dfbc8 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -926,14 +926,14 @@ void NavEKF::SelectFlowFusion() // update the time stamp prevFlowFusionTime_ms = imuSampleTime_ms; - } else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode){ + } else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !constPosMode && tiltOK){ // Fuse the optical flow Y axis data into the main filter FuseOptFlow(); // increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle flow_state.obsIndex = 2; // indicate that flow fusion has been performed. This is used for load spreading. flowFusePerformed = true; - } else if (((flowDataValid && flow_state.obsIndex == 2) || newDataRng) && !constPosMode) { + } else if (((flowDataValid && flow_state.obsIndex == 2) || newDataRng) && !constPosMode && tiltOK) { // enable fusion of range data if available and permitted if(newDataRng && useRngFinder()) { fuseRngData = true;