From a8fd1d8bcd6f10d945ed90fe57fe925bd10ce38c Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 19 Dec 2016 11:29:41 +1100 Subject: [PATCH] AP_NavEKF3: fix potential time-stamping bug Use a consistent time reference --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 87bcdf17a4..d4dc31b1e6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -283,7 +283,7 @@ void NavEKF3_core::readIMUData() dtIMUavg = ins.get_loop_delta_t(); // the imu sample time is used as a common time reference throughout the filter - imuSampleTime_ms = AP_HAL::millis(); + imuSampleTime_ms = frontend->imuSampleTime_us / 1000; // use the nominated imu or primary if not available if (ins.use_accel(imu_index)) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 183e4e38d2..0ee24eec08 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -498,9 +498,6 @@ void NavEKF3_core::UpdateFilter(bool predict) // TODO - in-flight restart method - //get starting time for update step - imuSampleTime_ms = frontend->imuSampleTime_us / 1000; - // Check arm status and perform required checks and mode changes controlFilterModes();