From cbb84c127c6f3c58e22f94020ce85da57bb337a6 Mon Sep 17 00:00:00 2001 From: Zebulon Date: Tue, 26 Jul 2022 08:01:49 +0800 Subject: [PATCH 1/4] AP_RangeFinder:add support for RDS02UF radar driver on serial parameter RNGFND1_TYPE is 36 --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 6 + libraries/AP_RangeFinder/AP_RangeFinder.h | 1 + .../AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp | 222 ++++++++++++++++++ .../AP_RangeFinder/AP_RangeFinder_RDS02UF.h | 135 +++++++++++ 4 files changed, 364 insertions(+) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 2d731ff335e42..b674406801620 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -51,6 +51,7 @@ #include "AP_RangeFinder_MSP.h" #include "AP_RangeFinder_USD1_CAN.h" #include "AP_RangeFinder_Benewake_CAN.h" +#include "AP_RangeFinder_RDS02UF.h" #include #include @@ -608,6 +609,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance); break; +#endif +#if AP_RANGEFINDER_RDS02UF_ENABLED + case Type::RDS02UF: + serial_create_fn = AP_RangeFinder_RDS02UF::create; + break; #endif case Type::NONE: break; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index dfb01b5674953..ac8bbd9133b54 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -103,6 +103,7 @@ class RangeFinder USD1_CAN = 33, Benewake_CAN = 34, TeraRanger_Serial = 35, + RDS02UF = 36, SIM = 100, }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp new file mode 100644 index 0000000000000..e96c0d90e268e --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp @@ -0,0 +1,222 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/** + * RDS02UF Note: + * Sensor range scope 1.5m~20.0m + * Azimuth Coverage ±17°,Elevation Coverage ±3° + * Frame Rate 20Hz + */ +#include "AP_RangeFinder_RDS02UF.h" + +#include +#include + +#define RDS02_HEAD1 0x55 +#define RDS02_HEAD2 0x55 +#define RDS02_END 0xAA + +#define RDS02UF_HEAD_LEN 2 +#define RDS02UF_PRE_DATA_LEN 6 +#define RDS02UF_DATA_LEN 10 +#define RDS02_DATA_Y_INDEX_L 13 +#define RDS02_DATA_Y_INDEX_H 14 +#define RDS02_DATA_FC_L 8 +#define RDS02_DATA_FC_H 9 +#define RDS02_TARGET_INFO 0x70C +#define RDS02UF_DIST_MAX_CM 2000 +#define RDS02UF_DIST_MIN_CM 150 +#define AP_RANGEFINDER_RDS02UF_TIMEOUT_MS 200 + + +extern const AP_HAL::HAL& hal; + +AP_RangeFinder_RDS02UF::AP_RangeFinder_RDS02UF( + RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params): + AP_RangeFinder_Backend_Serial(_state, _params) +{ + +} + +// return last value measured by sensor +bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m) +{ + if (uart == nullptr) { + return false; + } + + // read any available lines from the lidar + float sum = 0.0f; + uint16_t count = 0; + int16_t nbytes = uart->available(); + while (nbytes-- > 0) { + uint8_t c = uart->read(); + if (decode(c)) { + sum += _distance_m; + count++; + } + } + + // return false on failure + if (count == 0) { + return false; + } + + if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_RDS02UF_TIMEOUT_MS) { + set_status(RangeFinder::Status::NoData); + reading_m = 0.0f; + } else { + // return average of all measurements + reading_m = sum / count; + update_status(); + } + + // return average of all measurements + reading_m = sum / count; + return true; +} + + +// add a single character to the buffer and attempt to decode +// returns true if a distance was successfully decoded +bool AP_RangeFinder_RDS02UF::decode(uint8_t c) +{ + uint8_t data = c; + bool ret = false; + switch (decode_state) + { + case STATE0_SYNC_1: + if (data == RDS02_HEAD1) { + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE1_SYNC_2; + } + break; + case STATE1_SYNC_2: + if (data == RDS02_HEAD2) { + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE2_ADDRESS; + } else { + parserbuf_index = 0; + decode_state = STATE0_SYNC_1; + } + break; + case STATE2_ADDRESS: // address + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE3_ERROR_CODE; + break; + case STATE3_ERROR_CODE: // error_code + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE4_FC_CODE_L; + break; + case STATE4_FC_CODE_L: // fc_code low + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE5_FC_CODE_H; + break; + case STATE5_FC_CODE_H: // fc_code high + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE6_LENGTH_L; + break; + case STATE6_LENGTH_L: // lengh_low + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE7_LENGTH_H; + break; + case STATE7_LENGTH_H: // lengh_high + { + uint8_t read_len = data << 8 | rev_buffer[parserbuf_index-1]; + if ( read_len == RDS02UF_DATA_LEN) // rds02uf data length is 10 + { + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE8_REAL_DATA; + }else{ + parserbuf_index = 0; + decode_state = STATE0_SYNC_1; + } + break; + } + case STATE8_REAL_DATA: // real_data + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + if ( parserbuf_index == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN) ) { + decode_state = STATE9_CRC; + } + break; + case STATE9_CRC: // crc + { + uint8_t crc_data = 0; + crc_data = crc8(&rev_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN); + rev_buffer[parserbuf_index] = data; + if (crc_data == data || data == 0xff) { + parserbuf_index++; + decode_state = STATE10_END_1; + } else { + parserbuf_index = 0; + decode_state = STATE0_SYNC_1; + } + break; + } + case STATE10_END_1: // + if (data == RDS02_END) { + rev_buffer[parserbuf_index] = data; + parserbuf_index++; + decode_state = STATE11_END_2; + } else { + parserbuf_index = 0; + decode_state = STATE0_SYNC_1; + } + break; + case STATE11_END_2: // + { + uint16_t fc_code = (rev_buffer[STATE5_FC_CODE_H] << 8 | rev_buffer[STATE4_FC_CODE_L]); + uint8_t err_code = rev_buffer[STATE3_ERROR_CODE]; + if (data == RDS02_END) + { + if (fc_code == 0x03ff && err_code == 0) {// get targer information + uint16_t data_fc = (rev_buffer[RDS02_DATA_FC_H] << 8 | rev_buffer[RDS02_DATA_FC_L]); + if((data_fc & 0xf0f) == RDS02_TARGET_INFO){ // data_fc = 0x70C + ID * 0x10, ID: 0~0xF + float distance = (rev_buffer[RDS02_DATA_Y_INDEX_H] * 256 + rev_buffer[RDS02_DATA_Y_INDEX_L]) / 100.0f; + _distance_m = distance; + _distance_m = MIN(MAX(RDS02UF_DIST_MIN_CM, uint16_t(_distance_m*100)), RDS02UF_DIST_MAX_CM) * 0.01f; + ret = true; + state.last_reading_ms = AP_HAL::millis(); + } + + } + } + parserbuf_index = 0; + decode_state = STATE0_SYNC_1; + break; + } + } + + return ret; +} + +uint8_t AP_RangeFinder_RDS02UF::crc8(uint8_t* pbuf, int32_t len) +{ + uint8_t* data = pbuf; + uint8_t crc = 0; + while ( len-- ) + crc = crc8_table[crc^*(data++)]; + return crc; +} \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h new file mode 100644 index 0000000000000..1d8d03e465930 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h @@ -0,0 +1,135 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "AP_RangeFinder.h" +#include "AP_RangeFinder_Backend_Serial.h" + +#ifndef AP_RANGEFINDER_RDS02UF_ENABLED +#define AP_RANGEFINDER_RDS02UF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#endif + +#if AP_RANGEFINDER_RDS02UF_ENABLED + +#define RDS02_BUFFER_SIZE 50 + +class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial +{ + +public: + + static AP_RangeFinder_Backend_Serial *create( + RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params) { + return new AP_RangeFinder_RDS02UF(_state, _params); + } + +protected: + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_ULTRASOUND; + } + + +private: + AP_RangeFinder_RDS02UF(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + + uint8_t decode_state; + uint8_t parserbuf_index; + uint8_t rev_buffer[RDS02_BUFFER_SIZE]; + float _distance_m; + + void ParseRds02(uint8_t *data_in, uint8_t datalengh); + + // 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 for handled messages + 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 + }; + + // get a distance reading + bool get_reading(float &reading_m) override; + + // get temperature reading in C. returns true on success and populates temp argument + // bool get_temp(float &temp) const override; + + uint16_t read_timeout_ms() const override { return 3000; } + + uint8_t crc8(uint8_t *pbuf, int32_t len); + + bool decode(uint8_t c); + + 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, + }; +}; +#endif // AP_RANGEFINDER_NMEA_ENABLED From db26601a53d951bc3709aabfc37b904e9f2293ed Mon Sep 17 00:00:00 2001 From: zebulon-86 Date: Tue, 6 Sep 2022 13:50:44 +0800 Subject: [PATCH 2/4] Apply suggestions from code review Co-authored-by: Peter Barker --- .../AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp | 208 +++++++++--------- .../AP_RangeFinder/AP_RangeFinder_RDS02UF.h | 18 +- 2 files changed, 117 insertions(+), 109 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp index e96c0d90e268e..0f42d1e5ec014 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp @@ -24,33 +24,30 @@ #include #include -#define RDS02_HEAD1 0x55 -#define RDS02_HEAD2 0x55 -#define RDS02_END 0xAA - -#define RDS02UF_HEAD_LEN 2 -#define RDS02UF_PRE_DATA_LEN 6 -#define RDS02UF_DATA_LEN 10 -#define RDS02_DATA_Y_INDEX_L 13 -#define RDS02_DATA_Y_INDEX_H 14 -#define RDS02_DATA_FC_L 8 -#define RDS02_DATA_FC_H 9 -#define RDS02_TARGET_INFO 0x70C -#define RDS02UF_DIST_MAX_CM 2000 -#define RDS02UF_DIST_MIN_CM 150 -#define AP_RANGEFINDER_RDS02UF_TIMEOUT_MS 200 +#define RDS02_HEAD1 0x55 +#define RDS02_HEAD2 0x55 +#define RDS02_END 0xAA + +#define RDS02UF_HEAD_LEN 2 +#define RDS02UF_ERROR_CODE_INDEX 3 +#define RDS02UF_PRODUCTS_FC_INDEX_L 4 +#define RDS02UF_PRODUCTS_FC_INDEX_H 5 +#define RDS02UF_PRE_DATA_LEN 6 +#define RDS02UF_DATA_LEN 10 +#define RDS02_DATA_Y_INDEX_L 13 +#define RDS02_DATA_Y_INDEX_H 14 +#define RDS02_TARGET_FC_INDEX_L 8 +#define RDS02_TARGET_FC_INDEX_H 9 +#define RDS02_TARGET_INFO_FC 0x070C +#define RDS02UF_DIST_MAX_M 20.0 +#define RDS02UF_DIST_MIN_M 1.5 +#define RDS02UF_IGNORE_ID_BYTE 0x0F0F +#define RDS02UF_UAV_PRODUCTS_ID 0x03FF +#define RDS02UF_TIMEOUT_MS 200 extern const AP_HAL::HAL& hal; -AP_RangeFinder_RDS02UF::AP_RangeFinder_RDS02UF( - RangeFinder::RangeFinder_State &_state, - AP_RangeFinder_Params &_params): - AP_RangeFinder_Backend_Serial(_state, _params) -{ - -} - // return last value measured by sensor bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m) { @@ -58,24 +55,22 @@ bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m) return false; } - // read any available lines from the lidar + // read any available data from the lidar float sum = 0.0f; uint16_t count = 0; - int16_t nbytes = uart->available(); + uint32_t nbytes = uart->available(); while (nbytes-- > 0) { - uint8_t c = uart->read(); + int16_t c = uart->read(); + if (c < 0) { + continue; + } if (decode(c)) { sum += _distance_m; count++; } } - // return false on failure - if (count == 0) { - return false; - } - - if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_RDS02UF_TIMEOUT_MS) { + if (AP_HAL::millis() - state.last_reading_ms > RDS02UF_TIMEOUT_MS) { set_status(RangeFinder::Status::NoData); reading_m = 0.0f; } else { @@ -84,6 +79,11 @@ bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m) update_status(); } + // return false if no new readings + if (count == 0) { + return false; + } + // return average of all measurements reading_m = sum / count; return true; @@ -98,113 +98,121 @@ bool AP_RangeFinder_RDS02UF::decode(uint8_t c) bool ret = false; switch (decode_state) { - case STATE0_SYNC_1: + case RDS02UF_PARSE_STATE::STATE0_SYNC_1: if (data == RDS02_HEAD1) { - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE1_SYNC_2; + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE1_SYNC_2; } break; - case STATE1_SYNC_2: + case RDS02UF_PARSE_STATE::STATE1_SYNC_2: if (data == RDS02_HEAD2) { - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE2_ADDRESS; + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE2_ADDRESS; } else { - parserbuf_index = 0; - decode_state = STATE0_SYNC_1; + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; } break; - case STATE2_ADDRESS: // address - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE3_ERROR_CODE; + case RDS02UF_PARSE_STATE::STATE2_ADDRESS: // address + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE3_ERROR_CODE; break; - case STATE3_ERROR_CODE: // error_code - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE4_FC_CODE_L; + case RDS02UF_PARSE_STATE::STATE3_ERROR_CODE: // error_code + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE4_FC_CODE_L; break; - case STATE4_FC_CODE_L: // fc_code low - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE5_FC_CODE_H; + case RDS02UF_PARSE_STATE::STATE4_FC_CODE_L: // fc_code low + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE5_FC_CODE_H; break; - case STATE5_FC_CODE_H: // fc_code high - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE6_LENGTH_L; + case RDS02UF_PARSE_STATE::STATE5_FC_CODE_H: // fc_code high + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE6_LENGTH_L; break; - case STATE6_LENGTH_L: // lengh_low - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE7_LENGTH_H; + case RDS02UF_PARSE_STATE::STATE6_LENGTH_L: // lengh_low + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE7_LENGTH_H; break; - case STATE7_LENGTH_H: // lengh_high + case RDS02UF_PARSE_STATE::STATE7_LENGTH_H: // lengh_high { - uint8_t read_len = data << 8 | rev_buffer[parserbuf_index-1]; + uint8_t read_len = data << 8 | parser_buffer[parser_index-1]; if ( read_len == RDS02UF_DATA_LEN) // rds02uf data length is 10 { - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE8_REAL_DATA; + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE8_REAL_DATA; }else{ - parserbuf_index = 0; - decode_state = STATE0_SYNC_1; + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; } break; } - case STATE8_REAL_DATA: // real_data - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - if ( parserbuf_index == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN) ) { - decode_state = STATE9_CRC; + case RDS02UF_PARSE_STATE::STATE8_REAL_DATA: // real_data + parser_buffer[parser_index] = data; + parser_index++; + if ( parser_index == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN) ) { + decode_state = RDS02UF_PARSE_STATE::STATE9_CRC; } break; - case STATE9_CRC: // crc + case RDS02UF_PARSE_STATE::STATE9_CRC: // crc { - uint8_t crc_data = 0; - crc_data = crc8(&rev_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN); - rev_buffer[parserbuf_index] = data; + uint8_t crc_data; + crc_data = crc8(&parser_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN); + parser_buffer[parser_index] = data; if (crc_data == data || data == 0xff) { - parserbuf_index++; - decode_state = STATE10_END_1; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE10_END_1; } else { - parserbuf_index = 0; - decode_state = STATE0_SYNC_1; + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; } break; } - case STATE10_END_1: // + case RDS02UF_PARSE_STATE::STATE10_END_1: // if (data == RDS02_END) { - rev_buffer[parserbuf_index] = data; - parserbuf_index++; - decode_state = STATE11_END_2; + parser_buffer[parser_index] = data; + parser_index++; + decode_state = RDS02UF_PARSE_STATE::STATE11_END_2; } else { - parserbuf_index = 0; - decode_state = STATE0_SYNC_1; + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; } break; - case STATE11_END_2: // + case RDS02UF_PARSE_STATE::STATE11_END_2: // { - uint16_t fc_code = (rev_buffer[STATE5_FC_CODE_H] << 8 | rev_buffer[STATE4_FC_CODE_L]); - uint8_t err_code = rev_buffer[STATE3_ERROR_CODE]; + uint16_t fc_code = (parser_buffer[RDS02UF_PRODUCTS_FC_INDEX_H] << 8 | parser_buffer[RDS02UF_PRODUCTS_FC_INDEX_L]); + uint8_t err_code = parser_buffer[RDS02UF_ERROR_CODE_INDEX]; + if (data == RDS02_END) { - if (fc_code == 0x03ff && err_code == 0) {// get targer information - uint16_t data_fc = (rev_buffer[RDS02_DATA_FC_H] << 8 | rev_buffer[RDS02_DATA_FC_L]); - if((data_fc & 0xf0f) == RDS02_TARGET_INFO){ // data_fc = 0x70C + ID * 0x10, ID: 0~0xF - float distance = (rev_buffer[RDS02_DATA_Y_INDEX_H] * 256 + rev_buffer[RDS02_DATA_Y_INDEX_L]) / 100.0f; - _distance_m = distance; - _distance_m = MIN(MAX(RDS02UF_DIST_MIN_CM, uint16_t(_distance_m*100)), RDS02UF_DIST_MAX_CM) * 0.01f; + if (fc_code == RDS02UF_UAV_PRODUCTS_ID && err_code == 0) {// get targer information + uint16_t read_info_fc = (parser_buffer[RDS02_TARGET_FC_INDEX_H] << 8 | parser_buffer[RDS02_TARGET_FC_INDEX_L]); + if((read_info_fc & RDS02UF_IGNORE_ID_BYTE) == RDS02_TARGET_INFO_FC){ // read_info_fc = 0x70C + ID * 0x10, ID: 0~0xF + _distance_m = (parser_buffer[RDS02_DATA_Y_INDEX_H] * 256 + parser_buffer[RDS02_DATA_Y_INDEX_L]) / 100.0f; + if (_distance_m > RDS02UF_DIST_MAX_M) + { + _distance_m = RDS02UF_DIST_MAX_M; + set_status(RangeFinder::Status::OutOfRangeHigh); + }else if (_distance_m < RDS02UF_DIST_MIN_M) + { + _distance_m = RDS02UF_DIST_MIN_M; + set_status(RangeFinder::Status::OutOfRangeLow); + } ret = true; state.last_reading_ms = AP_HAL::millis(); } } } - parserbuf_index = 0; - decode_state = STATE0_SYNC_1; + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; break; } } @@ -217,6 +225,6 @@ uint8_t AP_RangeFinder_RDS02UF::crc8(uint8_t* pbuf, int32_t len) uint8_t* data = pbuf; uint8_t crc = 0; while ( len-- ) - crc = crc8_table[crc^*(data++)]; + crc = crc8_table[crc^*(data++)]; return crc; } \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h index 1d8d03e465930..cb13b2f700432 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h @@ -45,14 +45,7 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial private: - AP_RangeFinder_RDS02UF(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); - - uint8_t decode_state; - uint8_t parserbuf_index; - uint8_t rev_buffer[RDS02_BUFFER_SIZE]; - float _distance_m; - - void ParseRds02(uint8_t *data_in, uint8_t datalengh); + using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; // Data Format for Benewake Rds02UF // =============================== @@ -70,7 +63,7 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial // 11) END_1 0xAA // 12) END_2 0xAA /// enum for handled messages - enum RDS02UF_PARSE_STATE { + enum class RDS02UF_PARSE_STATE { STATE0_SYNC_1 = 0, STATE1_SYNC_2, STATE2_ADDRESS, @@ -85,6 +78,13 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial STATE11_END_2 }; + RDS02UF_PARSE_STATE decode_state; + uint8_t parser_index; + uint8_t parser_buffer[RDS02_BUFFER_SIZE]; + float _distance_m; + + void ParseRds02(uint8_t *data_in, uint8_t datalengh); + // get a distance reading bool get_reading(float &reading_m) override; From 6a964e5f16c38694c8829be0aaf1ab5cab3f505a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 6 Sep 2022 14:35:02 +1000 Subject: [PATCH 3/4] AP_RangeFinder: add simulator for RDS02UF rangefinder --- libraries/AP_HAL/SIMState.cpp | 3 + libraries/AP_HAL/SIMState.h | 3 + libraries/AP_HAL_SITL/SITL_State.cpp | 9 ++ libraries/AP_HAL_SITL/SITL_State.h | 3 + libraries/SITL/SIM_RF_RDS02UF.cpp | 124 +++++++++++++++++++++++++++ libraries/SITL/SIM_RF_RDS02UF.h | 43 ++++++++++ 6 files changed, 185 insertions(+) create mode 100644 libraries/SITL/SIM_RF_RDS02UF.cpp create mode 100644 libraries/SITL/SIM_RF_RDS02UF.h diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp index 8dda977bde9df..add214a265036 100644 --- a/libraries/AP_HAL/SIMState.cpp +++ b/libraries/AP_HAL/SIMState.cpp @@ -134,6 +134,9 @@ void SIMState::fdm_input_local(void) if (leddarone != nullptr) { leddarone->update(sitl_model->rangefinder_range()); } + if (rds02uf != nullptr) { + rds02uf->update(sitl_model->rangefinder_range()); + } if (USD1_v0 != nullptr) { USD1_v0->update(sitl_model->rangefinder_range()); } diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index 5275b8b4d3bc5..a69132d47f95c 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -141,6 +142,8 @@ class AP_HAL::SIMState { SITL::RF_BLping *blping; // simulated LeddarOne rangefinder: SITL::RF_LeddarOne *leddarone; + // simulated RDS02UF rangefinder: + SITL::RF_RDS02UF *rds02uf; // simulated USD1 v0 rangefinder: SITL::RF_USD1_v0 *USD1_v0; // simulated USD1 v1 rangefinder: diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index e728ab2204aa8..f853e12ee2864 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -272,6 +272,12 @@ SITL::SerialDevice *SITL_State::create_serial_sim(const char *name, const char * } leddarone = new SITL::RF_LeddarOne(); return leddarone; + } else if (streq(name, "rds02uf")) { + if (rds02uf != nullptr) { + AP_HAL::panic("Only one rds02uf at a time"); + } + rds02uf = new SITL::RF_RDS02UF(); + return rds02uf; } else if (streq(name, "USD1_v0")) { if (USD1_v0 != nullptr) { AP_HAL::panic("Only one USD1_v0 at a time"); @@ -598,6 +604,9 @@ void SITL_State::_fdm_input_local(void) if (leddarone != nullptr) { leddarone->update(sitl_model->rangefinder_range()); } + if (rds02uf != nullptr) { + rds02uf->update(sitl_model->rangefinder_range()); + } if (USD1_v0 != nullptr) { USD1_v0->update(sitl_model->rangefinder_range()); } diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 872c33f460f8d..a1d4cefa3f7fc 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -226,6 +227,8 @@ class HALSITL::SITL_State { SITL::RF_BLping *blping; // simulated LeddarOne rangefinder: SITL::RF_LeddarOne *leddarone; + // simulated RDS02UF rangefinder: + SITL::RF_RDS02UF *rds02uf; // simulated USD1 v0 rangefinder: SITL::RF_USD1_v0 *USD1_v0; // simulated USD1 v1 rangefinder: diff --git a/libraries/SITL/SIM_RF_RDS02UF.cpp b/libraries/SITL/SIM_RF_RDS02UF.cpp new file mode 100644 index 0000000000000..d3c1e1fc33416 --- /dev/null +++ b/libraries/SITL/SIM_RF_RDS02UF.cpp @@ -0,0 +1,124 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for the RDS02UF rangefinder +*/ + +#include "SIM_RF_RDS02UF.h" + +#include + +using namespace SITL; + + + 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, + }; +uint8_t xcrc8(uint8_t* pbuf, int32_t len) +{ + uint8_t* data = pbuf; + uint8_t crc = 0; + while ( len-- ) + crc = crc8_table[crc^*(data++)]; + return crc; +} + + +uint32_t RF_RDS02UF::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) +{ + const uint16_t fc_code = 0x3ff; // NFI what this means + const uint16_t data_fc = 0x70c; // NFI what this means + + // bodgy fixed-length response to keep things simple: + union response_t { + struct { + uint8_t header1; + uint8_t header2; + uint8_t address; + uint8_t error_code; + uint8_t fc_code_l; + uint8_t fc_code_h; + uint8_t length_l; + uint8_t length_h; + uint8_t data0; // used + uint8_t data1; // used + uint8_t data2; + uint8_t data3; + uint8_t data4; + uint8_t data5; // distance-low + uint8_t data6; // distance-high + uint8_t data7; + uint8_t data8; + uint8_t data9; + uint8_t crc8; + uint8_t footer1; + uint8_t footer2; + }; + uint8_t buffer[21]; + } response{}; + + response.header1 = 0x55; + response.header2 = 0x55; + response.fc_code_l = fc_code & 0xff; + response.fc_code_h = fc_code >> 8; + response.length_l = 10; + response.length_h = 0; + response.data0 = data_fc & 0xff; + response.data1 = data_fc >> 8; + response.data5 = alt_cm & 0xff; + response.data6 = alt_cm >> 8; + response.crc8 = xcrc8(&response.buffer[2], 16); + response.footer1 = 0xAA; + response.footer2 = 0xAA; + + if (buflen < ARRAY_SIZE(response.buffer)) { + AP_HAL::panic("Too short a buffer"); + } + + memcpy(buffer, response.buffer, ARRAY_SIZE(response.buffer)); + + return ARRAY_SIZE(response.buffer); +} diff --git a/libraries/SITL/SIM_RF_RDS02UF.h b/libraries/SITL/SIM_RF_RDS02UF.h new file mode 100644 index 0000000000000..a96bb9b56564d --- /dev/null +++ b/libraries/SITL/SIM_RF_RDS02UF.h @@ -0,0 +1,43 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + Simulator for the RDS02UF rangefinder + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:rds02uf --speedup=1 + +param set SERIAL5_PROTOCOL 9 +param set RNGFND1_TYPE 36 +graph RANGEFINDER.distance +graph GLOBAL_POSITION_INT.relative_alt/1000-RANGEFINDER.distance +reboot + +arm throttle +rc 3 1600 +*/ + +#pragma once + +#include "SIM_SerialRangeFinder.h" + +namespace SITL { + +class RF_RDS02UF : public SerialRangeFinder { +public: + + uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; + +}; + +} From 5930afcac2dae0979a5f47c2c96395e0a7d3b558 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 6 Sep 2022 14:49:00 +1000 Subject: [PATCH 4/4] autotest: add simulator for RDS02UF rangefinder --- Tools/autotest/arducopter.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 3eb32649d0d38..5cbfebc0f92db 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7276,6 +7276,7 @@ def fly_rangefinder_drivers(self): ("benewake_tf03", 27), ("gyus42v2", 31), ("teraranger_serial", 35), + ("rds02uf", 36), ] while len(drivers): do_drivers = drivers[0:3]