AP_GPS: improved delay health threshold

allow for missing frames without showing unhealthy on GCS

libraries/AP_GPS/AP_GPS.cpp

@@ -767,6 +767,25 @@ void AP_GPS::update_instance(uint8_t instance)
     }
 #endif
 
+    if (data_should_be_logged) {
+        // keep count of delayed frames and average frame delay for health reporting
+        const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer
+        GPS_timing &t = timing[instance];
+
+        if (t.delta_time_ms > gps_max_delta_ms) {
+            t.delayed_count++;
+        } else {
+            t.delayed_count = 0;
+        }
+        if (t.delta_time_ms < 2000) {
+            if (t.average_delta_ms <= 0) {
+                t.average_delta_ms = t.delta_time_ms;
+            } else {
+                t.average_delta_ms = 0.98 * t.average_delta_ms + 0.02 * t.delta_time_ms;
+            }
+        }
+    }
+    
 #ifndef HAL_BUILD_AP_PERIPH
     if (data_should_be_logged &&
         (should_log() || AP::ahrs().have_ekf_logging())) {
@@ -1686,18 +1705,22 @@ bool AP_GPS::is_healthy(uint8_t instance) const
         return false;
     }
 
-    const uint16_t gps_max_delta_ms = 245; // 200 ms (5Hz) + 45 ms buffer
-
-    bool last_msg_valid = last_message_delta_time_ms(instance) < gps_max_delta_ms;
+    /*
+      allow two lost frames before declaring the GPS unhealthy, but
+      require the average frame rate to be close to 5Hz
+     */
+    const uint8_t delay_threshold = 2;
+    const float delay_avg_max = 245;
+    const GPS_timing &t = timing[instance];
+    bool delay_ok = (t.delayed_count < delay_threshold) && t.average_delta_ms < delay_avg_max;
 
 #if defined(GPS_BLENDED_INSTANCE)
     if (instance == GPS_BLENDED_INSTANCE) {
-        return last_msg_valid && blend_health_check();
+        return delay_ok && blend_health_check();
     }
 #endif
 
-    return last_msg_valid &&
-           drivers[instance] != nullptr &&
+    return delay_ok && drivers[instance] != nullptr &&
            drivers[instance]->is_healthy();
 }
 

libraries/AP_GPS/AP_GPS.h

@@ -520,6 +520,12 @@ private:
 
         // delta time between the last pair of GPS updates in system milliseconds
         uint16_t delta_time_ms;
+
+        // count of delayed frames
+        uint8_t delayed_count;
+
+        // the average time delta
+        float average_delta_ms;
     };
     // Note allowance for an additional instance to contain blended data
     GPS_timing timing[GPS_MAX_INSTANCES];