串口驱动编译通过

src/drivers/distance_sensor/rds02uf/rds02uf.cpp

@@ -230,7 +230,7 @@ void
 Rds02uf::start()
 {
 	// schedule a cycle to start things (the sensor sends at 100Hz, but we run a bit faster to avoid missing data)
-	ScheduleOnInterval(7_ms);
+	ScheduleOnInterval(50_ms);
 }
 
 void
@@ -252,7 +252,7 @@ Rds02uf::Run()
 	if (collect() == -EAGAIN) {
 		// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps
 		ScheduleClear();
-		ScheduleOnInterval(7_ms, 87 * 9);
+		ScheduleOnInterval(50_ms, 87 * 12);
 		return;
 	}
 }

src/drivers/distance_sensor/rds02uf/rds02uf.hpp

@@ -77,7 +77,7 @@ private:
 
 	PX4Rangefinder	_px4_rangefinder;
 
-	RDS02UF_PARSE_STATE _parse_state {RDS02UF_PARSE_STATE::STATE0_UNSYNC};
+	RDS02UF_PARSE_STATE _parse_state {RDS02UF_PARSE_STATE::STATE1_SYNC_1};
 
 	char _linebuf[10] {};
 	char _port[20] {};

src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp

@@ -42,112 +42,124 @@
 #include <string.h>
 #include <stdlib.h>
 
+uint16_t data_len;
+uint16_t data_index;
+uint8_t crc_data;
+uint16_t fc_code;
+uint8_t err_code;
+
 int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist)
 {
 	int ret = -1;
+	char data = c;
 	//char *end;
 
 	switch (*state) {
-	case RDS02UF_PARSE_STATE::STATE6_GOT_CHECKSUM:
-		if (c == 'Y') {
-			*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
-			parserbuf[*parserbuf_index] = c;
+	case RDS02UF_PARSE_STATE::STATE1_SYNC_1:
+		if (data == RDS02_HEAD1)
+		{
+			parserbuf[*parserbuf_index] = data;
 			(*parserbuf_index)++;
-
-		} else {
-			*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC;
+			*state = RDS02UF_PARSE_STATE::STATE2_SYNC_2;
 		}
-
 		break;
-
-	case RDS02UF_PARSE_STATE::STATE0_UNSYNC:
-		if (c == 'Y') {
-			*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
-			parserbuf[*parserbuf_index] = c;
+	case RDS02UF_PARSE_STATE::STATE2_SYNC_2:
+		if (data == RDS02_HEAD2)
+		{
+			parserbuf[*parserbuf_index] = data;
 			(*parserbuf_index)++;
+			*state = RDS02UF_PARSE_STATE::STATE3_ADDRESS;
+		}
+		else
+		{
+			ParseDataFailed(parserbuf, sizeof(parserbuf));
 		}
-
 		break;
-
-	case RDS02UF_PARSE_STATE::STATE1_SYNC_1:
-		if (c == 'Y') {
-			*state = RDS02UF_PARSE_STATE::STATE1_SYNC_2;
-			parserbuf[*parserbuf_index] = c;
+	case RDS02UF_PARSE_STATE::STATE3_ADDRESS: // address
+		parserbuf[*parserbuf_index] = data;
 		(*parserbuf_index)++;
-
-		} else {
-			*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC;
-			*parserbuf_index = 0;
-		}
-
+		*state = RDS02UF_PARSE_STATE::STATE4_ERROR_CODE;
 		break;
-
-	case RDS02UF_PARSE_STATE::STATE1_SYNC_2:
-		*state = RDS02UF_PARSE_STATE::STATE2_GOT_DIST_L;
-		parserbuf[*parserbuf_index] = c;
+	case RDS02UF_PARSE_STATE::STATE4_ERROR_CODE: // error_code
+		parserbuf[*parserbuf_index] = data;
+		err_code = data;
 		(*parserbuf_index)++;
-
+		*state = RDS02UF_PARSE_STATE::STATE5_FC_CODE_L;
 		break;
-
-	case RDS02UF_PARSE_STATE::STATE2_GOT_DIST_L:
-		*state = RDS02UF_PARSE_STATE::STATE2_GOT_DIST_H;
-		parserbuf[*parserbuf_index] = c;
+	case RDS02UF_PARSE_STATE::STATE5_FC_CODE_L: // fc_code low
+		parserbuf[*parserbuf_index] = data;
+		fc_code = data;
 		(*parserbuf_index)++;
-
+		*state = RDS02UF_PARSE_STATE::STATE6_FC_CODE_H;
 		break;
-
-	case RDS02UF_PARSE_STATE::STATE2_GOT_DIST_H:
-		*state = RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_L;
-		parserbuf[*parserbuf_index] = c;
+	case RDS02UF_PARSE_STATE::STATE6_FC_CODE_H: // fc_code high
+		parserbuf[*parserbuf_index] = data;
+		fc_code = fc_code | (data << 8);
 		(*parserbuf_index)++;
-
+		*state = RDS02UF_PARSE_STATE::STATE7_LENGTH_L;
 		break;
-
-	case RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_L:
-		*state = RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_H;
-		parserbuf[*parserbuf_index] = c;
+	case RDS02UF_PARSE_STATE::STATE7_LENGTH_L: // lengh_low
+		data_len = data;
+		parserbuf[*parserbuf_index] = data;
 		(*parserbuf_index)++;
-
+		*state = RDS02UF_PARSE_STATE::STATE8_LENGTH_H;
 		break;
-
-	case RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_H:
-		*state = RDS02UF_PARSE_STATE::STATE4_GOT_RESERVED;
-		parserbuf[*parserbuf_index] = c;
+	case RDS02UF_PARSE_STATE::STATE8_LENGTH_H: // lengh_high
+		data_len = data << 8 | data_len;
+		parserbuf[*parserbuf_index] = data;
 		(*parserbuf_index)++;
-
+		*state = RDS02UF_PARSE_STATE::STATE9_REAL_DATA;
 		break;
+	case RDS02UF_PARSE_STATE::STATE9_REAL_DATA: // real_data
+		parserbuf[*parserbuf_index + data_index] = data;
 
-	case RDS02UF_PARSE_STATE::STATE4_GOT_RESERVED:
-		*state = RDS02UF_PARSE_STATE::STATE5_GOT_QUALITY;
-		parserbuf[*parserbuf_index] = c;
+		data_index++;
+		if (data_index == data_len)
+		{
 			(*parserbuf_index)++;
-
+		*state = RDS02UF_PARSE_STATE::STATE10_CRC;
+		}
 		break;
-
-	case RDS02UF_PARSE_STATE::STATE5_GOT_QUALITY:
-		// Find the checksum
-		unsigned char cksm = 0;
-
-		for (int i = 0; i < 8; i++) {
-			cksm += parserbuf[i];
+	case RDS02UF_PARSE_STATE::STATE10_CRC: // crc
+		crc_data = crc8(&parserbuf[2], 6 + data_len);
+		parserbuf[*parserbuf_index + data_index] = data;
+		if (crc_data == data || data == 0xff)
+		{
+			(*parserbuf_index)++;
+			*state = RDS02UF_PARSE_STATE::STATE11_END_1;
 		}
-
-		if (c == cksm) {
-			parserbuf[*parserbuf_index] = '\0';
-			unsigned int t1 = parserbuf[2];
-			unsigned int t2 = parserbuf[3];
-			t2 <<= 8;
-			t2 += t1;
-			*dist = ((float)t2) / 100;
-			*state = RDS02UF_PARSE_STATE::STATE6_GOT_CHECKSUM;
-			*parserbuf_index = 0;
-			ret = 0;
-
-		} else {
-			*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC;
-			*parserbuf_index = 0;
+		else
+		{
+			ParseDataFailed(parserbuf, sizeof(parserbuf));
+		}
+		break;
+	case RDS02UF_PARSE_STATE::STATE11_END_1: //
+		if (data == RDS02_END)
+		{
+			parserbuf[*parserbuf_index + data_index] = data;
+			(*parserbuf_index)++;
+			*state = RDS02UF_PARSE_STATE::STATE12_END_2;
+		}
+		else
+		{
+			ParseDataFailed(parserbuf, sizeof(parserbuf));
 		}
 
+		break;
+	case RDS02UF_PARSE_STATE::STATE12_END_2: //
+		if (data == RDS02_END)
+		{
+			if (fc_code == 0x03ff && err_code == 0)
+			{
+			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;
+			}
+			}
+			ParseDataFailed(parserbuf, sizeof(parserbuf));
+		}
 		break;
 	}
 
@@ -157,3 +169,22 @@ int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PA
 
 	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;
+     uint8_t crc = 0;
+     while ( len-- )
+     crc = crc8_table[crc^*(data++)];
+     return crc;
+}

src/drivers/distance_sensor/rds02uf/rds02uf_parser.h

@@ -42,6 +42,17 @@
 
 #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 TFmini
 // ===============================
 // 9 bytes total per message:
@@ -56,17 +67,56 @@
 // 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though
 
 
-enum class RDS02UF_PARSE_STATE {
-	STATE0_UNSYNC = 0,
-	STATE1_SYNC_1,
-	STATE1_SYNC_2,
-	STATE2_GOT_DIST_L,
-	STATE2_GOT_DIST_H,
-	STATE3_GOT_STRENGTH_L,
-	STATE3_GOT_STRENGTH_H,
-	STATE4_GOT_RESERVED,
-	STATE5_GOT_QUALITY,
-	STATE6_GOT_CHECKSUM
+enum RDS02UF_PARSE_STATE {
+	STATE1_SYNC_1 = 0,
+	STATE2_SYNC_2,
+	STATE3_ADDRESS,
+	STATE4_ERROR_CODE,
+	STATE5_FC_CODE_L,
+	STATE6_FC_CODE_H,
+	STATE7_LENGTH_L,
+	STATE8_LENGTH_H,
+	STATE9_REAL_DATA,
+	STATE10_CRC,
+	STATE11_END_1,
+	STATE12_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);
+void ParseDataFailed(char* data_in, uint8_t datalengh);
+
+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,
+    };