AP_RangeFinder: LightwareSerial driver handles invalid distances

reports the longer of 100m or RNGFNDx_MAX_CM+1m

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,13 +97,22 @@ 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;
+
+    // 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;
+}
+
 /* 
    update the state of the sensor
 */