diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 0d43744cbd..6e73189101 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -20,9 +20,6 @@ void NavEKF3_core::SelectFlowFusion() // start performance timer hal.util->perf_begin(_perf_FuseOptFlow); - // Check for data at the fusion time horizon - flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); - // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz // If so, don't fuse measurements on this time step to reduce frame over-runs // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements @@ -33,6 +30,9 @@ void NavEKF3_core::SelectFlowFusion() optFlowFusionDelayed = false; } + // Check for data at the fusion time horizon + flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); + // Perform Data Checks // Check if the optical flow data is still valid flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);