From ccdd7699dece7c927e46be7f9a76bd85ec1ba8b5 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Fri, 15 Mar 2024 15:00:58 -0700
Subject: [PATCH] AP_Rangefinder: added a rate-limited and a compile-out option
 for the error reporting

---
 .../AP_RangeFinder_Ainstein_LR_D1.cpp              | 14 ++++++++++++--
 .../AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h |  1 +
 2 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp
index 32c923f547cf57..3e18c96bbe3aa4 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp
@@ -17,6 +17,10 @@
 
 #if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
 
+#ifndef AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS
+#define AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS 1
+#endif
+
 #include "AP_RangeFinder_Ainstein_LR_D1.h"
 #include <GCS_MAVLink/GCS.h>
 
@@ -69,10 +73,14 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m)
 
         has_data = true;
 
-        if (malfunction_alert_prev != malfunction_alert) {
+#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) {
             malfunction_alert_prev = malfunction_alert;
+            malfunction_alert_last_send_ms = now_ms;
             report_malfunction(malfunction_alert);
         }
+#endif
 
         /* From datasheet:
             Altitude measurements associated with a SNR value 
@@ -101,6 +109,7 @@ bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m)
     return has_data;
 }
 
+#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<uint8_t>(MalfunctionAlert::Temperature)) {
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Temperature alert");
@@ -115,5 +124,6 @@ void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint8_t _malfunctio
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Altitude reading overflow alert");
     }
 }
+#endif // AP_RANGEFINDER_AINSTEIN_LR_D1_SHOW_MALFUNCTIONS
 
-#endif
\ No newline at end of file
+#endif // AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h
index 54a87968d91406..f39bdea95b1ec5 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h
@@ -57,6 +57,7 @@ class AP_RangeFinder_Ainstein_LR_D1 : public AP_RangeFinder_Backend_Serial
 
     static constexpr uint8_t PACKET_SIZE = 32;
     uint8_t malfunction_alert_prev;
+    uint32_t malfunction_alert_last_send_ms;
     int8_t signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;    
 };
 #endif
\ No newline at end of file