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

@@ -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
+        // if we have not overrun by more than 3 IMU frames, and we
-        // don't start a new cycle to allow time for fusion operations to complete if the update
+        // have already used more than 1/3 of the CPU budget for this
-        // rate is higher than 200Hz
+        // loop then suppress the prediction step. This allows
-        if ((i > 0) && (core[i-1].getFramesSincePredict() < 2) && (ins.get_sample_rate() > 200)) {
+        // 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;

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

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);

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_MS 10.0
-#define EKF_TARGET_DT       0.01f
+#define EKF_TARGET_DT    0.01
 
 class AP_AHRS;