Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Utilize the GPS drivers estimate for lag

Browse Source
This commit is contained in:
Michael du Breuil 2018-06-28 15:37:38 -07:00 committed by Andrew Tridgell
parent 1a406dacfc
commit ee9cc28fda
3 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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

1
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -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

5
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -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;