From a04aff7a7dc8edbba92498c429731fa0121d8e68 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 29 Apr 2017 10:48:49 +1000
Subject: [PATCH] AP_NavEKF3: added inter-EKF scheduling cooperation

this changes the stragegy for load levelling between EKF cores so it
works between EK2 and EK3, and with future estimators as well.

It allows us to run EK3 and EK2 at the same time with good scheduling
performance
---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp            | 18 +++++++++++++-----
 libraries/AP_NavEKF3/AP_NavEKF3.h              |  3 +++
 .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp     |  3 ++-
 libraries/AP_NavEKF3/AP_NavEKF3_core.h         |  4 ++--
 4 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index 7b6c0e0ff2..cba2b06aff 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -607,9 +607,16 @@ bool NavEKF3::InitialiseFilter(void)
     if (_enable == 0) {
         return false;
     }
+    const AP_InertialSensor &ins = _ahrs->get_ins();
 
     imuSampleTime_us = AP_HAL::micros64();
 
+    // remember expected frame time
+    _frameTimeUsec = 1e6 / ins.get_sample_rate();
+
+    // expected number of IMU frames per prediction
+    _framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));
+    
     if (core == nullptr) {
 
         // see if we will be doing logging
@@ -619,7 +626,6 @@ bool NavEKF3::InitialiseFilter(void)
         }
 
         // don't run multiple filters for 1 IMU
-        const AP_InertialSensor &ins = _ahrs->get_ins();
         uint8_t mask = (1U<<ins.get_accel_count())-1;
         _imuMask.set(_imuMask.get() & mask);
         
@@ -705,10 +711,12 @@ void NavEKF3::UpdateFilter(void)
 
     bool statePredictEnabled[num_cores];
     for (uint8_t i=0; i<num_cores; i++) {
-        // if the previous core has only recently finished a new state prediction cycle, then
-        // don't start a new cycle to allow time for fusion operations to complete if the update
-        // rate is higher than 200Hz
-        if ((i > 0) && (core[i-1].getFramesSincePredict() < 2) && (ins.get_sample_rate() > 200)) {
+        // if we have not overrun by more than 3 IMU frames, and we
+        // have already used more than 1/3 of the CPU budget for this
+        // loop then suppress the prediction step. This allows
+        // multiple EKF instances to cooperate on scheduling
+        if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
+            (AP_HAL::micros() - ins.get_last_update_usec()) > _frameTimeUsec/3) {
             statePredictEnabled[i] = false;
         } else {
             statePredictEnabled[i] = true;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h
index b48adb1596..ab68243e66 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.h
@@ -339,6 +339,9 @@ private:
     AP_Baro &_baro;
     const RangeFinder &_rng;
 
+    uint32_t _frameTimeUsec;        // time per IMU frame
+    uint8_t  _framesPerPrediction;  // expected number of IMU frames per prediction
+    
     // EKF Mavlink Tuneable Parameters
     AP_Int8  _enable;               // zero to disable EKF3
     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index fc0ee49723..ae00893f94 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -352,7 +352,8 @@ void NavEKF3_core::readIMUData()
      * then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more
      * than twice the target time has lapsed.
      */
-    if ((imuDataDownSampledNew.delAngDT >= EKF_TARGET_DT && startPredictEnabled) || (imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) {
+    if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5)) && startPredictEnabled) ||
+        (imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) {
 
         // convert the accumulated quaternion to an equivalent delta angle
         imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 6201a96537..1c2f51e362 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -59,8 +59,8 @@
 #define ACCEL_BIAS_LIM_SCALER 0.2f
 
 // target update time for the EKF in msec and sec
-#define EKF_TARGET_DT_MS    10.0f
-#define EKF_TARGET_DT       0.01f
+#define EKF_TARGET_DT_MS 10.0
+#define EKF_TARGET_DT    0.01
 
 class AP_AHRS;