From 8d5c51748f670a822ad7acb6c612dfb0f2a2b0ec Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 24 Jul 2020 13:13:27 +1000
Subject: [PATCH] AP_RangeFinder: fixed legacy parsing of 65436 for lightware
 i2c

some lidars will probe as legacy protocol and return 65436 as range
---
 libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
index 4fda54ebeb..2eac970d3a 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp
@@ -345,8 +345,13 @@ bool AP_RangeFinder_LightWareI2C::legacy_get_reading(uint16_t &reading_cm)
 
     // read the high and low byte distance registers
     if (_dev->transfer(&read_reg, 1, (uint8_t *)&val, sizeof(val))) {
-        // combine results into distance
-        reading_cm = be16toh(val);
+        int16_t signed_val = int16_t(be16toh(val));
+        if (signed_val < 0) {
+            // some lidar firmwares will return 65436 for out of range
+            reading_cm = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM);
+        } else {
+            reading_cm = uint16_t(signed_val);
+        }
         return true;
     }
     return false;