diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 06ea248129..e68a843602 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -861,3 +861,15 @@ void AP_GPS::inject_data_all(const uint8_t *data, uint16_t len) } } + +/* + return expected lag from a GPS + */ +float AP_GPS::get_lag(uint8_t instance) const +{ + if (drivers[instance] == nullptr || state[instance].status == NO_GPS) { + // return default; + return 0.2f; + } + return drivers[instance]->get_lag(); +} diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index a1c0531f67..2ebc6bc023 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -314,7 +314,8 @@ public: } // the expected lag (in seconds) in the position and velocity readings from the gps - float get_lag() const { return 0.2f; } + float get_lag(uint8_t instance) const; + float get_lag(void) const { return get_lag(primary_instance); } // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin const Vector3f &get_antenna_offset(uint8_t instance) const { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 690d30758a..02429d3783 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1284,3 +1284,21 @@ AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const { } } } + +/* + return velocity lag in seconds + */ +float AP_GPS_UBLOX::get_lag(void) const +{ + switch (_hardware_generation) { + case UBLOX_5: + case UBLOX_6: + default: + return 0.22f; + case UBLOX_7: + case UBLOX_M8: + // based on flight logs the 7 and 8 series seem to produce about 120ms lag + return 0.12f; + break; + }; +} diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index d318f81bf2..cfbe13ddf4 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -111,6 +111,10 @@ public: } void broadcast_configuration_failure_reason(void) const override; + + // return velocity lag + float get_lag(void) const override; + private: // u-blox UBX protocol essentials struct PACKED ubx_header { diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 9c9f4a07ca..566792268b 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -52,6 +52,9 @@ public: virtual void handle_msg(const mavlink_message_t *msg) { return ; } + // driver specific lag + virtual float get_lag(void) const { return 0.2f; } + protected: AP_HAL::UARTDriver *port; ///< UART we are attached to AP_GPS &gps; ///< access to frontend (for parameters)