diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index f9ffb4e4f819e1..a25ad2209679e6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -174,8 +174,10 @@ class RangeFinder #if AP_RANGEFINDER_JRE_SERIAL_ENABLED JRE_Serial = 41, #endif -#if AP_RANGEFINDER_SIM_ENABLED +#if AP_RANGEFINDER_RDS02UF_ENABLED RDS02UF = 42, +#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 bdfe34ebb2493e..6548673f8c9679 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp @@ -19,12 +19,15 @@ * 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 + +extern const AP_HAL::HAL& hal; #define RDS02_HEAD1 0x55 #define RDS02_HEAD2 0x55 @@ -40,44 +43,34 @@ #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 +#define RDS02UF_NO_ERR 0x00 -extern const AP_HAL::HAL& hal; - -// return last value measured by sensor +// return average of distances measured by sensor bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m) { if (uart == nullptr) { return false; } - // read any available data from the lidar + // read any available data from the radar float sum = 0.0f; uint16_t count = 0; - uint32_t nbytes = MAX(uart->available(), 1024U); + uint32_t nbytes = MIN(uart->available(), 1024U); + float distance_m; while (nbytes-- > 0) { - int16_t c = uart->read(); - if (c < 0) { + uint8_t c; + if (!uart->read(c)) { continue; } - if (decode(c)) { - sum += _distance_m; + if (decode(c, distance_m)) { + 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; @@ -85,18 +78,15 @@ bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m) // 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) +bool AP_RangeFinder_RDS02UF::decode(uint8_t data, float &distance_m) { - uint8_t data = c; bool ret = false; - switch (decode_state){ + switch (decode_state) { case RDS02UF_PARSE_STATE::STATE0_SYNC_1: if (data == RDS02_HEAD1) { parser_buffer[parser_index++] = data; @@ -112,30 +102,35 @@ bool AP_RangeFinder_RDS02UF::decode(uint8_t c) decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; } break; - case RDS02UF_PARSE_STATE::STATE2_ADDRESS: // address + 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 + 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 + 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 + 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 + case RDS02UF_PARSE_STATE::STATE6_LENGTH_L: + // length_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) { + case RDS02UF_PARSE_STATE::STATE7_LENGTH_H:{ + // length_high + uint16_t read_len = data << 8 | parser_buffer[parser_index-1]; + if (read_len == RDS02UF_DATA_LEN && read_len < RDS02_BUFFER_SIZE) { parser_buffer[parser_index++] = data; decode_state = RDS02UF_PARSE_STATE::STATE8_REAL_DATA; } else { @@ -150,20 +145,17 @@ bool AP_RangeFinder_RDS02UF::decode(uint8_t c) 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. + case RDS02UF_PARSE_STATE::STATE9_CRC:{ + // crc + const uint8_t crc_data = crc8_rds02uf(&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: @@ -174,22 +166,22 @@ bool AP_RangeFinder_RDS02UF::decode(uint8_t c) parser_index = 0; decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; } + + if (parser_index == RDS02_BUFFER_SIZE) { + 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]; + const uint16_t fc_code = (parser_buffer[RDS02UF_PRODUCTS_FC_INDEX_H] << 8 | parser_buffer[RDS02UF_PRODUCTS_FC_INDEX_L]); + const 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); - } + if (fc_code == RDS02UF_UAV_PRODUCTS_ID && err_code == RDS02UF_NO_ERR) { + // get target information + const 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]) * 0.01f; ret = true; state.last_reading_ms = AP_HAL::millis(); } @@ -203,18 +195,4 @@ bool AP_RangeFinder_RDS02UF::decode(uint8_t c) 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 +#endif // AP_RANGEFINDER_RDS02UF_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h index d9b8b6fc4400a4..c14a367a0b3fc0 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h @@ -15,18 +15,16 @@ #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 class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial { @@ -45,7 +43,6 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial return MAV_DISTANCE_SENSOR_RADAR; } - private: using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial; @@ -83,49 +80,15 @@ class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial 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 + 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); } + + // parse a single character + bool decode(uint8_t c, float &distance_m); }; #endif // AP_RANGEFINDER_RDS02UF_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h index a6c793c340517b..37b50abf30f468 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