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 @peterbarker
rishabsingh3003 authored and peterbarker committed Apr 1, 2024
1 parent 276ee86 commit b22e4fa
Show file tree
Hide file tree
Showing 4 changed files with 118 additions and 236 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 @@ -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
};
Expand Down
233 changes: 69 additions & 164 deletions libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <AP_HAL/AP_HAL.h>
#include <ctype.h>

#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
#endif // AP_RANGEFINDER_RDS02UF_ENABLED

Loading

0 comments on commit b22e4fa

Please sign in to comment.