Browse Source

AP_NavEKF3: fix skipping of optflow fusion if mag fusion performed

c415-sdk
Randy Mackay 5 years ago
parent
commit
41acf555bf
  1. 6
      libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

6
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@ -20,9 +20,6 @@ void NavEKF3_core::SelectFlowFusion() @@ -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() @@ -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…
Cancel
Save