AP_NavEKF3: fix skipping of optflow fusion if mag fusion performed

This commit is contained in:
Randy Mackay 2020-08-18 10:51:50 +09:00
parent 8b92af732a
commit 41acf555bf
1 changed files with 3 additions and 3 deletions

View File

@ -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);