From 4640673cb14b61be52fa84d777e016e4644d92c4 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Wed, 21 Oct 2015 10:10:11 +1100
Subject: [PATCH] AP_NavEKF2: Fix bug causing incorrect fusion timing

---
 libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index 3fe286302f..11fe4cca49 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -386,12 +386,17 @@ void NavEKF2_core::readIMUData()
 // store imu in the FIFO
 void NavEKF2_core::StoreIMU()
 {
-    fifoIndexDelayed = fifoIndexNow;
+    // increment the index and write new data
     fifoIndexNow = fifoIndexNow + 1;
     if (fifoIndexNow >= IMU_BUFFER_LENGTH) {
         fifoIndexNow = 0;
     }
     storedIMU[fifoIndexNow] = imuDataNew;
+    // set the index required to access the oldest data
+    fifoIndexDelayed = fifoIndexNow + 1;
+    if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
+        fifoIndexDelayed = 0;
+    }
 }
 
 // reset the stored imu history and store the current value