From 2340e18fdcf55107eb22694f5651de57e95afb32 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 6 Nov 2015 12:04:20 +1100
Subject: [PATCH] AP_NavEKF2: Offset the fusion time horizon between multiple
 instances

Prevents frame over-runs due to simultaneous fusion of measurements on each instance.
The offset is only applied if less than 5msec available between frames
---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp              | 3 ++-
 libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 5 +++--
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp         | 7 ++++++-
 libraries/AP_NavEKF2/AP_NavEKF2_core.h           | 7 ++++++-
 4 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index b1ad3af8a6..60cc07f49a 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -472,7 +472,8 @@ bool NavEKF2::InitialiseFilter(void)
         num_cores = 0;
         for (uint8_t i=0; i<7; i++) {
             if (_imuMask & (1U<<i)) {
-                core[num_cores++].setup_core(this, i);
+                core[num_cores].setup_core(this, i, num_cores);
+                num_cores++;
             }
         }
     }
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index 4ee78408a2..a1665a21ff 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -316,8 +316,9 @@ void NavEKF2_core::StoreIMU()
         fifoIndexNow = 0;
     }
     storedIMU[fifoIndexNow] = imuDataNew;
-    // set the index required to access the oldest data
-    fifoIndexDelayed = fifoIndexNow + 1;
+    // set the index required to access the oldest data, applying an offset to the fusion time horizon that is used to
+    // prevent the same fusion operation being performed on the same frame across multiple EKF's
+    fifoIndexDelayed = fifoIndexNow + 1 + fusionHorizonOffset;
     if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
         fifoIndexDelayed = 0;
     }
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index b4e367bfeb..d2c822a75a 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@@ -55,10 +55,11 @@ NavEKF2_core::NavEKF2_core(void) :
 }
 
 // setup this core backend
-void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index)
+void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index)
 {
     frontend = _frontend;
     imu_index = _imu_index;
+    core_index = _core_index;
     _ahrs = frontend->_ahrs;
 }
     
@@ -70,6 +71,10 @@ void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index)
 // Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
 void NavEKF2_core::InitialiseVariables()
 {
+    // Offset the fusion horizon if necessary to prevent frame over-runs
+    if (dtIMUavg < 0.005) {
+        fusionHorizonOffset = 2*core_index;
+    }
     // initialise time stamps
     imuSampleTime_ms = hal.scheduler->millis();
     lastHealthyMagTime_ms = imuSampleTime_ms;
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 56ffaddc38..ffcf144fce 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -47,6 +47,9 @@
  * Samples*delta_time must be > max sensor delay
 */
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+// Note that if using more than 2 instances of the EKF, as set by EK2_IMU_MASK, this delay should be increased by 2 samples
+// for each additional instance to allow for the need to offset the fusion time horizon for each instance to avoid simultaneous fusion
+// of measurements by each instance
 #define IMU_BUFFER_LENGTH       104 // maximum 260 msec delay at 400 Hz
 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
 #define IMU_BUFFER_LENGTH       13 // maximum 260 msec delay at 50 Hz
@@ -65,7 +68,7 @@ public:
     NavEKF2_core(void);
 
     // setup this core backend
-    void setup_core(NavEKF2 *_frontend, uint8_t _imu_index);
+    void setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index);
     
     // Initialise the states from accelerometer and magnetometer data (if present)
     // This method can only be used when the vehicle is static
@@ -266,6 +269,7 @@ private:
     // Reference to the global EKF frontend for parameters
     NavEKF2 *frontend;
     uint8_t imu_index;
+    uint8_t core_index;
 
     typedef float ftype;
 #if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1)
@@ -777,6 +781,7 @@ private:
     uint32_t lastVelReset_ms;       // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
     float yawTestRatio;             // square of magnetometer yaw angle innovation divided by fail threshold
     Quaternion prevQuatMagReset;    // Quaternion from the last time the magnetic field state reset condition test was performed
+    uint8_t fusionHorizonOffset;    // number of IMU samples that the fusion time horizon  has been shifted forward to prevent multiple EKF instances fusing data at the same time
 
     // variables used to calulate a vertical velocity that is kinematically consistent with the verical position
     float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.