From a183d00b7ec2751f515e62d53cc9b0c46a9409d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Tue, 28 Jan 2020 15:41:25 -0300 Subject: [PATCH] AP_RangeFinder: BLPing: Rework class to work with new firmware version 3.28 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Uses the continuous message request Signed-off-by: Patrick José Pereira --- .../AP_RangeFinder/AP_RangeFinder_BLPing.cpp | 239 ++++++++---------- .../AP_RangeFinder/AP_RangeFinder_BLPing.h | 174 ++++++++++--- 2 files changed, 243 insertions(+), 170 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp index 34614ea899..6d3b6e690f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp @@ -17,36 +17,6 @@ #include #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(&ping_interval), sizeof(ping_interval)); + + // Send a message requesting a continuous + uint16_t continuous_message = static_cast(PingProtocol::MessageId::DISTANCE_SIMPLE); + protocol.send_message(uart, PingProtocol::MessageId::CONTINUOUS_START, reinterpret_cast(&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; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h index aaaa6c512c..ed906113dd 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h @@ -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(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; +};