diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp index 775a004268af1..e8a2f8c319b0c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp @@ -74,9 +74,9 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m) #if AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS const uint32_t now_ms = AP_HAL::millis(); if (malfunction_alert_prev != malfunction_alert && now_ms - malfunction_alert_last_send_ms >= 1000) { + report_malfunction(malfunction_alert, malfunction_alert_prev); malfunction_alert_prev = malfunction_alert; malfunction_alert_last_send_ms = now_ms; - report_malfunction(malfunction_alert); } #endif @@ -109,18 +109,28 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m) } #if AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS -void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint8_t _malfunction_alert_) { - if (_malfunction_alert_ & static_cast(MalfunctionAlert::Temperature)) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Temperature alert"); - } - if (_malfunction_alert_ & static_cast(MalfunctionAlert::Voltage)) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Voltage alert"); - } - if (_malfunction_alert_ & static_cast(MalfunctionAlert::IFSignalSaturation)) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: IF signal saturation alert"); - } - if (_malfunction_alert_ & static_cast(MalfunctionAlert::AltitudeReading)) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Altitude reading overflow alert"); +void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint8_t _malfunction_alert_, const uint8_t _malfunction_alert_prev_) +{ + static const struct { + MalfunctionAlert bit; + const char *name; + } alerts[] { + { MalfunctionAlert::Temperature, "Temperature" }, + { MalfunctionAlert::Voltage, "Voltage" }, + { MalfunctionAlert::IFSignalSaturation, "IF signal saturation" }, + { MalfunctionAlert::AltitudeReading, "Attitude reading overflow" }, + }; + + for (const auto &alert : alerts) { + if ((_malfunction_alert_ & uint8_t(alert.bit)) == 0) { + // alert not current + continue; + } + if ((_malfunction_alert_prev_ & uint8_t(alert.bit)) != 0) { + // alert is not new + continue; + } + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: %s alert", alert.name); } } #endif // AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h index 3bf2d348c56aa..a0a8120bc4a9b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h @@ -46,7 +46,7 @@ class AP_RangeFinder_Ainstein_LR_D1 : public AP_RangeFinder_Backend_Serial // quality is not available int8_t get_signal_quality_pct() const override { return signal_quality_pct; }; - static void report_malfunction(const uint8_t _malfunction_alert_); + static void report_malfunction(const uint8_t _malfunction_alert_, const uint8_t _malfunction_alert_prev_); enum class MalfunctionAlert : uint8_t { Temperature = (1U << 0), // 0x01