Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: Notify different alerts

Browse Source
This commit is contained in:
muramura 2024-03-24 01:09:30 +09:00 committed by Andrew Tridgell
parent beb3dd1519
commit 95f4b326a4
2 changed files with 14 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp
View File

@ -74,9 +74,9 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m)
#if AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS #if AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS
const uint32_t now_ms = AP_HAL::millis(); const uint32_t now_ms = AP_HAL::millis();
if (malfunction_alert_prev != malfunction_alert && now_ms - malfunction_alert_last_send_ms >= 1000) { 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_prev = malfunction_alert;
malfunction_alert_last_send_ms = now_ms; malfunction_alert_last_send_ms = now_ms;
report_malfunction(malfunction_alert);
} }
#endif #endif
@ -109,17 +109,24 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m)
} }
#if AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS #if AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS
void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint8_t _malfunction_alert_) { void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint8_t _malfunction_alert_, const uint8_t _malfunction_alert_prev_)
if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::Temperature)) { {
// _malfunction_alert_ _malfunction_alert_prev_
// 0 0 : not message
// 1 0 : message
// 0 1 : not message
// 1 1 : not message
const uint8_t alert = _malfunction_alert_ & (_malfunction_alert_ ^ _malfunction_alert_prev_);
if (alert & static_cast<uint8_t>(MalfunctionAlert::Temperature)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Temperature alert"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Temperature alert");
} }
if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::Voltage)) { if (alert & static_cast<uint8_t>(MalfunctionAlert::Voltage)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Voltage alert"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Voltage alert");
} }
if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::IFSignalSaturation)) { if (alert & static_cast<uint8_t>(MalfunctionAlert::IFSignalSaturation)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: IF signal saturation alert"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: IF signal saturation alert");
} }
if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::AltitudeReading)) { if (alert & static_cast<uint8_t>(MalfunctionAlert::AltitudeReading)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Altitude reading overflow alert"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Altitude reading overflow alert");
} }
} }

2
libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h
View File

@ -46,7 +46,7 @@ private:
// quality is not available // quality is not available
int8_t get_signal_quality_pct() const override { return signal_quality_pct; }; 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 { enum class MalfunctionAlert : uint8_t {
Temperature = (1U << 0), // 0x01 Temperature = (1U << 0), // 0x01