From f6aeb019944663fc1dcd3df6c7666adca8fad8a0 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 19 Nov 2021 11:03:14 +1100
Subject: [PATCH] AP_RangeFinder: signal quality reporting

---
 libraries/AP_RangeFinder/AP_RangeFinder_Backend.h          | 4 ++++
 .../AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp      | 2 ++
 libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h  | 7 +++++++
 3 files changed, 13 insertions(+)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
index 6751180d6c..ce9a83554a 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h
@@ -64,6 +64,10 @@ public:
     // get temperature reading in C.  returns true on success and populates temp argument
     virtual bool get_temp(float &temp) const { return false; }
 
+    // 0 is no return value, 100 is perfect.  false means signal
+    // quality is not available
+    virtual bool get_signal_quality_pct(uint8_t &quality_pct) const { return false; }
+
 protected:
 
     // update status based on distance measurement
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
index f5801ddaab..79a1e2055c 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
@@ -119,12 +119,14 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
     // return average of all valid readings
     if (valid_count > 0) {
         reading_m = sum / valid_count;
+        no_signal = false;
         return true;
     }
 
     // all readings were invalid so return out-of-range-high value
     if (invalid_count > 0) {
         reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
+        no_signal = true;
         return true;
     }
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
index 596f655fa9..590f1c55e7 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
@@ -16,6 +16,11 @@ protected:
         return MAV_DISTANCE_SENSOR_LASER;
     }
 
+    bool get_signal_quality_pct(uint8_t &quality_pct) const override {
+        quality_pct = no_signal ? 0 : 100;
+        return true;
+    }
+
 private:
     // get a reading
     bool get_reading(float &reading_m) override;
@@ -35,4 +40,6 @@ private:
     } protocol_state;
     uint8_t legacy_valid_count;
     uint8_t binary_valid_count;
+
+    bool no_signal = false;
 };