RDS02UF rangefinder serial driver ok

src/drivers/distance_sensor/rds02uf/module.yaml

@@ -1,15 +1,14 @@
 module_name: Benewake RDS02UF Rangefinder
 serial_config:
-    - command: rds02uf start -d ${SERIAL_DEV} -R SENS_RDS02_ROT
+    - command: rds02uf start -d ${SERIAL_DEV} -R SENS_RDS02UF_ROT
       port_config_param:
         name: SENS_RDS02UF_CFG
         group: Sensors
 
-
 parameters:
     - group: Sensors
       definitions:
-        SENS_RDS02_ROT:
+        SENS_RDS02UF_ROT:
             description:
                 short: Distance Sensor Rotation
                 long: |

src/drivers/distance_sensor/rds02uf/rds02uf.cpp

@@ -34,6 +34,7 @@
 #include "rds02uf.hpp"
 
 #include <lib/drivers/device/Device.hpp>
+#include <parameters/param.h>
 #include <fcntl.h>
 
 Rds02uf::Rds02uf(const char *port, uint8_t rotation) :
@@ -72,10 +73,16 @@ Rds02uf::~Rds02uf()
 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(12.0f);
+	_px4_rangefinder.set_max_distance(20.0f);
-	_px4_rangefinder.set_fov(math::radians(1.15f));
+	// _px4_rangefinder.set_fov(math::radians(1.15f));
 
 	// status
 	int ret = 0;
@@ -157,17 +164,13 @@ 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);
@@ -176,14 +179,11 @@ Rds02uf::collect()
 		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);
@@ -252,7 +252,7 @@ Rds02uf::Run()
 	if (collect() == -EAGAIN) {
 		// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps
 		ScheduleClear();
-		ScheduleOnInterval(50_ms, 87 * 12);
+		ScheduleOnInterval(50_ms, 87 * 21);
 		return;
 	}
 }

src/drivers/distance_sensor/rds02uf/rds02uf.hpp

@@ -79,10 +79,10 @@ private:
 
 	RDS02UF_PARSE_STATE _parse_state {RDS02UF_PARSE_STATE::STATE1_SYNC_1};
 
-	char _linebuf[10] {};
+	char _linebuf[21] {};
 	char _port[20] {};
 
-	static constexpr int kCONVERSIONINTERVAL{9_ms};
+	static constexpr int kCONVERSIONINTERVAL{50_ms};
 
 	int _fd{-1};
 

src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp

@@ -33,26 +33,49 @@
 
 /**
  * @file modified from sf0x_parser.cpp
- * @author
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Chuong Nguyen <chnguye7@asu.edu>
+ * @author Ayush Gaud <ayush.gaud@gmail.com>
+ * @author Jim Brown <zbr3550@163.com>
  *
- * Declarations of parser for the Benewake TFmini laser rangefinder series
+ * Declarations of parser for the Benewake Rds02UF rangefinder series
  */
 
 #include "rds02uf_parser.h"
 #include <string.h>
 #include <stdlib.h>
 
-uint16_t data_len;
+// #define RDS02UF_DEBUG
-uint16_t data_index;
-uint8_t crc_data;
-uint16_t fc_code;
-uint8_t err_code;
 
+#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
+
+uint16_t data_len = 0;
+uint16_t data_index = 0;
+uint16_t fc_code = 0;
+uint8_t err_code = 0;
 int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist)
 {
 	int ret = -1;
 	char data = c;
-	//char *end;
+	uint8_t crc_data = 0;
 
 	switch (*state) {
 	case RDS02UF_PARSE_STATE::STATE1_SYNC_1:
@@ -69,10 +92,9 @@ int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PA
 			parserbuf[*parserbuf_index] = data;
 			(*parserbuf_index)++;
 			*state = RDS02UF_PARSE_STATE::STATE3_ADDRESS;
-		}
+		}else{
-		else
+			*parserbuf_index = 0;
-		{
+			*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
-			ParseDataFailed(parserbuf, sizeof(parserbuf));
 		}
 		break;
 	case RDS02UF_PARSE_STATE::STATE3_ADDRESS: // address
@@ -106,43 +128,49 @@ int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PA
 		break;
 	case RDS02UF_PARSE_STATE::STATE8_LENGTH_H: // lengh_high
 		data_len = data << 8 | data_len;
+		if (data_len == 10)
+		{
 			parserbuf[*parserbuf_index] = data;
 			(*parserbuf_index)++;
+			data_index = 0;
 			*state = RDS02UF_PARSE_STATE::STATE9_REAL_DATA;
+		}else{
+			*parserbuf_index = 0;
+			*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
+		}
+
 		break;
 	case RDS02UF_PARSE_STATE::STATE9_REAL_DATA: // real_data
-		parserbuf[*parserbuf_index + data_index] = data;
+		parserbuf[*parserbuf_index] = data;
 
 		data_index++;
+		(*parserbuf_index)++;
 		if (data_index == data_len)
 		{
-			(*parserbuf_index)++;
 			*state = RDS02UF_PARSE_STATE::STATE10_CRC;
 		}
 		break;
 	case RDS02UF_PARSE_STATE::STATE10_CRC: // crc
 		crc_data = crc8(&parserbuf[2], 6 + data_len);
-		parserbuf[*parserbuf_index + data_index] = data;
+		parserbuf[*parserbuf_index] = data;
 		if (crc_data == data || data == 0xff)
 		{
 			(*parserbuf_index)++;
 			*state = RDS02UF_PARSE_STATE::STATE11_END_1;
-		}
+		}else{
-		else
+			*parserbuf_index = 0;
-		{
+			*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
-			ParseDataFailed(parserbuf, sizeof(parserbuf));
 		}
 		break;
 	case RDS02UF_PARSE_STATE::STATE11_END_1: //
 		if (data == RDS02_END)
 		{
-			parserbuf[*parserbuf_index + data_index] = data;
+			parserbuf[*parserbuf_index] = data;
 			(*parserbuf_index)++;
 			*state = RDS02UF_PARSE_STATE::STATE12_END_2;
-		}
+		}else{
-		else
+			*parserbuf_index = 0;
-		{
+			*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
-			ParseDataFailed(parserbuf, sizeof(parserbuf));
 		}
 
 		break;
@@ -158,28 +186,29 @@ int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PA
 				ret = true;
 			}
 			}
-			ParseDataFailed(parserbuf, sizeof(parserbuf));
 		}
+
+		*parserbuf_index = 0;
+		*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
+
 		break;
 	}
 
 #ifdef RDS02UF_DEBUG
-	printf("state: RDS02UF_PARSE_STATE%s\n", parser_state[*state]);
+	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;
 }
 
-void ParseDataFailed(char* data_in, uint8_t datalengh)
-{
-    memset(data_in, 0, datalengh);
-    data_len = 0;
-    data_index = 0;
-    crc_data = 0;
-    fc_code = 0;
-    err_code = 0;
-}
-
 uint8_t crc8(char* pbuf, int32_t len)
 {
      char* data = pbuf;

src/drivers/distance_sensor/rds02uf/rds02uf_parser.h

@@ -36,8 +36,9 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Chuong Nguyen <chnguye7@asu.edu>
  * @author Ayush Gaud <ayush.gaud@gmail.com>
+ * @author Jim Brown <zbr3550@163.com>
  *
- * Declarations of parser for the Benewake TFmini laser rangefinder series
+ * Declarations of parser for the Benewake Rds02UF rangefinder series
  */
 
 #pragma once
@@ -53,18 +54,22 @@
 
 
 #define RDS02_TARGET_INFO  0x0C
-// Data Format for Benewake TFmini
+// Data Format for Benewake Rds02UF
 // ===============================
-// 9 bytes total per message:
+// 21 bytes total per message:
-// 1) 0x59
+// 1) 0x55
-// 2) 0x59
+// 2) 0x55
-// 3) Dist_L (low 8bit)
+// 3) address
-// 4) Dist_H (high 8bit)
+// 4) error_code
-// 5) Strength_L (low 8bit)
+// 5) FC_CODE_L (low 8bit)
-// 6) Strength_H (high 8bit)
+// 6) FC_CODE_H (high 8bit)
-// 7) Reserved bytes
+// 7) LENGTH_L (low 8bit)
-// 8) Original signal quality degree
+// 8) LENGTH_H (high 8bit)
-// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though
+// 9) REAL_DATA (10Byte)
+// 10) CRC8
+// 11) END_1 0xAA
+// 12) END_2 0xAA
+
 
 
 enum RDS02UF_PARSE_STATE {
@@ -84,7 +89,6 @@ enum RDS02UF_PARSE_STATE {
 
 int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist);
 uint8_t crc8(char* pbuf, int32_t len);
-void ParseDataFailed(char* data_in, uint8_t datalengh);
 
 const uint8_t crc8_table[256] = {
     0x93,0x98,0xE4,0x46,0xEB,0xBA,0x04,0x4C,