diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index e5ebe04327..cc28e88b81 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -827,4 +827,22 @@ bool AP_GPS_NMEA::is_healthy(void) const return true; } +// get the velocity lag +bool AP_GPS_NMEA::get_lag(float &lag_sec) const +{ + switch (get_type()) { +#if AP_GPS_NMEA_UNICORE_ENABLED + case AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA: + case AP_GPS::GPS_TYPE_UNICORE_NMEA: + lag_sec = 0.14; + break; +#endif // AP_GPS_NMEA_UNICORE_ENABLED + + default: + lag_sec = 0.2; + break; + } + return true; +} + #endif // AP_GPS_NMEA_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 81198edbd3..f7f9bfbd0d 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -68,7 +68,10 @@ public: // driver specific health, returns true if the driver is healthy bool is_healthy(void) const override; - + + // get lag in seconds + bool get_lag(float &lag_sec) const override; + private: /// Coding for the GPS sentences that the parser handles enum _sentence_types : uint16_t { //there are some more than 10 fields in some sentences , thus we have to increase these value.