Skip to content

Commit

Permalink
AP_RangeFinder: Refactor RDS02UF rangefinder
Browse files Browse the repository at this point in the history
  • Loading branch information
@rishabsingh3003
rishabsingh3003 committed Feb 26, 2024
1 parent f5dc60a commit 6bd9ba8
Show file tree
Hide file tree
Showing 4 changed files with 70 additions and 123 deletions.
4 changes: 3 additions & 1 deletion libraries/AP_RangeFinder/AP_RangeFinder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
};
Expand Down
120 changes: 49 additions & 71 deletions libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <AP_HAL/AP_HAL.h>
#include <ctype.h>

extern const AP_HAL::HAL& hal;

#define RDS02_HEAD1 0x55
#define RDS02_HEAD2 0x55
Expand All @@ -40,63 +43,50 @@
#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;
}

// 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;
Expand All @@ -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 {
Expand All @@ -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:
Expand All @@ -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();
}
Expand All @@ -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
#endif // AP_RANGEFINDER_RDS02UF_ENABLED
65 changes: 14 additions & 51 deletions libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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;

Expand Down Expand Up @@ -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
4 changes: 4 additions & 0 deletions libraries/AP_RangeFinder/AP_RangeFinder_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 6bd9ba8

Please sign in to comment.