zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_RangeFinder:add support for RDS02UF radar driver on serial

master
Zebulon 3 years ago
parent
a7aa5b6897
commit
e83c3ccd30
4 changed files with 364 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      libraries/AP_RangeFinder/AP_RangeFinder.cpp
  2. 1
      libraries/AP_RangeFinder/AP_RangeFinder.h
  3. 222
      libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp
  4. 135
      libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h

6
libraries/AP_RangeFinder/AP_RangeFinder.cpp
Unescape View File

@ -51,6 +51,7 @@
#include "AP_RangeFinder_MSP.h" #include "AP_RangeFinder_MSP.h"
#include "AP_RangeFinder_USD1_CAN.h" #include "AP_RangeFinder_USD1_CAN.h"
#include "AP_RangeFinder_Benewake_CAN.h" #include "AP_RangeFinder_Benewake_CAN.h"
#include "AP_RangeFinder_RDS02UF.h"
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
@ -608,6 +609,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
#if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
_add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance); _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance);
break; break;
#endif
#if AP_RANGEFINDER_RDS02UF_ENABLED
case Type::RDS02UF:
serial_create_fn = AP_RangeFinder_RDS02UF::create;
break;
#endif #endif
case Type::NONE: case Type::NONE:
break; break;

1
libraries/AP_RangeFinder/AP_RangeFinder.h
Unescape View File

@ -103,6 +103,7 @@ public:
USD1_CAN = 33, USD1_CAN = 33,
Benewake_CAN = 34, Benewake_CAN = 34,
TeraRanger_Serial = 35, TeraRanger_Serial = 35,
RDS02UF = 36,
SIM = 100, SIM = 100,
}; };

222
libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp
Unescape View File

@ -0,0 +1,222 @@
/*
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 <http://www.gnu.org/licenses/>.
*/
/**
* RDS02UF Note:
* Sensor range scope 1.5m~20.0m
* Azimuth Coverage ±17°,Elevation Coverage ±3°
* Frame Rate 20Hz
*/
#include "AP_RangeFinder_RDS02UF.h"
#include <AP_HAL/AP_HAL.h>
#include <ctype.h>
#define RDS02_HEAD1 0x55
#define RDS02_HEAD2 0x55
#define RDS02_END 0xAA
#define RDS02UF_HEAD_LEN 2
#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_DATA_FC_L 8
#define RDS02_DATA_FC_H 9
#define RDS02_TARGET_INFO 0x70C
#define RDS02UF_DIST_MAX_CM 2000
#define RDS02UF_DIST_MIN_CM 150
#define AP_RANGEFINDER_RDS02UF_TIMEOUT_MS 200
extern const AP_HAL::HAL& hal;
AP_RangeFinder_RDS02UF::AP_RangeFinder_RDS02UF(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params):
AP_RangeFinder_Backend_Serial(_state, _params)
{
}
// return last value measured by sensor
bool AP_RangeFinder_RDS02UF::get_reading(float &reading_m)
{
if (uart == nullptr) {
return false;
}
// read any available lines from the lidar
float sum = 0.0f;
uint16_t count = 0;
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
uint8_t c = uart->read();
if (decode(c)) {
sum += _distance_m;
count++;
}
}
// return false on failure
if (count == 0) {
return false;
}
if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_RDS02UF_TIMEOUT_MS) {
set_status(RangeFinder::Status::NoData);
reading_m = 0.0f;
} else {
// return average of all measurements
reading_m = sum / count;
update_status();
}
// return average of all measurements
reading_m = sum / count;
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 STATE0_SYNC_1:
if (data == RDS02_HEAD1) {
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE1_SYNC_2;
}
break;
case STATE1_SYNC_2:
if (data == RDS02_HEAD2) {
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE2_ADDRESS;
} else {
parserbuf_index = 0;
decode_state = STATE0_SYNC_1;
}
break;
case STATE2_ADDRESS: // address
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE3_ERROR_CODE;
break;
case STATE3_ERROR_CODE: // error_code
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE4_FC_CODE_L;
break;
case STATE4_FC_CODE_L: // fc_code low
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE5_FC_CODE_H;
break;
case STATE5_FC_CODE_H: // fc_code high
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE6_LENGTH_L;
break;
case STATE6_LENGTH_L: // lengh_low
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE7_LENGTH_H;
break;
case STATE7_LENGTH_H: // lengh_high
{
uint8_t read_len = data << 8 | rev_buffer[parserbuf_index-1];
if ( read_len == RDS02UF_DATA_LEN) // rds02uf data length is 10
{
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE8_REAL_DATA;
}else{
parserbuf_index = 0;
decode_state = STATE0_SYNC_1;
}
break;
}
case STATE8_REAL_DATA: // real_data
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
if ( parserbuf_index == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN) ) {
decode_state = STATE9_CRC;
}
break;
case STATE9_CRC: // crc
{
uint8_t crc_data = 0;
crc_data = crc8(&rev_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN);
rev_buffer[parserbuf_index] = data;
if (crc_data == data || data == 0xff) {
parserbuf_index++;
decode_state = STATE10_END_1;
} else {
parserbuf_index = 0;
decode_state = STATE0_SYNC_1;
}
break;
}
case STATE10_END_1: //
if (data == RDS02_END) {
rev_buffer[parserbuf_index] = data;
parserbuf_index++;
decode_state = STATE11_END_2;
} else {
parserbuf_index = 0;
decode_state = STATE0_SYNC_1;
}
break;
case STATE11_END_2: //
{
uint16_t fc_code = (rev_buffer[STATE5_FC_CODE_H] << 8 | rev_buffer[STATE4_FC_CODE_L]);
uint8_t err_code = rev_buffer[STATE3_ERROR_CODE];
if (data == RDS02_END)
{
if (fc_code == 0x03ff && err_code == 0) {// get targer information
uint16_t data_fc = (rev_buffer[RDS02_DATA_FC_H] << 8 | rev_buffer[RDS02_DATA_FC_L]);
if((data_fc & 0xf0f) == RDS02_TARGET_INFO){ // data_fc = 0x70C + ID * 0x10, ID: 0~0xF
float distance = (rev_buffer[RDS02_DATA_Y_INDEX_H] * 256 + rev_buffer[RDS02_DATA_Y_INDEX_L]) / 100.0f;
_distance_m = distance;
_distance_m = MIN(MAX(RDS02UF_DIST_MIN_CM, uint16_t(_distance_m*100)), RDS02UF_DIST_MAX_CM) * 0.01f;
ret = true;
state.last_reading_ms = AP_HAL::millis();
}
}
}
parserbuf_index = 0;
decode_state = STATE0_SYNC_1;
break;
}
}
return ret;
}
uint8_t AP_RangeFinder_RDS02UF::crc8(uint8_t* pbuf, int32_t len)
{
uint8_t* data = pbuf;
uint8_t crc = 0;
while ( len-- )
crc = crc8_table[crc^*(data++)];
return crc;
}

135
libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h
Unescape View File

@ -0,0 +1,135 @@
/*
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 <http://www.gnu.org/licenses/>.
*/
#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
#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_ULTRASOUND;
}
private:
AP_RangeFinder_RDS02UF(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
uint8_t decode_state;
uint8_t parserbuf_index;
uint8_t rev_buffer[RDS02_BUFFER_SIZE];
float _distance_m;
void ParseRds02(uint8_t *data_in, uint8_t datalengh);
// 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 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
};
// get a distance reading
bool get_reading(float &reading_m) override;
// get temperature reading in C. returns true on success and populates temp argument
// bool get_temp(float &temp) const override;
uint16_t read_timeout_ms() const override { return 3000; }
uint8_t crc8(uint8_t *pbuf, int32_t len);
bool decode(uint8_t c);
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 // AP_RANGEFINDER_NMEA_ENABLED
Write Preview
Loading…
Cancel
Save