diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index c26a8c452a..b642b57931 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1639,6 +1639,20 @@ bool AP_GPS::prepare_for_arming(void) { return all_passed; } +bool AP_GPS::logging_failed(void) const { + if (!logging_enabled()) { + return false; + } + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if ((drivers[i] != nullptr) && !(drivers[i]->logging_healthy())) { + return true; + } + } + + return false; +} + namespace AP { AP_GPS &gps() diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 98a40beca3..c065247d82 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -431,6 +431,12 @@ public: // returns true if all GPS instances have passed all final arming checks/state changes bool prepare_for_arming(void); + // returns false if any GPS drivers are not performing their logging appropriately + bool logging_failed(void) const; + + bool logging_present(void) const { return _raw_data != 0; } + bool logging_enabled(void) const { return _raw_data != 0; } + // used to disable GPS for GPS failure testing in flight void force_disable(bool disable) { _force_disable_gps = disable; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 30d6c046a9..33f9daed45 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -106,6 +106,17 @@ AP_GPS_SBF::read(void) return ret; } +bool AP_GPS_SBF::logging_healthy(void) const +{ + switch (gps._raw_data) { + case 1: + default: + return (RxState & SBF_DISK_MOUNTED) && (RxState & SBF_DISK_ACTIVITY); + case 2: + return ((RxState & SBF_DISK_MOUNTED) && (RxState & SBF_DISK_ACTIVITY)) || (!hal.util->get_soft_armed() && _has_been_armed); + } +} + bool AP_GPS_SBF::parse(uint8_t temp) { diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index e98f3ad83e..1eb306faeb 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -47,6 +47,8 @@ public: bool is_healthy(void) const override; + bool logging_healthy(void) const override; + bool prepare_for_arming(void) override; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 973d7611aa..cac356ab81 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -57,6 +57,8 @@ public: // driver specific health, returns true if the driver is healthy virtual bool is_healthy(void) const { return true; } + // returns true if the GPS is doing any logging it is expected to + virtual bool logging_healthy(void) const { return true; } virtual const char *name() const = 0;