Skip to content

Commit

Permalink
AP_RangeFinder: BLPing: Rework class to work with new firmware versio…
Browse files Browse the repository at this point in the history 
…n 3.28

- Uses the continuous message request

Signed-off-by: Patrick José Pereira <[email protected]>
  • Loading branch information
@patrickelectric @jaxxzer
patrickelectric authored and jaxxzer committed Feb 11, 2020
1 parent 670ac12 commit a183d00
Show file tree
Hide file tree
Showing 2 changed files with 244 additions and 171 deletions.
239 changes: 103 additions & 136 deletions libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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();
}
Expand All @@ -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;
Expand All @@ -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) {
Expand All @@ -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;
Expand All @@ -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;
}
Loading

0 comments on commit a183d00

Please sign in to comment.