From 8a6c274cb28941f7167fd4e879958b35e7ce0d27 Mon Sep 17 00:00:00 2001 From: zebulon-86 Date: Wed, 10 May 2023 00:06:56 +0800 Subject: [PATCH 1/7] AP_RangeFinder:add support for RDS02UF radar driver on serial parameter RNGFNDx_TYPE is 42 Apply suggestions from code review Co-authored-by: Peter Barker --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 6 + libraries/AP_RangeFinder/AP_RangeFinder.h | 1 + .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 2 +- .../AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp | 220 ++++++++++++++++++ .../AP_RangeFinder/AP_RangeFinder_RDS02UF.h | 131 +++++++++++ 5 files changed, 359 insertions(+), 1 deletion(-) 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 16e8f089c1fa1..0da1912c84e5e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -60,6 +60,7 @@ #include "AP_RangeFinder_TOFSenseF_I2C.h" #include "AP_RangeFinder_JRE_Serial.h" #include "AP_RangeFinder_Ainstein_LR_D1.h" +#include "AP_RangeFinder_RDS02UF.h" #include #include @@ -591,6 +592,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_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 79d38ccc5784e..64124327f69b7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -178,6 +178,7 @@ class RangeFinder Ainstein_LR_D1 = 42, #endif #if AP_RANGEFINDER_SIM_ENABLED + RDS02UF = 43, SIM = 100, #endif }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 13cd7e613746f..83816dd865376 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -10,7 +10,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @Param: TYPE // @DisplayName: Rangefinder type // @Description: Type of connected rangefinder - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,100:SITL + // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,100:SITL // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp new file mode 100644 index 0000000000000..bdfe34ebb2493 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp @@ -0,0 +1,220 @@ +/* + 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" + +#if AP_RANGEFINDER_RDS02UF_ENABLED + +#include +#include + +#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 +#define RDS02UF_IGNORE_CRC 0xFF +#define RDS02UF_NO_ERR 0x00 + +extern const AP_HAL::HAL& hal; + +// return last value measured by sensor +bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m) +{ + if (uart == nullptr) { + return false; + } + + // read any available data from the lidar + float sum = 0.0f; + uint16_t count = 0; + uint32_t nbytes = MAX(uart->available(), 1024U); + while (nbytes-- > 0) { + int16_t c = uart->read(); + if (c < 0) { + continue; + } + if (decode(c)) { + sum += _distance_m; + count++; + } + } + + if (AP_HAL::millis() - state.last_reading_ms > RDS02UF_TIMEOUT_MS) { + set_status(RangeFinder::Status::NoData); + reading_m = 0.0f; + return false; + } + + // return false if no new readings + if (count == 0) { + return false; + } + + // return average of all measurements + reading_m = sum / count; + update_status(); + 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 RDS02UF_PARSE_STATE::STATE0_SYNC_1: + if (data == RDS02_HEAD1) { + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE1_SYNC_2; + } + break; + case RDS02UF_PARSE_STATE::STATE1_SYNC_2: + if (data == RDS02_HEAD2) { + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE2_ADDRESS; + } else { + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + } + break; + case RDS02UF_PARSE_STATE::STATE2_ADDRESS: // address + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE3_ERROR_CODE; + break; + case RDS02UF_PARSE_STATE::STATE3_ERROR_CODE: // error_code + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE4_FC_CODE_L; + break; + case RDS02UF_PARSE_STATE::STATE4_FC_CODE_L: // fc_code low + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE5_FC_CODE_H; + break; + case RDS02UF_PARSE_STATE::STATE5_FC_CODE_H: // fc_code high + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE6_LENGTH_L; + break; + case RDS02UF_PARSE_STATE::STATE6_LENGTH_L: // lengh_low + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE7_LENGTH_H; + break; + case RDS02UF_PARSE_STATE::STATE7_LENGTH_H:{ // lengh_high + uint8_t read_len = data << 8 | parser_buffer[parser_index-1]; + // rds02uf data length is always 10 + if (read_len == RDS02UF_DATA_LEN) { + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE8_REAL_DATA; + } else { + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + } + break; + } + case RDS02UF_PARSE_STATE::STATE8_REAL_DATA: // real_data + parser_buffer[parser_index++] = data; + if (parser_index == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN) ) { + decode_state = RDS02UF_PARSE_STATE::STATE9_CRC; + } + break; + case RDS02UF_PARSE_STATE::STATE9_CRC:{ // crc + #if RDS02_USE_CRC + const uint8_t crc_data = crc8(&parser_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN); + if (data == crc_data || data == RDS02UF_IGNORE_CRC) { // 0xff indicates that the current frame does not need to be checked. + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE10_END_1; + } else { + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + } + #else + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE10_END_1; + #endif + break; + } + case RDS02UF_PARSE_STATE::STATE10_END_1: + if (data == RDS02_END) { + parser_buffer[parser_index++] = data; + decode_state = RDS02UF_PARSE_STATE::STATE11_END_2; + } else { + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + } + break; + case RDS02UF_PARSE_STATE::STATE11_END_2:{ + 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 == RDS02UF_UAV_PRODUCTS_ID && err_code == RDS02UF_NO_ERR) {// 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(); + } + } + } + parser_index = 0; + decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + break; + } + } + + return ret; +} + +uint8_t AP_RangeFinder_RDS02UF::crc8(uint8_t* pbuf, int32_t len) +{ +#if RDS02_USE_CRC + uint8_t crc = 0; + uint8_t* data = pbuf; + while (len--) + crc = crc8_table[crc^*(data++)]; + return crc; +#else + return 0; +#endif +} + +#endif // AP_RANGEFINDER_RDS02UF_ENABLED \ 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..d9b8b6fc4400a --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h @@ -0,0 +1,131 @@ +/* + 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 + +// In order to save MCU resources, CRC check is not used +#define RDS02_USE_CRC 0 +#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_RADAR; + } + + +private: + using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; + + // 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 class 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 + }; + + RDS02UF_PARSE_STATE decode_state; + uint8_t parser_index; + uint8_t parser_buffer[RDS02_BUFFER_SIZE]; + float _distance_m; + + // get a distance reading + bool get_reading(float &reading_m) override; + uint16_t read_timeout_ms() const override { return 3000; } + uint8_t crc8(uint8_t *pbuf, int32_t len); + bool decode(uint8_t c); + +#if RDS02_USE_CRC + 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 +}; +#endif // AP_RANGEFINDER_RDS02UF_ENABLED From dc64fe61ce930c8f3b62d4b819bb7e5eb977e43f Mon Sep 17 00:00:00 2001 From: zebulon-86 Date: Wed, 10 May 2023 00:05:56 +0800 Subject: [PATCH 2/7] Tools: add rangefinder backends to custom build server options --- Tools/scripts/build_options.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 5d9c1511b4c92..ecba978f823a0 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -248,6 +248,7 @@ def __init__(self, Feature('Rangefinder', 'RANGEFINDER_VL53L0X', 'AP_RANGEFINDER_VL53L0X_ENABLED', "Enable Rangefinder - VL53L0X", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_VL53L1X', 'AP_RANGEFINDER_VL53L1X_ENABLED', "Enable Rangefinder - VL53L1X", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_WASP', 'AP_RANGEFINDER_WASP_ENABLED', "Enable Rangefinder - Wasp", 0, "RANGEFINDER"), # NOQA: E501 + Feature('Rangefinder', 'RANGEFINDER_RDS02UF', 'AP_RANGEFINDER_RDS02UF_ENABLED', "Enable Rangefinder - RDS02UF", 0, "RANGEFINDER"), # NOQA: E501 Feature('Sensors', 'OPTICALFLOW', 'AP_OPTICALFLOW_ENABLED', 'Enable Optical Flow', 0, None), Feature('Sensors', 'OPTICALFLOW_CXOF', 'AP_OPTICALFLOW_CXOF_ENABLED', 'Enable Optical flow CXOF Sensor', 0, "OPTICALFLOW"), From 75eeee24665c702d29b98a117b2f7b6226744c5b Mon Sep 17 00:00:00 2001 From: zebulon-86 Date: Tue, 9 May 2023 23:58:31 +0800 Subject: [PATCH 3/7] Tools:add simulator for RDS02UF in autotest --- Tools/autotest/arducopter.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a961faded9a21..c94119bfe69a5 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -8396,6 +8396,7 @@ def RangeFinderDrivers(self): ("teraranger_serial", 35), ("nooploop_tofsense", 37), ("ainsteinlrd1", 42), + ("rds02uf", 43), ] while len(drivers): do_drivers = drivers[0:3] From 66717221e5a678061c8746d8ef25cfa731860717 Mon Sep 17 00:00:00 2001 From: zebulon-86 Date: Wed, 10 May 2023 08:04:45 +0800 Subject: [PATCH 4/7] Tools: extract_features.py: add AP_RangeFinder_RDS02UF --- Tools/scripts/extract_features.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 55db1039a595a..c78aa76492899 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -84,6 +84,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED', r'AP_RangeFinder_MaxsonarSerialLV::get_reading\b',), ('AP_RANGEFINDER_TRI2C_ENABLED', r'AP_RangeFinder_TeraRangerI2C::update\b',), ('AP_RANGEFINDER_JRE_SERIAL_ENABLED', r'AP_RangeFinder_JRE_Serial::get_reading\b',), + ('AP_RANGEFINDER_RDS02UF_ENABLED', r'AP_RangeFinder_RDS02UF::get_reading\b',), ('AP_GPS_{type}_ENABLED', r'AP_GPS_(?P.*)::read\b',), From a5251fd39effc43c5cc8b0a25fca83edef787599 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Mon, 25 Dec 2023 21:54:21 +0530 Subject: [PATCH 5/7] SITL: Update RDS02UF CRC --- libraries/SITL/SIM_RF_RDS02UF.cpp | 48 ++----------------------------- libraries/SITL/SIM_RF_RDS02UF.h | 3 +- 2 files changed, 4 insertions(+), 47 deletions(-) diff --git a/libraries/SITL/SIM_RF_RDS02UF.cpp b/libraries/SITL/SIM_RF_RDS02UF.cpp index 0a0a160e202b1..fe069789efa81 100644 --- a/libraries/SITL/SIM_RF_RDS02UF.cpp +++ b/libraries/SITL/SIM_RF_RDS02UF.cpp @@ -19,54 +19,10 @@ #include "SIM_RF_RDS02UF.h" #include +#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, - }; -static 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 @@ -110,7 +66,7 @@ uint32_t RF_RDS02UF::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t bu response.data1 = data_fc >> 8; response.data5 = alt_cm & 0xff; response.data6 = alt_cm >> 8; - response.crc8 = xcrc8(&response.buffer[2], 16); + response.crc8 = crc8_rds02uf(&response.buffer[2], 16); response.footer1 = 0xAA; response.footer2 = 0xAA; diff --git a/libraries/SITL/SIM_RF_RDS02UF.h b/libraries/SITL/SIM_RF_RDS02UF.h index 44a387370db6b..17e7e3fe52611 100644 --- a/libraries/SITL/SIM_RF_RDS02UF.h +++ b/libraries/SITL/SIM_RF_RDS02UF.h @@ -18,7 +18,8 @@ ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rds02uf --speedup=1 param set SERIAL5_PROTOCOL 9 -param set RNGFND1_TYPE 36 +param set RNGFND1_TYPE 43 +module load graph graph RANGEFINDER.distance graph GLOBAL_POSITION_INT.relative_alt/1000-RANGEFINDER.distance reboot From 9e41c62b0b4f89091e20e37dba553ce91fceb208 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Mon, 25 Dec 2023 21:55:26 +0530 Subject: [PATCH 6/7] AP_Math: Add RDS02UF RangeFinder CRC --- libraries/AP_Math/crc.cpp | 45 +++++++++++++++++++++++++++++++++++++++ libraries/AP_Math/crc.h | 3 ++- 2 files changed, 47 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index de81543b485cc..244cd31496013 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -227,6 +227,51 @@ uint8_t crc8_sae(const uint8_t *data, uint16_t length) return crc; } +// crc table for rangefinder rds02uf +static const uint8_t crc8_table_rds02uf[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 crc8_rds02uf(const uint8_t *data, uint16_t length) +{ + uint8_t crc = 0; + while (length--) { + crc = crc8_table_rds02uf[crc^*(data++)]; + } + return crc; +} + /* xmodem CRC thanks to avr-liberty https://github.com/dreamiurg/avr-liberty diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h index 5aa39e70a89e2..1c6ef78835bb3 100644 --- a/libraries/AP_Math/crc.h +++ b/libraries/AP_Math/crc.h @@ -13,7 +13,7 @@ along with this program. If not, see . */ /* - interfaces to ArduPilot collection of CRCs. + interfaces to ArduPilot collection of CRCs. */ #pragma once @@ -28,6 +28,7 @@ uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length); uint8_t crc8_dvb_update(uint8_t crc, const uint8_t* buf, const uint16_t buf_len); uint8_t crc8_maxim(const uint8_t *data, uint16_t length); uint8_t crc8_sae(const uint8_t *data, uint16_t length); +uint8_t crc8_rds02uf(const uint8_t *data, uint16_t length); uint16_t crc_xmodem_update(uint16_t crc, uint8_t data); uint16_t crc_xmodem(const uint8_t *data, uint16_t len); uint32_t crc_crc32(uint32_t crc, const uint8_t *buf, uint32_t size); From bf17b5a990e737ebae1e5f68953be0b851f189b4 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Mon, 25 Dec 2023 21:56:17 +0530 Subject: [PATCH 7/7] AP_RangeFinder: Refactor RDS02UF rangefinder --- libraries/AP_RangeFinder/AP_RangeFinder.h | 4 +- .../AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp | 233 ++++++------------ .../AP_RangeFinder/AP_RangeFinder_RDS02UF.h | 113 ++++----- .../AP_RangeFinder/AP_RangeFinder_config.h | 4 + 4 files changed, 118 insertions(+), 236 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 64124327f69b7..f94b0205ef7b8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -177,8 +177,10 @@ class RangeFinder #if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED Ainstein_LR_D1 = 42, #endif -#if AP_RANGEFINDER_SIM_ENABLED +#if AP_RANGEFINDER_RDS02UF_ENABLED RDS02UF = 43, +#endif +#if AP_RANGEFINDER_SIM_ENABLED SIM = 100, #endif }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp index bdfe34ebb2493..6e6087aaa6642 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp @@ -19,202 +19,107 @@ * Azimuth Coverage ±17°,Elevation Coverage ±3° * Frame Rate 20Hz */ -#include "AP_RangeFinder_RDS02UF.h" + +#include "AP_RangeFinder_config.h" #if AP_RANGEFINDER_RDS02UF_ENABLED +#include "AP_RangeFinder_RDS02UF.h" #include -#include -#define RDS02_HEAD1 0x55 -#define RDS02_HEAD2 0x55 +#define RDS02_HEAD 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 #define RDS02UF_IGNORE_CRC 0xFF -#define RDS02UF_NO_ERR 0x00 - -extern const AP_HAL::HAL& hal; +#define RDS02UF_NO_ERR 0x00 -// return last value measured by sensor -bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m) +bool AP_RangeFinder_RDS02UF::get_reading(float &distance_m) { if (uart == nullptr) { return false; } - // read any available data from the lidar - float sum = 0.0f; - uint16_t count = 0; - uint32_t nbytes = MAX(uart->available(), 1024U); - while (nbytes-- > 0) { - int16_t c = uart->read(); - if (c < 0) { - continue; - } - if (decode(c)) { - sum += _distance_m; - count++; - } + const uint32_t nbytes = uart->read(&u.parse_buffer[body_length], + ARRAY_SIZE(u.parse_buffer)-body_length); + if (nbytes == 0) { + return false; } + body_length += nbytes; - if (AP_HAL::millis() - state.last_reading_ms > RDS02UF_TIMEOUT_MS) { - set_status(RangeFinder::Status::NoData); - reading_m = 0.0f; + move_header_in_buffer(0); + + // header byte 1 is correct. + if (body_length < ARRAY_SIZE(u.parse_buffer)) { + // need a full buffer to have a valid message... return false; } - // return false if no new readings - if (count == 0) { + if (u.packet.headermagic2 != RDS02_HEAD) { + move_header_in_buffer(1); return false; } - // return average of all measurements - reading_m = sum / count; - update_status(); - return true; -} + const uint16_t read_len = u.packet.length_h << 8 | u.packet.length_l; + if (read_len != RDS02UF_DATA_LEN) { + // we can only accept the fixed length message + move_header_in_buffer(1); + return false; + } + + // check for the footer signatures: + if (u.packet.footermagic1 != RDS02_END) { + move_header_in_buffer(1); + return false; + } + if (u.packet.footermagic2 != RDS02_END) { + move_header_in_buffer(1); + return false; + } + // calculate checksum + const uint8_t checksum = crc8_rds02uf(&u.parse_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN); + if (u.packet.checksum != checksum && u.packet.checksum != RDS02UF_IGNORE_CRC) { + move_header_in_buffer(1); + return false; + } -// 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 RDS02UF_PARSE_STATE::STATE0_SYNC_1: - if (data == RDS02_HEAD1) { - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE1_SYNC_2; - } - break; - case RDS02UF_PARSE_STATE::STATE1_SYNC_2: - if (data == RDS02_HEAD2) { - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE2_ADDRESS; - } else { - parser_index = 0; - decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; - } - break; - case RDS02UF_PARSE_STATE::STATE2_ADDRESS: // address - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE3_ERROR_CODE; - break; - case RDS02UF_PARSE_STATE::STATE3_ERROR_CODE: // error_code - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE4_FC_CODE_L; - break; - case RDS02UF_PARSE_STATE::STATE4_FC_CODE_L: // fc_code low - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE5_FC_CODE_H; - break; - case RDS02UF_PARSE_STATE::STATE5_FC_CODE_H: // fc_code high - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE6_LENGTH_L; - break; - case RDS02UF_PARSE_STATE::STATE6_LENGTH_L: // lengh_low - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE7_LENGTH_H; - break; - case RDS02UF_PARSE_STATE::STATE7_LENGTH_H:{ // lengh_high - uint8_t read_len = data << 8 | parser_buffer[parser_index-1]; - // rds02uf data length is always 10 - if (read_len == RDS02UF_DATA_LEN) { - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE8_REAL_DATA; - } else { - parser_index = 0; - decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; - } - break; - } - case RDS02UF_PARSE_STATE::STATE8_REAL_DATA: // real_data - parser_buffer[parser_index++] = data; - if (parser_index == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN) ) { - decode_state = RDS02UF_PARSE_STATE::STATE9_CRC; - } - break; - case RDS02UF_PARSE_STATE::STATE9_CRC:{ // crc - #if RDS02_USE_CRC - const uint8_t crc_data = crc8(&parser_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN); - if (data == crc_data || data == RDS02UF_IGNORE_CRC) { // 0xff indicates that the current frame does not need to be checked. - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE10_END_1; - } else { - parser_index = 0; - decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; - } - #else - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE10_END_1; - #endif - break; - } - case RDS02UF_PARSE_STATE::STATE10_END_1: - if (data == RDS02_END) { - parser_buffer[parser_index++] = data; - decode_state = RDS02UF_PARSE_STATE::STATE11_END_2; - } else { - parser_index = 0; - decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; - } - break; - case RDS02UF_PARSE_STATE::STATE11_END_2:{ - 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 == RDS02UF_UAV_PRODUCTS_ID && err_code == RDS02UF_NO_ERR) {// 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(); - } - } - } - parser_index = 0; - decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; - break; + const uint16_t fc_code = (u.packet.fc_high << 8 | u.packet.fc_low); + if (fc_code == RDS02UF_UAV_PRODUCTS_ID && u.packet.error_code == RDS02UF_NO_ERR) { + // get target information + const uint16_t read_info_fc = (u.packet.data[1] << 8 | u.packet.data[0]); + if ((read_info_fc & RDS02UF_IGNORE_ID_BYTE) == RDS02_TARGET_INFO_FC) { + // read_info_fc = 0x70C + ID * 0x10, ID: 0~0xF + distance_m = (u.packet.data[6] * 256 + u.packet.data[5]) * 0.01f; + state.last_reading_ms = AP_HAL::millis(); } } - return ret; + // reset buffer + body_length = 0; + return true; } -uint8_t AP_RangeFinder_RDS02UF::crc8(uint8_t* pbuf, int32_t len) +// find a RDS02UF message in the buffer, starting at +// initial_offset. If found, that message (or partial message) will +// be moved to the start of the buffer. +void AP_RangeFinder_RDS02UF::move_header_in_buffer(uint8_t initial_offset) { -#if RDS02_USE_CRC - uint8_t crc = 0; - uint8_t* data = pbuf; - while (len--) - crc = crc8_table[crc^*(data++)]; - return crc; -#else - return 0; -#endif + uint8_t* header_ptr = (uint8_t*)memchr(&u.parse_buffer[initial_offset], RDS02_HEAD, body_length - initial_offset); + if (header_ptr != nullptr) { + size_t header_offset = header_ptr - &u.parse_buffer[0]; + if (header_offset != 0) { + // header was found, but not at index 0; move it back to start of array + memmove(u.parse_buffer, header_ptr, body_length - header_offset); + body_length -= header_offset; + } + } else { + // no header found; reset buffer + body_length = 0; + } } -#endif // AP_RANGEFINDER_RDS02UF_ENABLED \ No newline at end of file +#endif // AP_RANGEFINDER_RDS02UF_ENABLED + diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h index d9b8b6fc4400a..e4fc161c09164 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h @@ -15,18 +15,17 @@ #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 +#include "AP_RangeFinder_config.h" #if AP_RANGEFINDER_RDS02UF_ENABLED -// In order to save MCU resources, CRC check is not used -#define RDS02_USE_CRC 0 -#define RDS02_BUFFER_SIZE 50 +#include "AP_RangeFinder.h" +#include "AP_RangeFinder_Backend_Serial.h" + +#define RDS02_BUFFER_SIZE 50 +#define RDS02UF_DIST_MAX_CM 2000 +#define RDS02UF_DIST_MIN_CM 150 +#define RDS02UF_DATA_LEN 10 class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial { @@ -45,11 +44,23 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial return MAV_DISTANCE_SENSOR_RADAR; } - private: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; - // Data Format for Benewake Rds02UF + // find a RDS02UF message in the buffer, starting at + // initial_offset. If found, that message (or partial message) will + // be moved to the start of the buffer. + void move_header_in_buffer(uint8_t initial_offset); + + // get a distance reading + bool get_reading(float &reading_m) override; + uint16_t read_timeout_ms() const override { return 500; } + + // make sure readings go out-of-range when necessary + int16_t max_distance_cm()const override { return MIN(params.max_distance_cm, RDS02UF_DIST_MAX_CM); } + int16_t min_distance_cm() const override { return MAX(params.min_distance_cm, RDS02UF_DIST_MIN_CM); } + + // Data Format for Benewake Rds02UF // =============================== // 21 bytes total per message: // 1) 0x55 @@ -64,68 +75,28 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial // 10) CRC8 // 11) END_1 0xAA // 12) END_2 0xAA - /// enum for handled messages - enum class 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 + struct PACKED RDS02UFPacket { + uint8_t headermagic1; + uint8_t headermagic2; + uint8_t address; + uint8_t error_code; + uint8_t fc_low; + uint8_t fc_high; + uint8_t length_l; + uint8_t length_h; + uint8_t data[RDS02UF_DATA_LEN]; + uint8_t checksum; + uint8_t footermagic1; + uint8_t footermagic2; }; - RDS02UF_PARSE_STATE decode_state; - uint8_t parser_index; - uint8_t parser_buffer[RDS02_BUFFER_SIZE]; - float _distance_m; - - // get a distance reading - bool get_reading(float &reading_m) override; - uint16_t read_timeout_ms() const override { return 3000; } - uint8_t crc8(uint8_t *pbuf, int32_t len); - bool decode(uint8_t c); - -#if RDS02_USE_CRC - 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, + union RDS02UF_Union { + uint8_t parse_buffer[21]; + struct RDS02UFPacket packet; }; -#endif + RDS02UF_Union u; + + // number of bytes currently in the buffer + uint8_t body_length; }; #endif // AP_RANGEFINDER_RDS02UF_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h index 4bd647191f0d2..71e1d012170a0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -133,6 +133,10 @@ #define AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_RANGEFINDER_RDS02UF_ENABLED +#define AP_RANGEFINDER_RDS02UF_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 +#endif + #ifndef AP_RANGEFINDER_SIM_ENABLED #define AP_RANGEFINDER_SIM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) #endif