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; + } + } } }