AP_GPS: added measured lag for Unicore GPS

measured as 0.14s from flight log
This commit is contained in:
Andrew Tridgell 2022-12-05 07:02:11 +11:00
parent 9cebe3b880
commit 3c1a54c3df
2 changed files with 22 additions and 1 deletions

View File

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

View File

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