From 8526a8ba7eaae3a701e1c32cf96e2d570a2225fa Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Mon, 19 Oct 2015 09:34:11 +1100
Subject: [PATCH] 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 | 12 ++++++++++++
 libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp     |  4 ++++
 libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp |  6 ++++++
 libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp  |  6 ++++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp          |  6 +++---
 5 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
index 43b3f03906..270c29f541 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
+++ b/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
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
index 4e76b0ed0a..cf23c12110 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
+++ b/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();
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
index 107014331f..55ef4724b5 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
+++ b/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
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index f575049608..fe52f0a04a 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/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();
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index 36f69689dd..b69c1b87bc 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/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();