From cdbc0c782bf08978e2f2ac882bf6d25eb901c851 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 29 Jun 2020 20:16:54 +0900
Subject: [PATCH] AP_RangeFinder: add sf30d binary protocol support

Co-authored-by: Tatsuya Yamaguchi <susumu-336@hotmail.co.jp>
---
 .../AP_RangeFinder_LightWareSerial.cpp        | 71 +++++++++++++++----
 .../AP_RangeFinder_LightWareSerial.h          | 17 ++++-
 2 files changed, 71 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
index 97355b1af0..57f79f6682 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
@@ -38,23 +38,66 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
     int16_t nbytes = uart->available();
     while (nbytes-- > 0) {
         char c = uart->read();
-        if (c == '\r') {
-            linebuf[linebuf_len] = 0;
-            const float dist = strtof(linebuf, NULL);
-            if (!is_negative(dist)) {
-                sum += dist;
-                valid_count++;
-            } else {
-                invalid_count++;
-            }
-            linebuf_len = 0;
-        } else if (isdigit(c) || c == '.' || c == '-') {
-            linebuf[linebuf_len++] = c;
-            if (linebuf_len == sizeof(linebuf)) {
-                // too long, discard the line
+
+        // use legacy protocol
+        if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::LEGACY) {
+            if (c == '\r') {
+                linebuf[linebuf_len] = 0;
+                const float dist = strtof(linebuf, NULL);
+                if (!is_negative(dist)) {
+                    sum += dist;
+                    valid_count++;
+                    // if still determining protocol update legacy valid count
+                    if (protocol_state == ProtocolState::UNKNOWN) {
+                        legacy_valid_count++;
+                    }
+                } else {
+                    invalid_count++;
+                }
                 linebuf_len = 0;
+            } else if (isdigit(c) || c == '.' || c == '-') {
+                linebuf[linebuf_len++] = c;
+                if (linebuf_len == sizeof(linebuf)) {
+                    // too long, discard the line
+                    linebuf_len = 0;
+                }
             }
         }
+
+        // use binary protocol
+        if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::BINARY) {
+            bool msb_set = BIT_IS_SET(c, 7);
+            if (msb_set) {
+                // received the high byte
+                high_byte = c;
+                high_byte_received = true;
+            } else {
+                // received the low byte which should be second
+                if (high_byte_received) {
+                    const float dist = (high_byte & 0x7f) << 7 | (c & 0x7f);
+                    if (!is_negative(dist)) {
+                        sum += dist * 0.01f;
+                        valid_count++;
+                        // if still determining protocol update binary valid count
+                        if (protocol_state == ProtocolState::UNKNOWN) {
+                            binary_valid_count++;
+                        }
+                    } else {
+                        invalid_count++;
+                    }
+                }
+                high_byte_received = false;
+            }
+        }
+    }
+
+    // protocol set after 10 successful reads
+    if (protocol_state == ProtocolState::UNKNOWN) {
+        if (binary_valid_count > 10) {
+            protocol_state = ProtocolState::BINARY;
+        } else if (legacy_valid_count > 10) {
+            protocol_state = ProtocolState::LEGACY;
+        }
     }
 
     uint32_t now = AP_HAL::millis();
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
index dd59ca6507..9c95f5a4d9 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
@@ -20,7 +20,18 @@ private:
     // get a reading
     bool get_reading(uint16_t &reading_cm) override;
 
-    char linebuf[10];
-    uint8_t linebuf_len = 0;
-    uint32_t last_init_ms;
+    char linebuf[10];           // legacy protocol buffer
+    uint8_t linebuf_len = 0;    // legacy protocol buffer length
+    uint32_t last_init_ms;      // init time used to switch lw20 to serial mode
+    uint8_t high_byte;          // binary protocol high byte
+    bool high_byte_received;    // true if high byte has been received
+
+    // automatic protocol decision variables
+    enum class ProtocolState {
+        UNKNOWN,
+        LEGACY,
+        BINARY
+    } protocol_state;
+    uint8_t legacy_valid_count;
+    uint8_t binary_valid_count;
 };