mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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
Jacob Walser
parent
670ac124c0
commit
a183d00b7e
@ -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)
|
||||
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)
|
||||
|
||||
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,
|
||||
}
|
||||
|
||||
// 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,
|
||||
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)
|
||||
PingProtocol::MessageId PingProtocol::parse_byte(uint8_t b)
|
||||
{
|
||||
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
|
||||
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)
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
/**
|
||||
* @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:
|
||||
|
||||
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:
|
||||
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;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user