From a09cd8411d2dc9d2597c353e098ff07ef640d7c2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 8 Nov 2018 10:04:03 +0900
Subject: [PATCH] AP_RangeFinder: benewake lidar returns max range + 1m when
 out-of-range

also returns out-of-range when signal is weak
---
 .../AP_RangeFinder_Benewake.cpp               | 57 +++++++++++--------
 .../AP_RangeFinder/AP_RangeFinder_Benewake.h  |  4 +-
 2 files changed, 34 insertions(+), 27 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
index c61d6bcb39..1260ff458a 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
@@ -24,6 +24,7 @@ extern const AP_HAL::HAL& hal;
 #define BENEWAKE_FRAME_HEADER 0x59
 #define BENEWAKE_FRAME_LENGTH 9
 #define BENEWAKE_DIST_MAX_CM 32768
+#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
 
 // format of serial packets received from benewake lidar
 //
@@ -69,7 +70,7 @@ bool AP_RangeFinder_Benewake::detect(AP_SerialManager &serial_manager, uint8_t s
 }
 
 // distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
-bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
+bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
 {
     if (uart == nullptr) {
         return false;
@@ -77,7 +78,7 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
 
     float sum_cm = 0;
     uint16_t count = 0;
-    bool dist_reliable = false;
+    uint16_t count_out_of_range = 0;
 
     // read any available lines from the lidar
     int16_t nbytes = uart->available();
@@ -95,7 +96,6 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
                 linebuf[linebuf_len++] = c;
             } else {
                 linebuf_len = 0;
-                dist_reliable = false;
             }
         } else {
             // add character to buffer
@@ -109,8 +109,6 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
                 }
                 // if checksum matches extract contents
                 if ((uint8_t)(checksum & 0xFF) == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
-                    // tell caller we are receiving packets
-                    signal_ok = true;
                     // calculate distance and add to sum
                     uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
                     if (dist < BENEWAKE_DIST_MAX_CM) {
@@ -119,30 +117,45 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
                             if (linebuf[6] == 0x02) {
                                 dist *= 0.1f;
                             }
-                            // no signal byte from TFmini
-                            dist_reliable = true;
-                        } else {
-                            // TF02 provides signal reliability (good = 7 or 8)
-                            dist_reliable = (linebuf[6] >= 7);
-                        }
-                        if (dist_reliable) {
+                            // no signal byte from TFmini so add distance to sum
                             sum_cm += dist;
                             count++;
+                        } else {
+                            // TF02 provides signal reliability (good = 7 or 8)
+                            if (linebuf[6] >= 7) {
+                                // add distance to sum
+                                sum_cm += dist;
+                                count++;
+                            } else {
+                                // this reading is out of range
+                                count_out_of_range++;
+                            }
                         }
+                    } else {
+                        // this reading is out of range
+                        count_out_of_range++;
                     }
                 }
                 // clear buffer
                 linebuf_len = 0;
-                dist_reliable = false;
             }
         }
     }
 
-    if (count == 0) {
-        return false;
+    if (count > 0) {
+        // return average distance of readings
+        reading_cm = sum_cm / count;
+        return true;
     }
-    reading_cm = sum_cm / count;
-    return true;
+
+    if (count_out_of_range > 0) {
+        // if only out of range readings return max range + 1m
+        reading_cm = max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM;
+        return true;
+    }
+
+    // no readings so return false
+    return false;
 }
 
 /* 
@@ -150,16 +163,10 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
 */
 void AP_RangeFinder_Benewake::update(void)
 {
-    bool signal_ok;
-    if (get_reading(state.distance_cm, signal_ok)) {
+    if (get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
         state.last_reading_ms = AP_HAL::millis();
-        if (signal_ok) {
-            update_status();
-        } else {
-            // if signal is weak set status to out-of-range
-            set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
-        }
+        update_status();
     } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
         set_status(RangeFinder::RangeFinder_NoData);
     }
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
index 570667b7ab..8abeab1c91 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
@@ -34,8 +34,8 @@ protected:
 private:
 
     // get a reading
-    // distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
-    bool get_reading(uint16_t &reading_cm, bool &signal_ok);
+    // distance returned in reading_cm
+    bool get_reading(uint16_t &reading_cm);
 
     AP_HAL::UARTDriver *uart = nullptr;
     benewake_model_type model_type;