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>
This commit is contained in:
Patrick José Pereira 2020-01-28 15:41:25 -03:00 committed by Jacob Walser
parent 670ac124c0
commit a183d00b7e
2 changed files with 243 additions and 170 deletions

View File

@ -17,36 +17,6 @@
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include "AP_RangeFinder_BLPing.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) void AP_RangeFinder_BLPing::update(void)
{ {
if (uart == nullptr) { if (uart == nullptr) {
@ -57,7 +27,7 @@ void AP_RangeFinder_BLPing::update(void)
if (status() == RangeFinder::Status::NoData) { if (status() == RangeFinder::Status::NoData) {
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
// initialise sensor if no distances recently // 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; last_init_ms = now;
init_sensor(); init_sensor();
} }
@ -66,12 +36,65 @@ void AP_RangeFinder_BLPing::update(void)
void AP_RangeFinder_BLPing::init_sensor() void AP_RangeFinder_BLPing::init_sensor()
{ {
// request distance from sensor // Set message interval between pings in ms
send_message(BLDPIN_MSGID_DISTANCE_SIMPLE, nullptr, 0); 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 // distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload, uint16_t payload_len) 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) { if (uart == nullptr) {
return; return;
@ -83,27 +106,27 @@ void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload,
} }
// write header // write header
uart->write((uint8_t)BLPING_FRAME_HEADER1); uart->write(_frame_header1);
uart->write((uint8_t)BLPING_FRAME_HEADER2); uart->write(_frame_header2);
uint16_t crc = BLPING_FRAME_HEADER1 + BLPING_FRAME_HEADER2; uint16_t crc = _frame_header1 + _frame_header2;
// write payload length // write payload length
uart->write(LOWBYTE(payload_len)); uart->write(LOWBYTE(payload_len));
uart->write(HIGHBYTE(payload_len)); uart->write(HIGHBYTE(payload_len));
crc += LOWBYTE(payload_len) + HIGHBYTE(payload_len); crc += LOWBYTE(payload_len) + HIGHBYTE(payload_len);
// msgid // message id
uart->write(LOWBYTE(msgid)); uart->write(LOWBYTE(msg_id));
uart->write(HIGHBYTE(msgid)); uart->write(HIGHBYTE(msg_id));
crc += LOWBYTE(msgid) + HIGHBYTE(msgid); crc += LOWBYTE(msg_id) + HIGHBYTE(msg_id);
// src dev id // src dev id
uart->write((uint8_t)BLPING_SRC_ID); uart->write(_src_id);
crc += BLPING_SRC_ID; crc += _src_id;
// destination dev id // destination dev id
uart->write((uint8_t)BLPING_DEST_ID); uart->write(_dst_id);
crc += BLPING_DEST_ID; crc += _dst_id;
// payload // payload
if (payload != nullptr) { if (payload != nullptr) {
@ -118,105 +141,64 @@ void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload,
uart->write(HIGHBYTE(crc)); uart->write(HIGHBYTE(crc));
} }
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal PingProtocol::MessageId PingProtocol::parse_byte(uint8_t b)
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)
{
bool got_distance = false;
// process byte depending upon current state // process byte depending upon current state
switch (msg.state) { switch (msg.state) {
case ParseState::HEADER1: case ParserState::HEADER1:
if (b == BLPING_FRAME_HEADER1) { if (b == _frame_header1) {
msg.crc_expected = BLPING_FRAME_HEADER1; msg.crc_expected = _frame_header1;
msg.state = ParseState::HEADER2; msg.state = ParserState::HEADER2;
msg.done = false;
} }
break; break;
case ParseState::HEADER2: case ParserState::HEADER2:
if (b == BLPING_FRAME_HEADER2) { if (b == _frame_header2) {
msg.crc_expected += BLPING_FRAME_HEADER2; msg.crc_expected += _frame_header2;
msg.state = ParseState::LEN_L; msg.state = ParserState::LEN_L;
} else { } else {
msg.state = ParseState::HEADER1; msg.state = ParserState::HEADER1;
} }
break; break;
case ParseState::LEN_L: case ParserState::LEN_L:
msg.payload_len = b; msg.payload_len = b;
msg.crc_expected += b; msg.crc_expected += b;
msg.state = ParseState::LEN_H; msg.state = ParserState::LEN_H;
break; break;
case ParseState::LEN_H: case ParserState::LEN_H:
msg.payload_len |= ((uint16_t)b << 8); msg.payload_len |= ((uint16_t)b << 8);
msg.payload_recv = 0; msg.payload_recv = 0;
msg.crc_expected += b; msg.crc_expected += b;
msg.state = ParseState::MSG_ID_L; msg.state = ParserState::MSG_ID_L;
break; break;
case ParseState::MSG_ID_L: case ParserState::MSG_ID_L:
msg.msgid = b; msg.id = b;
msg.crc_expected += b; msg.crc_expected += b;
msg.state = ParseState::MSG_ID_H; msg.state = ParserState::MSG_ID_H;
break; break;
case ParseState::MSG_ID_H: case ParserState::MSG_ID_H:
msg.msgid |= ((uint16_t)b << 8); msg.id |= ((uint16_t)b << 8);
msg.crc_expected += b; msg.crc_expected += b;
msg.state = ParseState::SRC_ID; msg.state = ParserState::SRC_ID;
break; break;
case ParseState::SRC_ID: case ParserState::SRC_ID:
msg.crc_expected += b; msg.crc_expected += b;
msg.state = ParseState::DST_ID; msg.state = ParserState::DST_ID;
break; break;
case ParseState::DST_ID: case ParserState::DST_ID:
msg.crc_expected += b; msg.crc_expected += b;
msg.state = ParseState::PAYLOAD; msg.state = ParserState::PAYLOAD;
break; break;
case ParseState::PAYLOAD: case ParserState::PAYLOAD:
if (msg.payload_recv < msg.payload_len) { if (msg.payload_recv < msg.payload_len) {
if (msg.payload_recv < ARRAY_SIZE(msg.payload)) { if (msg.payload_recv < ARRAY_SIZE(msg.payload)) {
msg.payload[msg.payload_recv] = b; msg.payload[msg.payload_recv] = b;
@ -225,37 +207,22 @@ bool AP_RangeFinder_BLPing::parse_byte(uint8_t b)
msg.crc_expected += b; msg.crc_expected += b;
} }
if (msg.payload_recv == msg.payload_len) { if (msg.payload_recv == msg.payload_len) {
msg.state = ParseState::CRC_L; msg.state = ParserState::CRC_L;
} }
break; break;
case ParseState::CRC_L: case ParserState::CRC_L:
msg.crc = b; msg.crc = b;
msg.state = ParseState::CRC_H; msg.state = ParserState::CRC_H;
break; break;
case ParseState::CRC_H: case ParserState::CRC_H:
msg.crc |= ((uint16_t)b << 8); msg.crc |= ((uint16_t)b << 8);
msg.state = ParseState::HEADER1; msg.state = ParserState::HEADER1;
if (msg.crc_expected == msg.crc) { msg.done = 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;
}
}
break; break;
} }
return got_distance;
return msg.done ? get_message_id() : MessageId::INVALID;
} }

View File

@ -3,40 +3,82 @@
#include "AP_RangeFinder.h" #include "AP_RangeFinder.h"
#include "AP_RangeFinder_Backend_Serial.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: 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; /**
* @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);
// update state /**
void update(void) override; * @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: protected:
/**
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { * @brief State for the parser logic
return MAV_DISTANCE_SENSOR_ULTRASOUND; *
} */
enum class ParserState {
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 {
HEADER1 = 0, HEADER1 = 0,
HEADER2, HEADER2,
LEN_L, LEN_L,
@ -50,17 +92,81 @@ private:
CRC_H CRC_H
}; };
uint32_t last_init_ms; // system time that sensor was last initialised /**
uint16_t distance_cm; // latest distance * @brief Structure holding the last message available with its state
*
// structure holding latest message contents */
struct { 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 uint8_t payload[20]; // payload
uint16_t payload_len; // latest message payload length 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 payload_recv; // number of message's payload bytes received so far
uint16_t crc; // latest message's crc uint16_t crc; // latest message's crc
uint16_t crc_expected; // latest message's expected crc uint16_t crc_expected; // latest message's expected crc
} msg; } 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;
};