mirror of https://github.com/ArduPilot/ardupilot
AP_RangeFinder:add support for RDS02UF radar driver on serial
parameter RNGFNDx_TYPE is 42 Apply suggestions from code review Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>
This commit is contained in:
zebulon-86
Peter Barker
parent
6d360ec79c
commit
871292cb29
|
|
@ -60,6 +60,7 @@
|
|||
#include "AP_RangeFinder_TOFSenseF_I2C.h"
|
||||
#include "AP_RangeFinder_JRE_Serial.h"
|
||||
#include "AP_RangeFinder_Ainstein_LR_D1.h"
|
||||
#include "AP_RangeFinder_RDS02UF.h"
|
||||
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
|
@ -591,6 +592,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if AP_RANGEFINDER_RDS02UF_ENABLED
|
||||
case Type::RDS02UF:
|
||||
serial_create_fn = AP_RangeFinder_RDS02UF::create;
|
||||
break;
|
||||
#endif
|
||||
case Type::NONE:
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -178,6 +178,7 @@ public:
|
|||
Ainstein_LR_D1 = 42,
|
||||
#endif
|
||||
#if AP_RANGEFINDER_SIM_ENABLED
|
||||
RDS02UF = 43,
|
||||
SIM = 100,
|
||||
#endif
|
||||
};
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
|
|||
// @Param: TYPE
|
||||
// @DisplayName: Rangefinder type
|
||||
// @Description: Type of connected rangefinder
|
||||
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,100:SITL
|
||||
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,100:SITL
|
||||
// @User: Standard
|
||||
AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,220 @@
|
|||
/*
|
||||
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"
|
||||
|
||||
#if AP_RANGEFINDER_RDS02UF_ENABLED
|
||||
|
||||
#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_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)
|
||||
{
|
||||
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++;
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,131 @@
|
|||
/*
|
||||
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
|
||||
|
||||
// In order to save MCU resources, CRC check is not used
|
||||
#define RDS02_USE_CRC 0
|
||||
#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_RADAR;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
|
||||
|
||||
// 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 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
|
||||
};
|
||||
|
||||
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
|
||||
};
|
||||
#endif // AP_RANGEFINDER_RDS02UF_ENABLED
|
||||
Loading…
Reference in New Issue