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

AP_RangeFinder: BLPing: Rework class to work with new firmware version 3.28 

- Uses the continuous message request

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
zr-v5.1
Patrick José Pereira 5 years ago committed by Jacob Walser
parent
670ac124c0
commit
a183d00b7e
2 changed files with 244 additions and 171 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. 239
      libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp
  2. 176
      libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h

239
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp
Unescape View File

@ -17,36 +17,6 @@ @@ -17,36 +17,6 @@
#include <GCS_MAVLink/GCS.h>
#include "AP_RangeFinder_BLPing.h"
#define BLPING_INIT_RATE_MS 1000 // initialise sensor at no more than 1hz
#define BLPING_FRAME_HEADER1 0x42 // header first byte ('B')
#define BLPING_FRAME_HEADER2 0x52 // header second byte ('R')
#define BLPING_SRC_ID 0 // vehicle's source id
#define BLPING_DEST_ID 1 // sensor's id
#define BLPING_MSGID_ACK 1
#define BLPING_MSGID_NACK 2
#define BLPING_MSGID_SET_PING_INTERVAL 1004
#define BLPING_MSGID_GET_DEVICE_ID 1201
#define BLDPIN_MSGID_DISTANCE_SIMPLE 1211
#define BLPING_MSGID_CONTINUOUS_START 1400
// Protocol implemented by this sensor can be found here: https://github.com/bluerobotics/ping-protocol
//
// Byte Type Name Description
// --------------------------------------------------------------------------------------------------------------
// 0 uint8_t start1 'B'
// 1 uint8_t start2 'R'
// 2-3 uint16_t payload_length number of bytes in payload (low byte, high byte)
// 4-5 uint16_t message id message id (low byte, high byte)
// 6 uint8_t src_device_id id of device sending the message
// 7 uint8_t dst_device_id id of device of the intended recipient
// 8-n uint8_t[] payload message payload
// (n+1)-(n+2) uint16_t checksum the sum of all the non-checksum bytes in the message (low byte, high byte)
/*
update the state of the sensor
*/
void AP_RangeFinder_BLPing::update(void)
{
if (uart == nullptr) {
@ -57,7 +27,7 @@ void AP_RangeFinder_BLPing::update(void) @@ -57,7 +27,7 @@ void AP_RangeFinder_BLPing::update(void)
if (status() == RangeFinder::Status::NoData) {
const uint32_t now = AP_HAL::millis();
// initialise sensor if no distances recently
if (now - last_init_ms > BLPING_INIT_RATE_MS) {
if (now - last_init_ms > read_timeout_ms()) {
last_init_ms = now;
init_sensor();
}
@ -66,12 +36,65 @@ void AP_RangeFinder_BLPing::update(void) @@ -66,12 +36,65 @@ void AP_RangeFinder_BLPing::update(void)
void AP_RangeFinder_BLPing::init_sensor()
{
// request distance from sensor
send_message(BLDPIN_MSGID_DISTANCE_SIMPLE, nullptr, 0);
// Set message interval between pings in ms
uint16_t ping_interval = _sensor_rate_ms;
protocol.send_message(uart, PingProtocol::MessageId::SET_PING_INTERVAL, reinterpret_cast<uint8_t*>(&ping_interval), sizeof(ping_interval));
// Send a message requesting a continuous
uint16_t continuous_message = static_cast<uint16_t>(PingProtocol::MessageId::DISTANCE_SIMPLE);
protocol.send_message(uart, PingProtocol::MessageId::CONTINUOUS_START, reinterpret_cast<uint8_t*>(&continuous_message), sizeof(continuous_message));
}
// send message to sensor
void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload, uint16_t payload_len)
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool AP_RangeFinder_BLPing::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
}
struct {
float sum_cm = 0;
uint16_t count = 0;
float mean() { return sum_cm / count; };
} averageStruct;
// read any available lines from the lidar
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
const int16_t b = uart->read();
if (b < 0) {
break;
}
if (protocol.parse_byte(b) == PingProtocol::MessageId::DISTANCE_SIMPLE) {
averageStruct.count++;
averageStruct.sum_cm += protocol.get_distance_mm()/10.0f;
}
}
if (averageStruct.count > 0) {
// return average distance of readings
reading_cm = averageStruct.mean();
return true;
}
// no readings so return false
return false;
}
uint8_t PingProtocol::get_confidence() const
{
return msg.payload[4];
}
uint32_t PingProtocol::get_distance_mm() const
{
return (uint32_t)msg.payload[0] |
(uint32_t)msg.payload[1] << 8 |
(uint32_t)msg.payload[2] << 16 |
(uint32_t)msg.payload[3] << 24;
}
void PingProtocol::send_message(AP_HAL::UARTDriver *uart, PingProtocol::MessageId msg_id, const uint8_t *payload, uint16_t payload_len) const
{
if (uart == nullptr) {
return;
@ -83,27 +106,27 @@ void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload, @@ -83,27 +106,27 @@ void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload,
}
// write header
uart->write((uint8_t)BLPING_FRAME_HEADER1);
uart->write((uint8_t)BLPING_FRAME_HEADER2);
uint16_t crc = BLPING_FRAME_HEADER1 + BLPING_FRAME_HEADER2;
uart->write(_frame_header1);
uart->write(_frame_header2);
uint16_t crc = _frame_header1 + _frame_header2;
// write payload length
uart->write(LOWBYTE(payload_len));
uart->write(HIGHBYTE(payload_len));
crc += LOWBYTE(payload_len) + HIGHBYTE(payload_len);
// msgid
uart->write(LOWBYTE(msgid));
uart->write(HIGHBYTE(msgid));
crc += LOWBYTE(msgid) + HIGHBYTE(msgid);
// message id
uart->write(LOWBYTE(msg_id));
uart->write(HIGHBYTE(msg_id));
crc += LOWBYTE(msg_id) + HIGHBYTE(msg_id);
// src dev id
uart->write((uint8_t)BLPING_SRC_ID);
crc += BLPING_SRC_ID;
uart->write(_src_id);
crc += _src_id;
// destination dev id
uart->write((uint8_t)BLPING_DEST_ID);
crc += BLPING_DEST_ID;
uart->write(_dst_id);
crc += _dst_id;
// payload
if (payload != nullptr) {
@ -118,105 +141,64 @@ void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload, @@ -118,105 +141,64 @@ void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload,
uart->write(HIGHBYTE(crc));
}
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool AP_RangeFinder_BLPing::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
}
float sum_cm = 0;
uint16_t count = 0;
// read any available lines from the lidar
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
const int16_t b = uart->read();
if (b < 0) {
break;
}
if (parse_byte(b)) {
count++;
sum_cm += distance_cm;
}
}
if (count > 0) {
// return average distance of readings
reading_cm = sum_cm / count;
// request another distance
send_message(BLDPIN_MSGID_DISTANCE_SIMPLE, nullptr, 0);
return true;
}
// no readings so return false
return false;
}
// process one byte received on serial port
// returns true if a distance message has been successfully parsed
// state is stored in msg structure
bool AP_RangeFinder_BLPing::parse_byte(uint8_t b)
PingProtocol::MessageId PingProtocol::parse_byte(uint8_t b)
{
bool got_distance = false;
// process byte depending upon current state
switch (msg.state) {
case ParseState::HEADER1:
if (b == BLPING_FRAME_HEADER1) {
msg.crc_expected = BLPING_FRAME_HEADER1;
msg.state = ParseState::HEADER2;
case ParserState::HEADER1:
if (b == _frame_header1) {
msg.crc_expected = _frame_header1;
msg.state = ParserState::HEADER2;
msg.done = false;
}
break;
case ParseState::HEADER2:
if (b == BLPING_FRAME_HEADER2) {
msg.crc_expected += BLPING_FRAME_HEADER2;
msg.state = ParseState::LEN_L;
case ParserState::HEADER2:
if (b == _frame_header2) {
msg.crc_expected += _frame_header2;
msg.state = ParserState::LEN_L;
} else {
msg.state = ParseState::HEADER1;
msg.state = ParserState::HEADER1;
}
break;
case ParseState::LEN_L:
case ParserState::LEN_L:
msg.payload_len = b;
msg.crc_expected += b;
msg.state = ParseState::LEN_H;
msg.state = ParserState::LEN_H;
break;
case ParseState::LEN_H:
case ParserState::LEN_H:
msg.payload_len |= ((uint16_t)b << 8);
msg.payload_recv = 0;
msg.crc_expected += b;
msg.state = ParseState::MSG_ID_L;
msg.state = ParserState::MSG_ID_L;
break;
case ParseState::MSG_ID_L:
msg.msgid = b;
case ParserState::MSG_ID_L:
msg.id = b;
msg.crc_expected += b;
msg.state = ParseState::MSG_ID_H;
msg.state = ParserState::MSG_ID_H;
break;
case ParseState::MSG_ID_H:
msg.msgid |= ((uint16_t)b << 8);
case ParserState::MSG_ID_H:
msg.id |= ((uint16_t)b << 8);
msg.crc_expected += b;
msg.state = ParseState::SRC_ID;
msg.state = ParserState::SRC_ID;
break;
case ParseState::SRC_ID:
case ParserState::SRC_ID:
msg.crc_expected += b;
msg.state = ParseState::DST_ID;
msg.state = ParserState::DST_ID;
break;
case ParseState::DST_ID:
case ParserState::DST_ID:
msg.crc_expected += b;
msg.state = ParseState::PAYLOAD;
msg.state = ParserState::PAYLOAD;
break;
case ParseState::PAYLOAD:
case ParserState::PAYLOAD:
if (msg.payload_recv < msg.payload_len) {
if (msg.payload_recv < ARRAY_SIZE(msg.payload)) {
msg.payload[msg.payload_recv] = b;
@ -225,37 +207,22 @@ bool AP_RangeFinder_BLPing::parse_byte(uint8_t b) @@ -225,37 +207,22 @@ bool AP_RangeFinder_BLPing::parse_byte(uint8_t b)
msg.crc_expected += b;
}
if (msg.payload_recv == msg.payload_len) {
msg.state = ParseState::CRC_L;
msg.state = ParserState::CRC_L;
}
break;
case ParseState::CRC_L:
case ParserState::CRC_L:
msg.crc = b;
msg.state = ParseState::CRC_H;
msg.state = ParserState::CRC_H;
break;
case ParseState::CRC_H:
case ParserState::CRC_H:
msg.crc |= ((uint16_t)b << 8);
msg.state = ParseState::HEADER1;
if (msg.crc_expected == msg.crc) {
// process payload
switch (msg.msgid) {
case BLPING_MSGID_ACK:
case BLPING_MSGID_NACK:
// ignore
break;
case BLDPIN_MSGID_DISTANCE_SIMPLE:
const uint32_t dist_mm = (uint32_t)msg.payload[0] | (uint32_t)msg.payload[1] << 8 | (uint32_t)msg.payload[2] << 16 | (uint32_t)msg.payload[3] << 24;
distance_cm = dist_mm / 10;
got_distance = true;
break;
}
}
msg.state = ParserState::HEADER1;
msg.done = msg.crc_expected == msg.crc;
break;
}
return got_distance;
return msg.done ? get_message_id() : MessageId::INVALID;
}

176
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h
Unescape View File

@ -3,40 +3,82 @@ @@ -3,40 +3,82 @@
#include "AP_RangeFinder.h"
#include "AP_RangeFinder_Backend_Serial.h"
class AP_RangeFinder_BLPing : public AP_RangeFinder_Backend_Serial
{
/**
* @brief Simple class to deal with Ping Protocol
*
* Protocol documentation can be found here: https:* github.com/bluerobotics/ping-protocol
*
* Byte Type Name Description
* --------------------------------------------------------------------------------------------------------------
* 0 uint8_t start1 'B'
* 1 uint8_t start2 'R'
* 2-3 uint16_t payload_length number of bytes in payload (low byte, high byte)
* 4-5 uint16_t message id message id (low byte, high byte)
* 6 uint8_t src_device_id id of device sending the message
* 7 uint8_t dst_device_id id of device of the intended recipient
* 8-n uint8_t[] payload message payload
* (n+1)-(n+2) uint16_t checksum the sum of all the non-checksum bytes in the message (low byte, high byte)
*/
class PingProtocol {
static constexpr uint8_t _frame_header1 = 0x42; // // header first byte ('B')
static constexpr uint8_t _frame_header2 = 0x52; // // header first byte ('R')
static constexpr uint16_t _src_id = 0; // vehicle's source id
static constexpr uint16_t _dst_id = 1; // sensor's id
public:
enum class MessageId {
INVALID = 0,
SET_PING_INTERVAL = 1004,
DISTANCE_SIMPLE = 1211,
CONTINUOUS_START = 1400,
};
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
// update state
void update(void) override;
/**
* @brief Process a single byte received on serial port
* return a valid MessageId if there is a message in buffer.
*
* @param byte
* @return MessageId
*/
MessageId parse_byte(uint8_t b);
/**
* @brief Send a message with a defined payload
*
* @param uart
* @param msgid
* @param payload
* @param payload_len
*/
void send_message(AP_HAL::UARTDriver *uart, PingProtocol::MessageId msg_id, const uint8_t *payload, uint16_t payload_len) const;
/**
* @brief Get distance from message
*
* @return uint32_t
*/
uint32_t get_distance_mm() const;
/**
* @brief Get confidence from message
*
* @return uint8_t
*/
uint8_t get_confidence() const;
/**
* @brief Get the message id available in bufffer
*
* @return MessageId
*/
MessageId get_message_id() const { return static_cast<MessageId>(msg.id); };
protected:
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_ULTRASOUND;
}
private:
void init_sensor();
// send message to sensor
void send_message(uint16_t msgid, const uint8_t *payload, uint16_t payload_len);
// read a distance from the sensor
bool get_reading(uint16_t &reading_cm) override;
uint16_t read_timeout_ms() const override { return 500; }
// process one byte received on serial port
// returns true if a distance message has been successfully parsed
// state is stored in msg structure
bool parse_byte(uint8_t b);
enum class ParseState {
/**
* @brief State for the parser logic
*
*/
enum class ParserState {
HEADER1 = 0,
HEADER2,
LEN_L,
@ -50,17 +92,81 @@ private: @@ -50,17 +92,81 @@ private:
CRC_H
};
uint32_t last_init_ms; // system time that sensor was last initialised
uint16_t distance_cm; // latest distance
// structure holding latest message contents
/**
* @brief Structure holding the last message available with its state
*
*/
struct {
ParseState state; // state of incoming message processing
ParserState state; // state of incoming message processing
bool done; // inform if the message is complete or not
uint8_t payload[20]; // payload
uint16_t payload_len; // latest message payload length
uint16_t msgid; // latest message's message id
uint16_t id; // latest message's message id
uint16_t payload_recv; // number of message's payload bytes received so far
uint16_t crc; // latest message's crc
uint16_t crc_expected; // latest message's expected crc
} msg;
};
/**
* @brief Class for Blue Robotics Ping1D sensor
*
*/
class AP_RangeFinder_BLPing : public AP_RangeFinder_Backend_Serial
{
static constexpr uint16_t _sensor_rate_ms = 50; // initialise sensor at no more than 20hz
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
/**
* @brief Update class state
*
*/
void update(void) override;
protected:
/**
* @brief Return the sensor type
*
* @return MAV_DISTANCE_SENSOR
*/
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_ULTRASOUND;
}
/**
* @brief Sensor protocol class
*
*/
PingProtocol protocol;
private:
/**
* @brief Do the necessary sensor initiation
*
*/
void init_sensor();
/**
* @brief Read serial interface and calculate new distance
*
* @param reading_cm
* @return true
* @return false
*/
bool get_reading(uint16_t &reading_cm) override;
/**
* @brief Timeout between messages
*
* @return uint16_t
*/
uint16_t read_timeout_ms() const override { return 1000; }
/**
* @brief system time that sensor was last initialised
*
*/
uint32_t last_init_ms;
};

Write Preview
Loading…
Cancel
Save