From 013b39d2a6b5dd8707938f7d975ddbcb1b68074c Mon Sep 17 00:00:00 2001
From: chobits <chobits@itri.org.tw>
Date: Wed, 12 Aug 2020 15:01:27 +0800
Subject: [PATCH] AP_NavEKF3: fix ext nav vel timestamp cal

---
 libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index d9daf0bc03..e5d13f0624 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -1036,8 +1036,10 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
         return;
     }
 
-    extNavVelMeasTime_ms = timeStamp_ms - delay_ms;
+    extNavVelMeasTime_ms = timeStamp_ms;
     useExtNavVel = true;
+    // calculate timestamp
+    timeStamp_ms = timeStamp_ms - delay_ms;
     // Correct for the average intersampling delay due to the filter updaterate
     timeStamp_ms -= localFilterTimeStep_ms/2;
     // Prevent time delay exceeding age of oldest IMU data in the buffer