AP_NavEKF2: Level processor loading between frames

Don't fuse other measurements on the same frame that magnetometer measurements arrive if running at a high frame rate as there will be insufficient time to complete other operations.

libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp

@@ -198,6 +198,12 @@ void NavEKF2_core::FuseAirspeed()
 // select fusion of true airspeed measurements
 void NavEKF2_core::SelectTasFusion()
 {
+    // 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
+    if (magFusePerformed && dtIMUavg < 0.005f) {
+        return;
+    }
+
     // get true airspeed measurement
     readAirSpdData();
 
@@ -262,6 +268,12 @@ bool NavEKF2_core::RecallTAS()
 // it requires a stable wind for best results and should not be used for aerobatic flight with manoeuvres that induce large sidslip angles (eg knife-edge, spins, etc)
 void NavEKF2_core::SelectBetaFusion()
 {
+    // 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
+    if (magFusePerformed && dtIMUavg < 0.005f) {
+        return;
+    }
+
     // set true when the fusion time interval has triggered
     bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend.betaAvg_ms);
     // set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -114,6 +114,10 @@ void NavEKF2_core::SelectMagFusion()
     // start performance timer
     perf_begin(_perf_FuseMagnetometer);
 
+    // clear the flag that lets other processes know that the expensive magnetometer fusion operation has been perfomred on that time step
+    // used for load levelling
+    magFusePerformed = false;
+
     // check for and read new magnetometer measurements
     readMagData();
 

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -29,6 +29,12 @@ extern const AP_HAL::HAL& hal;
 // select fusion of optical flow measurements
 void NavEKF2_core::SelectFlowFusion()
 {
+    // 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
+    if (magFusePerformed && dtIMUavg < 0.005f) {
+        return;
+    }
+
     // start performance timer
     perf_begin(_perf_FuseOptFlow);
     // Perform Data Checks

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -110,6 +110,12 @@ bool NavEKF2_core::resetHeightDatum(void)
 // select fusion of velocity, position and height measurements
 void NavEKF2_core::SelectVelPosFusion()
 {
+    // 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
+    if (magFusePerformed && dtIMUavg < 0.005f) {
+        return;
+    }
+
     // check for and read new GPS data
     readGpsData();
 

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -369,15 +369,15 @@ void NavEKF2_core::UpdateFilter()
     // Read range finder data which is used by both position and optical flow fusion
     readRangeFinder();
 
+    // Update states using  magnetometer data
+    SelectMagFusion();
+
     // Update states using GPS and altimeter data
     SelectVelPosFusion();
 
     // Update states using optical flow data
     SelectFlowFusion();
 
-    // Update states using  magnetometer data
-    SelectMagFusion();
-
     // Update states using airspeed data
     SelectTasFusion();