drivers/distance_sersor:add support for RDS02UF rangefinder driver on serial

src/drivers/distance_sensor/CMakeLists.txt

@@ -49,3 +49,4 @@ add_subdirectory(vl53l0x)
 add_subdirectory(vl53l1x)
 add_subdirectory(gy_us42)
 add_subdirectory(tfmini_i2c)
+add_subdirectory(rds02uf)

src/drivers/distance_sensor/Kconfig

@@ -19,6 +19,7 @@ menu "Distance sensors"
         select DRIVERS_DISTANCE_SENSOR_VL53L1X
         select DRIVERS_DISTANCE_SENSOR_GY_US42
         select DRIVERS_DISTANCE_SENSOR_TFMINI_I2C
+        select DRIVERS_DISTANCE_SENSOR_RDS02UF
         ---help---
             Enable default set of distance sensor drivers
 

src/drivers/distance_sensor/rds02uf/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__distance_sensor__rds02uf
+	MAIN rds02uf
+	SRCS
+		rds02uf.cpp
+		rds02uf.hpp
+		rds02uf_main.cpp
+		rds02uf_parser.cpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		drivers_rangefinder
+	)

src/drivers/distance_sensor/rds02uf/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_DISTANCE_SENSOR_RDS02UF
+	bool "tfmini"
+	default n
+	---help---
+		Enable support for rds02uf

src/drivers/distance_sensor/rds02uf/module.yaml

@@ -0,0 +1,26 @@
+module_name: Benewake RDS02UF Rangefinder
+serial_config:
+    - command: rds02uf start -d ${SERIAL_DEV} -R SENS_RDS02UF_ROT
+      port_config_param:
+        name: SENS_RDS02UF_CFG
+        group: Sensors
+
+parameters:
+    - group: Sensors
+      definitions:
+        SENS_RDS02UF_ROT:
+            description:
+                short: Distance Sensor Rotation
+                long: |
+                    Distance Sensor Rotation as MAV_SENSOR_ORIENTATION enum
+
+            type: enum
+            values:
+                25: ROTATION_DOWNWARD_FACING
+                24: ROTATION_UPWARD_FACING
+                12: ROTATION_BACKWARD_FACING
+                0: ROTATION_FORWARD_FACING
+                6: ROTATION_LEFT_FACING
+                2: ROTATION_RIGHT_FACING
+            reboot_required: true
+            default: 25

src/drivers/distance_sensor/rds02uf/rds02uf.cpp

@@ -0,0 +1,266 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2019, 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "rds02uf.hpp"
+
+#include <lib/drivers/device/Device.hpp>
+#include <parameters/param.h>
+#include <fcntl.h>
+
+Rds02uf::Rds02uf(const char *port, uint8_t rotation) :
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
+	_px4_rangefinder(0, rotation)
+{
+	// store port name
+	strncpy(_port, port, sizeof(_port) - 1);
+
+	// enforce null termination
+	_port[sizeof(_port) - 1] = '\0';
+
+	device::Device::DeviceId device_id;
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_RDS02UF;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	uint8_t bus_num = atoi(&_port[strlen(_port) - 1]); // Assuming '/dev/ttySx'
+
+	if (bus_num < 10) {
+		device_id.devid_s.bus = bus_num;
+	}
+
+	_px4_rangefinder.set_device_id(device_id.devid);
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR);
+}
+
+Rds02uf::~Rds02uf()
+{
+	// make sure we are truly inactive
+	stop();
+
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+Rds02uf::init()
+{
+	param_t _param_rds02uf_rot = param_find("SENS_RDS02UF_ROT");
+	int32_t rotation;
+	if (param_get(_param_rds02uf_rot, &rotation) == PX4_OK)
+	{
+		_px4_rangefinder.set_orientation(rotation);
+	}
+
+	_px4_rangefinder.set_min_distance(0.4f);
+	_px4_rangefinder.set_max_distance(20.0f);
+	// _px4_rangefinder.set_fov(math::radians(1.15f));
+
+	// status
+	int ret = 0;
+
+	do { // create a scope to handle exit conditions using break
+
+		// open fd
+		_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+		if (_fd < 0) {
+			PX4_ERR("Error opening fd");
+			return -1;
+		}
+
+		// baudrate 115200, 8 bits, no parity, 1 stop bit
+		unsigned speed = B115200;
+		termios uart_config{};
+		int termios_state{};
+
+		tcgetattr(_fd, &uart_config);
+
+		// clear ONLCR flag (which appends a CR for every LF)
+		uart_config.c_oflag &= ~ONLCR;
+
+		// set baud rate
+		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d ISPD", termios_state);
+			ret = -1;
+			break;
+		}
+
+		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d OSPD\n", termios_state);
+			ret = -1;
+			break;
+		}
+
+		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
+			PX4_ERR("baud %d ATTR", termios_state);
+			ret = -1;
+			break;
+		}
+
+		uart_config.c_cflag |= (CLOCAL | CREAD);	// ignore modem controls
+		uart_config.c_cflag &= ~CSIZE;
+		uart_config.c_cflag |= CS8;			// 8-bit characters
+		uart_config.c_cflag &= ~PARENB;			// no parity bit
+		uart_config.c_cflag &= ~CSTOPB;			// only need 1 stop bit
+		uart_config.c_cflag &= ~CRTSCTS;		// no hardware flowcontrol
+
+		// setup for non-canonical mode
+		uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+		uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+		uart_config.c_oflag &= ~OPOST;
+
+		// fetch bytes as they become available
+		uart_config.c_cc[VMIN] = 1;
+		uart_config.c_cc[VTIME] = 1;
+
+		if (_fd < 0) {
+			PX4_ERR("FAIL: laser fd");
+			ret = -1;
+			break;
+		}
+	} while (0);
+
+	// close the fd
+	::close(_fd);
+	_fd = -1;
+
+	if (ret == PX4_OK) {
+		start();
+	}
+
+	return ret;
+}
+
+int
+Rds02uf::collect()
+{
+	perf_begin(_sample_perf);
+	// clear buffer if last read was too long ago
+	int64_t read_elapsed = hrt_elapsed_time(&_last_read);
+	// the buffer for read chars is buflen minus null termination
+	char readbuf[sizeof(_linebuf)] {};
+	unsigned readlen = sizeof(readbuf) - 1;
+	int ret = 0;
+	float distance_m = -1.0f;
+	// Check the number of bytes available in the buffer
+	int bytes_available = 0;
+	::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
+
+	if (!bytes_available) {
+		perf_end(_sample_perf);
+		return 0;
+	}
+	// parse entire buffer
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	do {
+		// read from the sensor (uart buffer)
+		ret = ::read(_fd, &readbuf[0], readlen);
+		if (ret < 0) {
+			PX4_ERR("read err: %d", ret);
+			perf_count(_comms_errors);
+			perf_end(_sample_perf);
+
+			// only throw an error if we time out
+			if (read_elapsed > (kCONVERSIONINTERVAL * 2)) {
+				/* flush anything in RX buffer */
+				tcflush(_fd, TCIFLUSH);
+				return ret;
+
+			} else {
+				return -EAGAIN;
+			}
+		}
+
+		_last_read = hrt_absolute_time();
+
+		// parse buffer
+		for (int i = 0; i < ret; i++) {
+			rds02uf_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);
+		}
+
+		// bytes left to parse
+		bytes_available -= ret;
+
+	} while (bytes_available > 0);
+
+	// no valid measurement after parsing buffer
+	if (distance_m < 0.0f) {
+		perf_end(_sample_perf);
+		return -EAGAIN;
+	}
+
+	// publish most recent valid measurement from buffer
+	_px4_rangefinder.update(timestamp_sample, distance_m);
+
+	perf_end(_sample_perf);
+
+	return PX4_OK;
+}
+
+void
+Rds02uf::start()
+{
+	// schedule a cycle to start things (the sensor sends at 20Hz, but we run a bit faster to avoid missing data)
+	ScheduleOnInterval(47_ms);
+}
+
+void
+Rds02uf::stop()
+{
+	ScheduleClear();
+}
+
+void
+Rds02uf::Run()
+{
+	// fds initialized?
+	if (_fd < 0) {
+		// open fd
+		_fd = ::open(_port, O_RDWR | O_NOCTTY);
+	}
+
+	// perform collection
+	if (collect() == -EAGAIN) {
+		// reschedule to grab the missing bits, time to transmit 21 bytes @ 115200 bps
+		ScheduleClear();
+		ScheduleOnInterval(57_ms, 87 * 21);
+		return;
+	}
+}
+
+void
+Rds02uf::print_info()
+{
+	printf("Using port '%s'\n", _port);
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+}

src/drivers/distance_sensor/rds02uf/rds02uf.hpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2019, 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rds02uf.cpp
+ * @author
+ *
+ * Driver for the Benewake RDS02UF rangefinder series
+ */
+
+#pragma once
+
+#include <termios.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
+#include <uORB/topics/distance_sensor.h>
+
+#include "rds02uf_parser.h"
+
+#define RDS02UF_DEFAULT_PORT	"/dev/ttyS3"
+
+using namespace time_literals;
+
+class Rds02uf : public px4::ScheduledWorkItem
+{
+public:
+	Rds02uf(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	virtual ~Rds02uf();
+
+	int init();
+
+	void print_info();
+
+private:
+
+	int collect();
+
+	void Run() override;
+
+	void start();
+	void stop();
+
+	PX4Rangefinder	_px4_rangefinder;
+
+	RDS02UF_PARSE_STATE _parse_state {RDS02UF_PARSE_STATE::STATE0_SYNC_1};
+
+	char _linebuf[21] {};
+	char _port[20] {};
+
+	static constexpr int kCONVERSIONINTERVAL{50_ms};
+
+	int _fd{-1};
+
+	unsigned int _linebuf_index{0};
+
+	hrt_abstime _last_read{0};
+
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+
+};

src/drivers/distance_sensor/rds02uf/rds02uf_main.cpp

@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "rds02uf.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace rds02uf
+{
+
+Rds02uf	*g_dev{nullptr};
+
+int start(const char *port, uint8_t rotation);
+int status();
+int stop();
+int usage();
+
+int
+start(const char *port, uint8_t rotation)
+{
+	if (g_dev != nullptr) {
+		PX4_ERR("already started");
+		return PX4_OK;
+	}
+
+	// Instantiate the driver.
+	g_dev = new Rds02uf(port, rotation);
+
+	if (g_dev == nullptr) {
+		PX4_ERR("driver start failed");
+		return PX4_ERROR;
+	}
+
+	if (OK != g_dev->init()) {
+		PX4_ERR("driver start failed");
+		delete g_dev;
+		g_dev = nullptr;
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int
+status()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev != nullptr) {
+		PX4_INFO("stopping driver");
+		delete g_dev;
+		g_dev = nullptr;
+		PX4_INFO("driver stopped");
+
+	} else {
+		PX4_ERR("driver not running");
+		return 1;
+	}
+
+	return PX4_OK;
+}
+
+int
+usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+Serial bus driver for the Benewake RDS02UF radar.
+
+Most boards are configured to enable/start the driver on a specified UART using the SENS_RDS02_CFG parameter.
+
+Setup/usage information:
+
+### Examples
+
+Attempt to start driver on a specified serial device.
+$ rds02uf start -d /dev/ttyS1
+Stop driver
+$ rds02uf stop
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("rds02uf", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status","Driver status");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status");
+	return PX4_OK;
+}
+
+} // namespace
+
+extern "C" __EXPORT int rds02uf_main(int argc, char *argv[])
+{
+	int ch = 0;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	const char *device_path = RDS02UF_DEFAULT_PORT;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			break;
+
+		case 'd':
+			device_path = myoptarg;
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+			return PX4_ERROR;
+		}
+	}
+
+	if (myoptind >= argc) {
+		PX4_ERR("unrecognized command");
+		return rds02uf::usage();
+	}
+
+	if (!strcmp(argv[myoptind], "start")) {
+		if (strcmp(device_path, "") != 0) {
+			return rds02uf::start(device_path, rotation);
+
+		} else {
+			PX4_WARN("Please specify device path!");
+			return rds02uf::usage();
+		}
+
+	} else if (!strcmp(argv[myoptind], "stop")) {
+		return rds02uf::stop();
+
+	} else if (!strcmp(argv[myoptind], "status")) {
+		return rds02uf::status();
+	}
+
+	return rds02uf::usage();
+}

src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp

@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file modified from sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Chuong Nguyen <chnguye7@asu.edu>
+ * @author Ayush Gaud <ayush.gaud@gmail.com>
+ * @author Zebulon <zebulon-86@outlook.com>
+ *
+ * Declarations of parser for the Benewake Rds02UF rangefinder series
+ */
+
+#include "rds02uf_parser.h"
+#include <string.h>
+#include <stdlib.h>
+
+#define RDS02UF_HEAD_LEN 2
+#define RDS02UF_PRE_DATA_LEN 6
+#define RDS02UF_DATA_LEN 10
+// #define RDS02UF_DEBUG
+
+#ifdef RDS02UF_DEBUG
+#include <stdio.h>
+
+const char *parser_state[] = {
+
+	"0_STATE1_SYNC_1",
+	"1_STATE2_SYNC_2",
+	"2_STATE3_ADDRESS",
+	"3_STATE4_ERROR_CODE",
+	"4_STATE5_FC_CODE_L",
+	"5_STATE6_FC_CODE_H",
+	"6_STATE7_LENGTH_L",
+	"7_STATE8_LENGTH_H",
+	"8_STATE9_REAL_DATA",
+	"9_STATE10_CRC",
+	"10_STATE11_END_1",
+	"11_STATE12_END_2"
+};
+#endif
+
+int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist)
+{
+	int ret = -1;
+	char data = c;
+	uint8_t crc_data = 0;
+
+	switch (*state) {
+	case RDS02UF_PARSE_STATE::STATE0_SYNC_1:
+		if (data == RDS02_HEAD1)
+		{
+			parserbuf[*parserbuf_index] = data;
+			(*parserbuf_index)++;
+			*state = RDS02UF_PARSE_STATE::STATE1_SYNC_2;
+		}
+		break;
+	case RDS02UF_PARSE_STATE::STATE1_SYNC_2:
+		if (data == RDS02_HEAD2)
+		{
+			parserbuf[*parserbuf_index] = data;
+			(*parserbuf_index)++;
+			*state = RDS02UF_PARSE_STATE::STATE2_ADDRESS;
+		}else{
+			*parserbuf_index = 0;
+			*state = RDS02UF_PARSE_STATE::STATE0_SYNC_1;
+		}
+		break;
+	case RDS02UF_PARSE_STATE::STATE2_ADDRESS: // address
+		parserbuf[*parserbuf_index] = data;
+		(*parserbuf_index)++;
+		*state = RDS02UF_PARSE_STATE::STATE3_ERROR_CODE;
+		break;
+	case RDS02UF_PARSE_STATE::STATE3_ERROR_CODE: // error_code
+		parserbuf[*parserbuf_index] = data;
+		(*parserbuf_index)++;
+		*state = RDS02UF_PARSE_STATE::STATE4_FC_CODE_L;
+		break;
+	case RDS02UF_PARSE_STATE::STATE4_FC_CODE_L: // fc_code low
+		parserbuf[*parserbuf_index] = data;
+		(*parserbuf_index)++;
+		*state = RDS02UF_PARSE_STATE::STATE5_FC_CODE_H;
+		break;
+	case RDS02UF_PARSE_STATE::STATE5_FC_CODE_H: // fc_code high
+		parserbuf[*parserbuf_index] = data;
+		(*parserbuf_index)++;
+		*state = RDS02UF_PARSE_STATE::STATE6_LENGTH_L;
+		break;
+	case RDS02UF_PARSE_STATE::STATE6_LENGTH_L: // lengh_low
+		parserbuf[*parserbuf_index] = data;
+		(*parserbuf_index)++;
+		*state = RDS02UF_PARSE_STATE::STATE7_LENGTH_H;
+		break;
+	case RDS02UF_PARSE_STATE::STATE7_LENGTH_H: // lengh_high
+		{
+			uint8_t read_len = data << 8 | parserbuf[*parserbuf_index-1];
+			if ( read_len == RDS02UF_DATA_LEN)	// rds02uf data length is 10
+			{
+				parserbuf[*parserbuf_index] = data;
+				(*parserbuf_index)++;
+				*state = RDS02UF_PARSE_STATE::STATE8_REAL_DATA;
+			}else{
+				*parserbuf_index = 0;
+				*state = RDS02UF_PARSE_STATE::STATE0_SYNC_1;
+			}
+			break;
+		}
+
+	case RDS02UF_PARSE_STATE::STATE8_REAL_DATA: // real_data
+		parserbuf[*parserbuf_index] = data;
+		(*parserbuf_index)++;
+		if ((*parserbuf_index) == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN))
+		{
+			*state = RDS02UF_PARSE_STATE::STATE9_CRC;
+		}
+		break;
+	case RDS02UF_PARSE_STATE::STATE9_CRC: // crc
+		crc_data = crc8(&parserbuf[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN);
+		parserbuf[*parserbuf_index] = data;
+		if (crc_data == data || data == 0xff)
+		{
+			(*parserbuf_index)++;
+			*state = RDS02UF_PARSE_STATE::STATE10_END_1;
+		}else{
+			*parserbuf_index = 0;
+			*state = RDS02UF_PARSE_STATE::STATE0_SYNC_1;
+		}
+		break;
+	case RDS02UF_PARSE_STATE::STATE10_END_1: //
+		if (data == RDS02_END)
+		{
+			parserbuf[*parserbuf_index] = data;
+			(*parserbuf_index)++;
+			*state = RDS02UF_PARSE_STATE::STATE11_END_2;
+		}else{
+			*parserbuf_index = 0;
+			*state = RDS02UF_PARSE_STATE::STATE0_SYNC_1;
+		}
+
+		break;
+	case RDS02UF_PARSE_STATE::STATE11_END_2: //
+		{
+			uint16_t fc_code = (parserbuf[STATE5_FC_CODE_H] << 8 | parserbuf[STATE4_FC_CODE_L]);
+			uint8_t err_code = parserbuf[STATE3_ERROR_CODE];
+			if (data == RDS02_END)
+			{
+				if (fc_code == 0x03ff && err_code == 0)	// get targer information
+				{
+					if(parserbuf[RDS02_DATA_START_INDEX] == RDS02_TARGET_INFO)
+					{
+						float distance = (parserbuf[RDS02_DATA_Y_INDEX + 1] * 256 + parserbuf[RDS02_DATA_Y_INDEX]) / 100.0f;
+						*dist = distance;
+						ret = true;
+					}
+				}
+			}
+
+			*parserbuf_index = 0;
+			*state = RDS02UF_PARSE_STATE::STATE0_SYNC_1;
+
+			break;
+		}
+	}
+
+#ifdef RDS02UF_DEBUG
+	static u_int16_t cnt;
+	cnt += 1;
+	static RDS02UF_PARSE_STATE last_state = RDS02UF_PARSE_STATE::STATE12_END_2;
+	if (*state != last_state || cnt > 500)
+	{
+		printf("state: %s,read: %02x\n", parser_state[*state],data);
+		last_state = *state;
+		cnt = 0;
+	}
+#endif
+
+	return ret;
+}
+
+uint8_t crc8(char* pbuf, int32_t len)
+{
+     char* data = pbuf;
+     uint8_t crc = 0;
+     while ( len-- )
+     crc = crc8_table[crc^*(data++)];
+     return crc;
+}

src/drivers/distance_sensor/rds02uf/rds02uf_parser.h

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file modified from sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Chuong Nguyen <chnguye7@asu.edu>
+ * @author Ayush Gaud <ayush.gaud@gmail.com>
+ * @author Zebulon <zebulon-86@outlook.com>
+ *
+ * Declarations of parser for the Benewake Rds02UF rangefinder series
+ */
+
+#pragma once
+
+#define RDS02_HEAD1 0x55
+#define RDS02_HEAD2 0x55
+#define RDS02_END 0xAA
+
+#define RDS02_BUFFER_SIZE 50
+#define RDS02_DATA_START_INDEX 8
+#define RDS02_DATA_Y_INDEX 13
+#define RDS02_DATA_FC_INDEX    RDS02_DATA_START_INDEX
+
+
+#define RDS02_TARGET_INFO  0x0C
+// Data Format for Benewake Rds02UF
+// ===============================
+// 21 bytes total per message:
+// 1) 0x55
+// 2) 0x55
+// 3) address
+// 4) error_code
+// 5) FC_CODE_L (low 8bit)
+// 6) FC_CODE_H (high 8bit)
+// 7) LENGTH_L (low 8bit)
+// 8) LENGTH_H (high 8bit)
+// 9) REAL_DATA (10Byte)
+// 10) CRC8
+// 11) END_1 0xAA
+// 12) END_2 0xAA
+
+
+
+enum RDS02UF_PARSE_STATE {
+	STATE0_SYNC_1 = 0,
+	STATE1_SYNC_2,
+	STATE2_ADDRESS,
+	STATE3_ERROR_CODE,
+	STATE4_FC_CODE_L,
+	STATE5_FC_CODE_H,
+	STATE6_LENGTH_L,
+	STATE7_LENGTH_H,
+	STATE8_REAL_DATA,
+	STATE9_CRC,
+	STATE10_END_1,
+	STATE11_END_2
+};
+
+int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist);
+uint8_t crc8(char* pbuf, int32_t len);
+
+const uint8_t crc8_table[256] = {
+    0x93,0x98,0xE4,0x46,0xEB,0xBA,0x04,0x4C,
+    0xFA,0x40,0xB8,0x96,0x0E,0xB2,0xB7,0xC0,
+    0x0C,0x32,0x9B,0x80,0xFF,0x30,0x7F,0x9D,
+    0xB3,0x81,0x58,0xE7,0xF1,0x19,0x7E,0xB6,
+    0xCD,0xF7,0xB4,0xCB,0xBC,0x5C,0xD6,0x09,
+    0x20,0x0A,0xE0,0x37,0x51,0x67,0x24,0x95,
+    0xE1,0x62,0xF8,0x5E,0x38,0x15,0x54,0x77,
+    0x63,0x57,0x6D,0xE9,0x89,0x76,0xBE,0x41,
+    0x5D,0xF9,0xB1,0x4D,0x6C,0x53,0x9C,0xA2,
+    0x23,0xC4,0x8E,0xC8,0x05,0x42,0x61,0x71,
+    0xC5,0x00,0x18,0x6F,0x5F,0xFB,0x7B,0x11,
+    0x65,0x2D,0x8C,0xED,0x14,0xAB,0x88,0xD5,
+    0xD9,0xC2,0x36,0x34,0x7C,0x5B,0x3C,0xF6,
+    0x48,0x0B,0xEE,0x02,0x83,0x79,0x17,0xE6,
+    0xA8,0x78,0xF5,0xD3,0x4E,0x50,0x52,0x91,
+    0xD8,0xC6,0x22,0xEC,0x3B,0xE5,0x3F,0x86,
+    0x06,0xCF,0x2B,0x2F,0x3D,0x59,0x1C,0x87,
+    0xEF,0x4F,0x10,0xD2,0x7D,0xDA,0x72,0xA0,
+    0x9F,0xDE,0x6B,0x75,0x56,0xBD,0xC7,0xC1,
+    0x70,0x1D,0x25,0x92,0xA5,0x31,0xE2,0xD7,
+    0xD0,0x9A,0xAF,0xA9,0xC9,0x97,0x08,0x33,
+    0x5A,0x99,0xC3,0x16,0x84,0x82,0x8A,0xF3,
+    0x4A,0xCE,0xDB,0x29,0x0F,0xAE,0x6E,0xE3,
+    0x8B,0x07,0x3A,0x74,0x47,0xB0,0xBB,0xB5,
+    0x7A,0xAA,0x2C,0xD4,0x03,0x3E,0x1A,0xA7,
+    0x27,0x64,0x06,0xBF,0x55,0x73,0x1E,0xFE,
+    0x49,0x01,0x39,0x28,0xF4,0x26,0xDF,0xDD,
+    0x44,0x0D,0x21,0xF2,0x85,0xB9,0xEA,0x4B,
+    0xDC,0x6A,0xCA,0xAC,0x12,0xFC,0x2E,0x2A,
+    0xA3,0xF0,0x66,0xE8,0x60,0x45,0xA1,0x8D,
+    0x68,0x35,0xFD,0x8F,0x9E,0x1F,0x13,0xD1,
+    0xAD,0x69,0xCC,0xA4,0x94,0x90,0x1B,0x43,
+    };

src/drivers/drv_sensor.h

@@ -218,6 +218,7 @@
 #define DRV_BARO_DEVTYPE_ICP10111 0xC1
 
 #define DRV_DIST_DEVTYPE_TFMINI_I2C       0xC2
+#define DRV_DIST_DEVTYPE_RDS02UF       0xC3
 
 #define DRV_DEVTYPE_UNUSED		0xff