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

AP_RangeFinder:add support for Benewake RDS02UF driver on serial

Copter-4.2-BW
Zebulon 3 years ago
parent
e762380f1c
commit
4946331941
4 changed files with 327 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
Split 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. 184
      libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp
  4. 136
      libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h

6
libraries/AP_RangeFinder/AP_RangeFinder.cpp
Unescape View File

@ -49,6 +49,7 @@ @@ -49,6 +49,7 @@
#include "AP_RangeFinder_MSP.h"
#include "AP_RangeFinder_USD1_CAN.h"
#include "AP_RangeFinder_Benewake_CAN.h"
#include "AP_RangeFinder_RDS02UF.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h>
@ -584,6 +585,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) @@ -584,6 +585,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
_add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance);
break;
#endif
case Type::RDS02UF:
if (AP_RangeFinder_RDS02UF::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_RDS02UF(state[instance], params[instance]), instance, serial_instance++);
}
break;
case Type::NONE:
default:
break;

1
libraries/AP_RangeFinder/AP_RangeFinder.h
Unescape View File

@ -89,6 +89,7 @@ public: @@ -89,6 +89,7 @@ public:
MSP = 32,
USD1_CAN = 33,
Benewake_CAN = 34,
RDS02UF = 35,
SIM = 100,
};

184
libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.cpp
Unescape View File

@ -0,0 +1,184 @@ @@ -0,0 +1,184 @@
/*
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/>.
*/
#include "AP_RangeFinder_RDS02UF.h"
#include <AP_HAL/AP_HAL.h>
#include <ctype.h>
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 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;
}
// 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 (step)
{
case STATE1_SYNC_1:
if (data == RDS02_HEAD1)
{
rev_buffer[step] = data;
step++;
}
break;
case STATE2_SYNC_2:
if (data == RDS02_HEAD2)
{
rev_buffer[step] = data;
step++;
}
else
{
ParseDataFailed(rev_buffer, sizeof(rev_buffer));
}
break;
case STATE3_ADDRESS: // address
rev_buffer[step] = data;
step++;
break;
case STATE4_ERROR_CODE: // error_code
rev_buffer[step] = data;
err_code = data;
step++;
break;
case STATE5_FC_CODE_L: // fc_code low
rev_buffer[step] = data;
fc_code = data;
step++;
break;
case STATE6_FC_CODE_H: // fc_code high
rev_buffer[step] = data;
fc_code = fc_code | (data << 8);
step++;
break;
case STATE7_LENGTH_L: // lengh_low
data_len = data;
rev_buffer[step] = data;
step++;
break;
case STATE8_LENGTH_H: // lengh_high
data_len = data << 8 | data_len;
if(data_len == 10){
rev_buffer[step] = data;
step++;
}
else
{
ParseDataFailed(rev_buffer, sizeof(rev_buffer));
}
break;
case STATE9_REAL_DATA: // real_data
rev_buffer[step + data_index] = data;
data_index++;
if (data_index == data_len)
{
step++;
}
break;
case STATE10_CRC: // crc
crc_data = crc8(&rev_buffer[2], 6 + data_len);
rev_buffer[step + data_index] = data;
if (crc_data == data || data == 0xff)
{
step++;
}
else
{
ParseDataFailed(rev_buffer, sizeof(rev_buffer));
}
break;
case STATE11_END_1: //
if (data == RDS02_END)
{
rev_buffer[step + data_index] = data;
step++;
}
else
{
ParseDataFailed(rev_buffer, sizeof(rev_buffer));
}
break;
case STATE12_END_2: //
if (data == RDS02_END)
{
if (fc_code == 0x03ff && err_code == 0)
{
float distance = (rev_buffer[RDS02_ValueY_H] * 256 + rev_buffer[RDS02_ValueY_L]) / 100.0f;
_distance_m = distance;
ret = true;
}
ParseDataFailed(rev_buffer, sizeof(rev_buffer));
}
break;
}
return ret;
}
void AP_RangeFinder_RDS02UF::ParseDataFailed(uint8_t *data_in, uint8_t datalengh)
{
memset(data_in, 0, datalengh);
step = 0;
address = 0;
data_len = 0;
data_index = 0;
crc_data = 0;
fc_code = 0;
err_code = 0;
}
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;
}

136
libraries/AP_RangeFinder/AP_RangeFinder_RDS02UF.h
Unescape View File

@ -0,0 +1,136 @@ @@ -0,0 +1,136 @@
/*
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"
#define RDS02_HEAD1 0x55
#define RDS02_HEAD2 0x55
#define RDS02_END 0xAA
#define RDS02_BUFFER_SIZE 50
#define RDS02_ValueY_L 13
#define RDS02_ValueY_H 14
class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial
{
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
protected:
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_ULTRASOUND;
}
private:
uint8_t step;
uint8_t address;
uint16_t data_len;
uint16_t data_index;
uint8_t crc_data;
uint16_t fc_code;
uint8_t err_code;
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 {
STATE1_SYNC_1 = 0,
STATE2_SYNC_2,
STATE3_ADDRESS,
STATE4_ERROR_CODE,
STATE5_FC_CODE_L,
STATE6_FC_CODE_H,
STATE7_LENGTH_L,
STATE8_LENGTH_H,
STATE9_REAL_DATA,
STATE10_CRC,
STATE11_END_1,
STATE12_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);
void ParseDataFailed(uint8_t *data_in, uint8_t datalengh);
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,
};
};
Write Preview
Loading…
Cancel
Save