From 2de8777669ab79648853d7e116abe755df82d310 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_NavEKF2: 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_NavEKF2/AP_NavEKF2.cpp            | 18 +++++++++++++-----
 libraries/AP_NavEKF2/AP_NavEKF2.h              |  3 +++
 .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp     |  3 ++-
 libraries/AP_NavEKF2/AP_NavEKF2_core.h         |  5 +++--
 4 files changed, 21 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 420eca80a0..8d247b16d3 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -609,9 +609,16 @@ bool NavEKF2::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));
+
     // see if we will be doing logging
     DataFlash_Class *dataflash = DataFlash_Class::instance();
     if (dataflash != nullptr) {
@@ -621,7 +628,6 @@ bool NavEKF2::InitialiseFilter(void)
     if (core == nullptr) {
 
         // 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);
         
@@ -690,10 +696,12 @@ void NavEKF2::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_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index 70e7696df3..fbebad7a3f 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -322,6 +322,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 EKF2
     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index be49653931..5cf70b47d6 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -327,7 +327,8 @@ void NavEKF2_core::readIMUData()
 
     // If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data
     // to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed
-    if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {
+    if ((dtIMUavg*(float)framesSincePredict >= (EKF_TARGET_DT-(dtIMUavg*0.5)) &&
+         startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 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_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 51b2ec025b..b22201676a 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -47,6 +47,9 @@
 #define HGT_SOURCE_GPS  2
 #define HGT_SOURCE_BCN  3
 
+// target EKF update time step
+#define EKF_TARGET_DT 0.01f
+
 class AP_AHRS;
 
 class NavEKF2_core
@@ -728,8 +731,6 @@ private:
     bool badMagYaw;                 // boolean true if the magnetometer is declared to be producing bad data
     bool badIMUdata;                // boolean true if the bad IMU data is detected
 
-    const float EKF_TARGET_DT = 0.01f;    // target EKF update time step
-
     float gpsNoiseScaler;           // Used to scale the  GPS measurement noise and consistency gates to compensate for operation with small satellite counts
     Vector28 Kfusion;               // Kalman gain vector
     Matrix24 KH;                    // intermediate result used for covariance updates