mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: fix skipping of optflow fusion if mag fusion performed
This commit is contained in:
parent
8b92af732a
commit
41acf555bf
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue