diff --git a/src/drivers/distance_sensor/rds02uf/module.yaml b/src/drivers/distance_sensor/rds02uf/module.yaml index f682799c63..72f67f8205 100644 --- a/src/drivers/distance_sensor/rds02uf/module.yaml +++ b/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: | diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf.cpp b/src/drivers/distance_sensor/rds02uf/rds02uf.cpp index 835cf6bc05..1c3ca62528 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf.cpp +++ b/src/drivers/distance_sensor/rds02uf/rds02uf.cpp @@ -34,6 +34,7 @@ #include "rds02uf.hpp" #include +#include #include 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_fov(math::radians(1.15f)); + _px4_rangefinder.set_max_distance(20.0f); + // _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; } } diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf.hpp b/src/drivers/distance_sensor/rds02uf/rds02uf.hpp index 71c97e5346..a5711132e2 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf.hpp +++ b/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}; diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp index c5f4a2e917..0be0cde8fb 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp +++ b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp @@ -33,26 +33,49 @@ /** * @file modified from sf0x_parser.cpp - * @author + * @author Lorenz Meier + * @author Chuong Nguyen + * @author Ayush Gaud + * @author Jim Brown * - * Declarations of parser for the Benewake TFmini laser rangefinder series + * Declarations of parser for the Benewake Rds02UF rangefinder series */ #include "rds02uf_parser.h" #include #include -uint16_t data_len; -uint16_t data_index; -uint8_t crc_data; -uint16_t fc_code; -uint8_t err_code; +// #define RDS02UF_DEBUG +#ifdef RDS02UF_DEBUG +#include + +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 - { - ParseDataFailed(parserbuf, sizeof(parserbuf)); + }else{ + *parserbuf_index = 0; + *state = RDS02UF_PARSE_STATE::STATE1_SYNC_1; } 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; - parserbuf[*parserbuf_index] = data; - (*parserbuf_index)++; - *state = RDS02UF_PARSE_STATE::STATE9_REAL_DATA; + 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; + *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 - { - ParseDataFailed(parserbuf, sizeof(parserbuf)); + }else{ + *parserbuf_index = 0; + *state = RDS02UF_PARSE_STATE::STATE1_SYNC_1; } 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 - { - ParseDataFailed(parserbuf, sizeof(parserbuf)); + }else{ + *parserbuf_index = 0; + *state = RDS02UF_PARSE_STATE::STATE1_SYNC_1; } 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; diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h index 1035d64a7a..c155884cdc 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h +++ b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h @@ -36,8 +36,9 @@ * @author Lorenz Meier * @author Chuong Nguyen * @author Ayush Gaud + * @author Jim Brown * - * 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: -// 1) 0x59 -// 2) 0x59 -// 3) Dist_L (low 8bit) -// 4) Dist_H (high 8bit) -// 5) Strength_L (low 8bit) -// 6) Strength_H (high 8bit) -// 7) Reserved bytes -// 8) Original signal quality degree -// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though +// 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 { @@ -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,