From 0c601dca67d976aab61760755bb923f9b944c301 Mon Sep 17 00:00:00 2001 From: zbr3550 Date: Thu, 16 Jun 2022 08:13:24 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=B2=E5=8F=A3=E9=A9=B1=E5=8A=A8=E7=BC=96?= =?UTF-8?q?=E8=AF=91=E9=80=9A=E8=BF=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../distance_sensor/rds02uf/rds02uf.cpp | 4 +- .../distance_sensor/rds02uf/rds02uf.hpp | 2 +- .../rds02uf/rds02uf_parser.cpp | 185 ++++++++++-------- .../distance_sensor/rds02uf/rds02uf_parser.h | 72 +++++-- 4 files changed, 172 insertions(+), 91 deletions(-) diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf.cpp b/src/drivers/distance_sensor/rds02uf/rds02uf.cpp index 8ac62ffc9c..835cf6bc05 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf.cpp +++ b/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; } } diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf.hpp b/src/drivers/distance_sensor/rds02uf/rds02uf.hpp index c5ce196b27..71c97e5346 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf.hpp +++ b/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] {}; diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp index 3bdf9d96db..c5f4a2e917 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp +++ b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp @@ -42,112 +42,124 @@ #include #include +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; } - - break; - - case RDS02UF_PARSE_STATE::STATE1_SYNC_1: - if (c == 'Y') { - *state = RDS02UF_PARSE_STATE::STATE1_SYNC_2; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - - } else { - *state = RDS02UF_PARSE_STATE::STATE0_UNSYNC; - *parserbuf_index = 0; + else + { + ParseDataFailed(parserbuf, sizeof(parserbuf)); } - break; - - case RDS02UF_PARSE_STATE::STATE1_SYNC_2: - *state = RDS02UF_PARSE_STATE::STATE2_GOT_DIST_L; - parserbuf[*parserbuf_index] = c; + case RDS02UF_PARSE_STATE::STATE3_ADDRESS: // address + parserbuf[*parserbuf_index] = data; (*parserbuf_index)++; - + *state = RDS02UF_PARSE_STATE::STATE4_ERROR_CODE; 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::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_H: - *state = RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_L; - 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::STATE3_GOT_STRENGTH_L: - *state = RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_H; - 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_H: - *state = RDS02UF_PARSE_STATE::STATE4_GOT_RESERVED; - 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::STATE4_GOT_RESERVED: - *state = RDS02UF_PARSE_STATE::STATE5_GOT_QUALITY; - 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::STATE5_GOT_QUALITY: - // Find the checksum - unsigned char cksm = 0; - - for (int i = 0; i < 8; i++) { - cksm += parserbuf[i]; + data_index++; + if (data_index == data_len) + { + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE10_CRC; } - - 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; + break; + 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; + } + 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; +} diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h index 60b450647e..1035d64a7a 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h +++ b/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, + };