diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 6b2c24e63c..fc6274cd93 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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(); +} diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 8b12160a30..5e55c353e7 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/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: diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index f250e139d1..702a18b6d3 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/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; +} diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 1139d92135..9665e54ed9 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/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; }; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 9219fca27d..27e233d279 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/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;