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

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

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

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