diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp index b55c03ee9d..db3791478f 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp @@ -28,8 +28,6 @@ void AP_Generator_IE_FuelCell::init() return; } _uart->begin(AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_Generator, 0)); - - _health_warn_sent = false; } // Update fuelcell, expected to be called at 20hz @@ -62,7 +60,7 @@ void AP_Generator_IE_FuelCell::update() _healthy = (now - _last_time_ms) < HEALTHY_TIMEOUT_MS; // Check if we should notify gcs off any change of fuel cell state - check_status(); + check_status(now); update_frontend(); @@ -144,16 +142,16 @@ const AP_Generator_IE_FuelCell::Lookup_State AP_Generator_IE_FuelCell::lookup_st }; // Check for any change in error state or status and report to gcs -void AP_Generator_IE_FuelCell::check_status() +void AP_Generator_IE_FuelCell::check_status(const uint32_t now) { // Check driver health - if (!healthy() && !_health_warn_sent) { + if (!healthy() && (!_health_warn_last_ms || (now - _health_warn_last_ms >= 20000))) { // Don't spam GCS with unhealthy message - _health_warn_sent = true; + _health_warn_last_ms = now; gcs().send_text(MAV_SEVERITY_ALERT, "Generator: Not healthy"); } else if (healthy()) { - _health_warn_sent = false; + _health_warn_last_ms = 0; } // If fuel cell state has changed send gcs message diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h index 1f4f92e9ee..7b50018743 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h @@ -54,7 +54,7 @@ protected: State _last_state; // The previous PSU state uint32_t _last_time_ms; // Time we got a reading bool _healthy; // Is the driver working - bool _health_warn_sent; + uint32_t _health_warn_last_ms; // Time to persist warning message without spamming // Temporary state params struct ParsedValue { @@ -94,7 +94,7 @@ protected: virtual void decode_latest_term(void) = 0; // Check if we should notify on any change of fuel cell state - void check_status(void); + void check_status(const uint32_t now); // Check error codes and populate message with error code virtual bool check_for_err_code(char* msg_txt, uint8_t msg_len) const = 0;