From 480a3ebb03f4c9bfac28a3f574574fd963f761c9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 27 Jul 2019 15:05:42 +0900
Subject: [PATCH] AP_RangeFinder: LightwareSerial driver handles invalid
 distances

reports the longer of 100m or RNGFNDx_MAX_CM+1m
---
 .../AP_RangeFinder_LightWareSerial.cpp        | 37 ++++++++++++++-----
 1 file changed, 28 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
index ece4d08a05..de0ae939a9 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
@@ -20,6 +20,9 @@
 
 extern const AP_HAL::HAL& hal;
 
+#define LIGHTWARE_DIST_MAX_CM           10000
+#define LIGHTWARE_OUT_OF_RANGE_ADD_CM   100
+
 /* 
    The constructor also initialises the rangefinder. Note that this
    constructor is not called until detect() returns true, so we
@@ -54,18 +57,25 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
         return false;
     }
 
+    float sum = 0;              // sum of all readings taken
+    uint16_t valid_count = 0;   // number of valid readings
+    uint16_t invalid_count = 0; // number of invalid readings
+
     // read any available lines from the lidar
-    float sum = 0;
-    uint16_t count = 0;
     int16_t nbytes = uart->available();
     while (nbytes-- > 0) {
         char c = uart->read();
         if (c == '\r') {
             linebuf[linebuf_len] = 0;
-            sum += (float)atof(linebuf);
-            count++;
+            const float dist = (float)atof(linebuf);
+            if (!is_negative(dist)) {
+                sum += dist;
+                valid_count++;
+            } else {
+                invalid_count++;
+            }
             linebuf_len = 0;
-        } else if (isdigit(c) || c == '.') {
+        } else if (isdigit(c) || c == '.' || c == '-') {
             linebuf[linebuf_len++] = c;
             if (linebuf_len == sizeof(linebuf)) {
                 // too long, discard the line
@@ -87,11 +97,20 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
         uart->write('d');
     }
 
-    if (count == 0) {
-        return false;
+    // return average of all valid readings
+    if (valid_count > 0) {
+        reading_cm = 100 * sum / valid_count;
+        return true;
     }
-    reading_cm = 100 * sum / count;
-    return true;
+
+    // all readings were invalid so return out-of-range-high value
+    if (invalid_count > 0) {
+        reading_cm = MIN(MAX(LIGHTWARE_DIST_MAX_CM, max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX);
+        return true;
+    }
+
+    // no readings so return false
+    return false;
 }
 
 /*