From a7321cbae1c716c741d25fdf2be729b2269f37f5 Mon Sep 17 00:00:00 2001
From: "Wynn, Jesse Stewart" <wynn4@llnl.gov>
Date: Wed, 17 Nov 2021 13:21:48 -0800
Subject: [PATCH] AP_RangeFinder_LightWareSerial: check dist for lost signal
 flags

consider messages valid only if they are non-negative AND if the value is not a known lost-signal reading
---
 .../AP_RangeFinder_LightWareSerial.cpp        | 27 ++++++++++++++++---
 .../AP_RangeFinder_LightWareSerial.h          |  1 +
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
index 9e823dcaa8..f5801ddaab 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
@@ -34,6 +34,9 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
     uint16_t valid_count = 0;   // number of valid readings
     uint16_t invalid_count = 0; // number of invalid readings
 
+    // max distance the sensor can reliably measure - read from parameters
+    const int16_t distance_cm_max = max_distance_cm();
+
     // read any available lines from the lidar
     int16_t nbytes = uart->available();
     while (nbytes-- > 0) {
@@ -44,7 +47,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
             if (c == '\r') {
                 linebuf[linebuf_len] = 0;
                 const float dist = strtof(linebuf, nullptr);
-                if (!is_negative(dist)) {
+                if (!is_negative(dist) && !is_lost_signal_distance(dist * 100, distance_cm_max)) {
                     sum += dist;
                     valid_count++;
                     // if still determining protocol update legacy valid count
@@ -74,8 +77,8 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
             } else {
                 // received the low byte which should be second
                 if (high_byte_received) {
-                    const float dist = (high_byte & 0x7f) << 7 | (c & 0x7f);
-                    if (!is_negative(dist)) {
+                    const int16_t dist = (high_byte & 0x7f) << 7 | (c & 0x7f);
+                    if (dist >= 0 && !is_lost_signal_distance(dist, distance_cm_max)) {
                         sum += dist * 0.01f;
                         valid_count++;
                         // if still determining protocol update binary valid count
@@ -121,10 +124,26 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
 
     // all readings were invalid so return out-of-range-high value
     if (invalid_count > 0) {
-        reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
+        reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
         return true;
     }
 
     // no readings so return false
     return false;
 }
+
+// check to see if distance returned by the LiDAR is a known lost-signal distance flag
+bool AP_RangeFinder_LightWareSerial::is_lost_signal_distance(int16_t distance_cm, int16_t distance_cm_max)
+{
+    if (distance_cm < distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM) {
+        // in-range
+        return false;
+    }
+    const int16_t bad_distances[] { 13000, 16000, 23000, 25000 };
+    for (const auto bad_distance_cm : bad_distances) {
+        if (distance_cm == bad_distance_cm) {
+            return true;
+        }
+    }
+    return false;
+}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
index d98391f314..596f655fa9 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
@@ -19,6 +19,7 @@ protected:
 private:
     // get a reading
     bool get_reading(float &reading_m) override;
+    bool is_lost_signal_distance(int16_t distance_cm, int16_t distance_cm_max);
 
     char linebuf[10];           // legacy protocol buffer
     uint8_t linebuf_len;        // legacy protocol buffer length