AP_GPS: SBF track the GPS error bits

libraries/AP_GPS/AP_GPS.cpp

@@ -1500,3 +1500,9 @@ void AP_GPS::calc_blended_state(void)
     timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1;
     timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2;
 }
+
+bool AP_GPS::is_healthy(uint8_t instance) const {
+    return drivers[instance] != nullptr &&
+           last_message_delta_time_ms(instance) < GPS_MAX_DELTA_MS &&
+           drivers[instance]->is_healthy();
+}

libraries/AP_GPS/AP_GPS.h

@@ -401,8 +401,9 @@ public:
     // indicate which bit in LOG_BITMASK indicates gps logging enabled
     void set_log_gps_bit(uint32_t bit) { _log_gps_bit = bit; }
 
-    // report if the gps is healthy (this is defined as having received an update at a rate greater than 4Hz)
-    bool is_healthy(uint8_t instance) const { return last_message_delta_time_ms(instance) < GPS_MAX_DELTA_MS; }
+    // report if the gps is healthy (this is defined as existing, an update at a rate greater than 4Hz,
+    // as well as any driver specific behaviour)
+    bool is_healthy(uint8_t instance) const;
     bool is_healthy(void) const { return is_healthy(primary_instance); }
 
 protected:

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -41,6 +41,14 @@ do {                                            \
 
 #define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte
 
+#define RX_ERROR_MASK (SOFTWARE      | \
+                       CONGESTION    | \
+                       MISSEDEVENT   | \
+                       CPUOVERLOAD   | \
+                       INVALIDCONFIG | \
+                       OUTOFGEOFENCE)
+
+
 AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
                        AP_HAL::UARTDriver *_port) :
     AP_GPS_Backend(_gps, _state, _port)
@@ -339,6 +347,7 @@ AP_GPS_SBF::process_message(void)
     {
         const msg4014 &temp = sbf_msg.data.msg4014u;
         RxState = temp.RxState;
+        RxError = temp.RxError;
         break;
     }
     case VelCovGeodetic:
@@ -387,3 +396,7 @@ bool AP_GPS_SBF::is_configured (void) {
             (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
              _init_blob_index >= ARRAY_SIZE(_initialisation_blob));
 }
+
+bool AP_GPS_SBF::is_healthy (void) const {
+    return (RxError & RX_ERROR_MASK) == 0;
+}

libraries/AP_GPS/AP_GPS_SBF.h

@@ -46,6 +46,9 @@ public:
     // get the velocity lag, returns true if the driver is confident in the returned value
     bool get_lag(float &lag_sec) const override { lag_sec = 0.08f; return true; } ;
 
+    bool is_healthy(void) const override;
+
+
 private:
 
     bool parse(uint8_t temp);
@@ -66,6 +69,7 @@ private:
     uint32_t crc_error_counter = 0;
     uint32_t last_injected_data_ms = 0;
     uint32_t RxState;
+    uint32_t RxError;
 
     enum sbf_ids {
         DOP = 4001,
@@ -185,4 +189,14 @@ private:
     } sbf_msg;
 
     void log_ExtEventPVTGeodetic(const msg4007 &temp);
+
+    enum {
+        SOFTWARE      = (1 << 3),
+        WATCHDOG      = (1 << 4),
+        CONGESTION    = (1 << 6),
+        MISSEDEVENT   = (1 << 8),
+        CPUOVERLOAD   = (1 << 9),
+        INVALIDCONFIG = (1 << 10),
+        OUTOFGEOFENCE = (1 << 11),
+    } RxError_bits;
 };

libraries/AP_GPS/GPS_Backend.h

@@ -56,6 +56,9 @@ public:
     // driver specific lag, returns true if the driver is confident in the provided lag
     virtual bool get_lag(float &lag) const { lag = 0.2f; return true; }
 
+    // driver specific health, returns true if the driver is healthy
+    virtual bool is_healthy(void) const { return true; }
+
     virtual const char *name() const = 0;
 
     void broadcast_gps_type() const;