From 153756b933cc8e4aa9812aa7b06beb6fea88c9b0 Mon Sep 17 00:00:00 2001
From: JacobCrabill <jacob@flyvoly.com>
Date: Wed, 13 Jan 2021 12:33:50 -0800
Subject: [PATCH] drivers: Add support for SF30/[B,C] rangefinders (Serial)

---
 .../lightware_laser_serial.cpp                | 48 +++++++++++++++++--
 .../lightware_laser_serial.hpp                |  1 +
 2 files changed, 45 insertions(+), 4 deletions(-)

diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
index ec74a38676..e31bdd9964 100644
--- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
+++ b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
@@ -110,6 +110,22 @@ LightwareLaserSerial::init()
 		_interval = 50000;
 		break;
 
+	case 6:
+		/* SF30/B (50m 39Hz) */
+		_px4_rangefinder.set_min_distance(0.2f);
+		_px4_rangefinder.set_max_distance(50.0f);
+		_interval = 1e6 / 39;
+		_simple_serial = true;
+		break;
+
+	case 7:
+		/* SF30/C (100m 39Hz) */
+		_px4_rangefinder.set_min_distance(0.2f);
+		_px4_rangefinder.set_max_distance(100.0f);
+		_interval = 1e6 / 39;
+		_simple_serial = true;
+		break;
+
 	default:
 		PX4_ERR("invalid HW model %d.", hw_model);
 		return -1;
@@ -172,12 +188,36 @@ int LightwareLaserSerial::collect()
 	float distance_m = -1.0f;
 	bool valid = false;
 
-	for (int i = 0; i < ret; i++) {
-		if (OK == lightware_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
-			valid = true;
+	if (_simple_serial) {
+		// Simplified protocol used by the SF30/B and SF30/C
+		// First byte: MSB of byte is set;  remaining bits are high "byte" of reading
+		// Second byte: MSB of byte is not set; remaining bits are low "byte" of reading
+		// Distance in centimeters = (buf[0] & 0x7F)*128 + buf[0]
+		bool have_msb = false;
+
+		for (int i = 0; i < ret; i++) {
+			if (have_msb && !(readbuf[i] & 0x80)) {
+				distance_m += readbuf[i] * .01f;
+				valid = true;
+				break;
+
+			} else {
+				if (readbuf[i] & 0x80) {
+					have_msb = true;
+					distance_m = (readbuf[i] & 0x7F) * 1.28f;
+				}
+			}
+		}
+
+	} else {
+		for (int i = 0; i < ret; i++) {
+			if (OK == lightware_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
+				valid = true;
+			}
 		}
 	}
 
+
 	if (!valid) {
 		return -EAGAIN;
 	}
@@ -237,7 +277,7 @@ void LightwareLaserSerial::Run()
 
 		unsigned speed;
 
-		if (hw_model == 5) {
+		if (hw_model >= 5) {
 			speed = B115200;
 
 		} else {
diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp
index 8095256e63..6b1229d1a1 100644
--- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp
+++ b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp
@@ -78,6 +78,7 @@ private:
 	unsigned			_linebuf_index{0};
 	enum LW_PARSE_STATE		_parse_state {LW_PARSE_STATE0_UNSYNC};
 	hrt_abstime			_last_read{0};
+	bool				_simple_serial{false};
 
 	unsigned			_consecutive_fail_count;