From db7c8439c6262c38c401c6048ac6fbd593e66f57 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Fri, 28 Jul 2017 11:29:49 +1000
Subject: [PATCH] AP_NavEKF3: Fix bug in wheel odometry timestamp correction

---
 libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 6e89882296..780dddd7f9 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -144,12 +144,11 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
     wheelOdmDataNew.delAng = delAng;
     wheelOdmDataNew.radius = radius;
     wheelOdmDataNew.delTime = delTime;
-    wheelOdmDataNew.time_ms = timeStamp_ms - (uint32_t)(500.0f * delTime);
     wheelOdmMeasTime_ms = timeStamp_ms;
 
     // becasue we are currently converting to an equivalent velocity measurement before fusing
     // the measurement time is moved back to the middle of the sampling period
-    wheelOdmDataNew.time_ms -= (uint32_t)(500.0f * delTime);
+    wheelOdmDataNew.time_ms = timeStamp_ms - (uint32_t)(500.0f * delTime);
 
     storedWheelOdm.push(wheelOdmDataNew);