From 7d6328608867b7f8a5db5d785085228653739b73 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 22 May 2017 11:08:52 +1000 Subject: [PATCH] AP_NavEKF3: update to match AP_GPS interface change --- 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 80789fdaf0..17328b5fa1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -441,7 +441,9 @@ void NavEKF3_core::readGpsData() // estimate when the GPS fix was valid, allowing for GPS processing and other delays // ideally we should be using a timing signal from the GPS receiver to set this time // Use the driver specified delay - gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(_ahrs->get_gps().get_lag() * 1000.0f); + float gps_delay_sec = 0; + _ahrs->get_gps().get_lag(gps_delay_sec); + gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f); // Correct for the average intersampling delay due to the filter updaterate gpsDataNew.time_ms -= localFilterTimeStep_ms/2;