From 87c7781be2d9a91b94937ee43b9fb71abaa6c062 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Aug 2020 15:05:14 +1000 Subject: [PATCH] AP_GPS: show GPS as unhealthy if it is lagged too much this detects GPS data lag, and if 5 samples in a row are lagged by more than 50ms beyond the expected lag for the GPS then we declare the GPS as unhealthy. This is useful to detect users who have asked for more data from the GPS then it can send at the baudrate that is being used. The case that led to this path was a F9 GPS with GPS_RAW_DATA=1 at 115200 baud. In that case the UART data is quickly lagged by over 1s --- libraries/AP_GPS/AP_GPS.cpp | 4 +++- libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/GPS_Backend.cpp | 14 ++++++++++++++ 3 files changed, 18 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index da0e75ea37..2bb094cbef 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1757,7 +1757,9 @@ bool AP_GPS::is_healthy(uint8_t instance) const const uint8_t delay_threshold = 2; const float delay_avg_max = _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER?245:215; const GPS_timing &t = timing[instance]; - bool delay_ok = (t.delayed_count < delay_threshold) && t.average_delta_ms < delay_avg_max; + bool delay_ok = (t.delayed_count < delay_threshold) && + t.average_delta_ms < delay_avg_max && + state[instance].lagged_sample_count < 5; #if defined(GPS_BLENDED_INSTANCE) if (instance == GPS_BLENDED_INSTANCE) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 1368510e73..0a171a0cdc 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -173,6 +173,7 @@ public: bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds uint32_t uart_timestamp_ms; ///< optional timestamp from set_uart_timestamp() + uint32_t lagged_sample_count; ///< number of samples with 50ms more lag than expected // all the following fields must only all be filled by RTK capable backend drivers uint32_t rtk_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 1a64e3730f..a60f83e710 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -287,5 +287,19 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) // use msg arrival time, and correct for jitter uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us); state.uart_timestamp_ms = local_us / 1000U; + + // look for lagged data from the GPS. This is meant to detect + // the case that the GPS is trying to push more data into the + // UART than can fit (eg. with GPS_RAW_DATA at 115200). + float expected_lag; + if (gps.get_lag(state.instance, expected_lag)) { + float lag_s = (now - state.uart_timestamp_ms) * 0.001; + if (lag_s > expected_lag+0.05) { + // more than 50ms over expected lag, increment lag counter + state.lagged_sample_count++; + } else { + state.lagged_sample_count = 0; + } + } } }