From ee9cc28fdac6672ce19ae89e57e56ec7496c3266 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Thu, 28 Jun 2018 15:37:38 -0700 Subject: [PATCH] AP_NavEKF2: Utilize the GPS drivers estimate for lag --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 10 +--------- libraries/AP_NavEKF2/AP_NavEKF2.h | 1 - libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 5 ++++- 3 files changed, 5 insertions(+), 11 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 138998d0c8..414e64cae0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -188,15 +188,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Units: m AP_GROUPINFO("GLITCH_RAD", 7, NavEKF2, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT), - // @Param: GPS_DELAY - // @DisplayName: GPS measurement delay (msec) - // @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements. - // @Range: 0 250 - // @Increment: 10 - // @User: Advanced - // @Units: ms - // @RebootRequired: True - AP_GROUPINFO("GPS_DELAY", 8, NavEKF2, _gpsDelay_ms, 220), + // 8 was used for GPS_DELAY // Height measurement parameters diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 9f6b2d1b5e..09f80ff8cf 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -362,7 +362,6 @@ private: AP_Float _accNoise; // accelerometer process noise : m/s^2 AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2 - AP_Int16 _gpsDelay_ms; // effective average delay of GPS measurements relative to inertial measurement (msec) AP_Int16 _hgtDelay_ms; // effective average delay of Height measurements relative to inertial measurements (msec) AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity AP_Int16 _gpsVelInnovGate; // Percentage number of standard deviations applied to GPS velocity innovation consistency check diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 1c9a3a0c4e..8437623068 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -423,9 +423,12 @@ void NavEKF2_core::readGpsData() // get current fix time lastTimeGpsReceived_ms = gps.last_message_time_ms(); + // 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 - gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms; + float gps_delay = 0.0; + gps.get_lag(gps_delay); // ignore the return value + gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(1e3f * gps_delay); // Correct for the average intersampling delay due to the filter updaterate gpsDataNew.time_ms -= localFilterTimeStep_ms/2;