From 2dcf8a3b08b4cb16938235f37287475ac80ac450 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 30 Apr 2020 18:32:20 +1000
Subject: [PATCH] AP_RangeFinder: cope with beyond max range with LightwareI2C

---
 .../AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp  | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
index 02b850c116..4fda54ebeb 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
@@ -23,6 +23,8 @@ extern const AP_HAL::HAL& hal;
 #define LIGHTWARE_LOST_SIGNAL_TIMEOUT_WRITE_REG 23
 #define LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE 5
 
+#define LIGHTWARE_OUT_OF_RANGE_ADD_CM   100
+
 static const size_t lx20_max_reply_len_bytes = 32;
 static const size_t lx20_max_expected_stream_reply_len_bytes = 14;
 
@@ -400,6 +402,17 @@ bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf,
         (*p_num_processed_chars)++;
     }
 
+    /*
+      special case for being beyond maximum range, we receive a message like this:
+      ldl,1:-1.00
+      we will return max distance
+     */
+    if (strncmp((const char *)&stream_buf[*p_num_processed_chars], "-1.00", 5) == 0) {
+        val = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM);
+        (*p_num_processed_chars) += 5;
+        return true;
+    }
+
     /* Number is always returned in hundredths. So 6.33 is returned as 633. 6.3 is returned as 630.
      * 6 is returned as 600.
      * Extract number in format 6.33 or 123.99 (meters to be converted to centimeters).