mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_RangeFinder: Refactor RDS02UF rangefinder
This commit is contained in:
rishabsingh3003
Peter Barker
parent
276ee86f38
commit
b22e4fa085
@ -177,8 +177,10 @@ public:
|
||||
#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
|
||||
};
|
||||
|
||||
@ -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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
// 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;
|
||||
}
|
||||
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;
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
parser_index = 0;
|
||||
decode_state = RDS02UF_PARSE_STATE::STATE0_SYNC_1;
|
||||
break;
|
||||
}
|
||||
|
||||
// reset buffer
|
||||
body_length = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
|
||||
@ -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
|
||||
#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,10 +44,22 @@ protected:
|
||||
return MAV_DISTANCE_SENSOR_RADAR;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
|
||||
|
||||
// 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:
|
||||
@ -64,68 +75,28 @@ private:
|
||||
// 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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user