Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_RangeFinder_config.h"
       4             : 
       5             : #if AP_RANGEFINDER_BLPING_ENABLED
       6             : 
       7             : #include "AP_RangeFinder_Backend_Serial.h"
       8             : 
       9             : /**
      10             :  * @brief Simple class to deal with Ping Protocol
      11             :  *
      12             :  *  Protocol documentation can be found here: https:* github.com/bluerobotics/ping-protocol
      13             :  *
      14             :  *  Byte     Type        Name            Description
      15             :  *  --------------------------------------------------------------------------------------------------------------
      16             :  *  0        uint8_t     start1          'B'
      17             :  *  1        uint8_t     start2          'R'
      18             :  *  2-3      uint16_t    payload_length  number of bytes in payload (low byte, high byte)
      19             :  *  4-5      uint16_t    message id      message id (low byte, high byte)
      20             :  *  6        uint8_t     src_device_id   id of device sending the message
      21             :  *  7        uint8_t     dst_device_id   id of device of the intended recipient
      22             :  *  8-n      uint8_t[]   payload         message payload
      23             :  *  (n+1)-(n+2)  uint16_t    checksum    the sum of all the non-checksum bytes in the message (low byte, high byte)
      24             :  */
      25             : class PingProtocol {
      26             :     static constexpr uint8_t _frame_header1 = 0x42; // // header first byte ('B')
      27             :     static constexpr uint8_t _frame_header2 = 0x52; // // header first byte ('R')
      28             :     static constexpr uint16_t _src_id = 0; // vehicle's source id
      29             :     static constexpr uint16_t _dst_id = 1; // sensor's id
      30             : 
      31             : public:
      32             :     enum class MessageId {
      33             :         INVALID = 0,
      34             :         SET_PING_INTERVAL = 1004,
      35             :         DISTANCE_SIMPLE = 1211,
      36             :         CONTINUOUS_START = 1400,
      37             :     };
      38             : 
      39             :     /**
      40             :      * @brief Process a single byte received on serial port
      41             :      *  return a valid MessageId if there is a message in buffer.
      42             :      *
      43             :      * @param byte
      44             :      * @return MessageId
      45             :      */
      46             :     MessageId parse_byte(uint8_t b);
      47             : 
      48             :     /**
      49             :      * @brief Send a message with a defined payload
      50             :      *
      51             :      * @param uart
      52             :      * @param msgid
      53             :      * @param payload
      54             :      * @param payload_len
      55             :      */
      56             :     void send_message(AP_HAL::UARTDriver *uart, PingProtocol::MessageId msg_id, const uint8_t *payload, uint16_t payload_len) const;
      57             : 
      58             :     /**
      59             :      * @brief Get distance from message
      60             :      *
      61             :      * @return uint32_t
      62             :      */
      63             :     uint32_t get_distance_mm() const;
      64             : 
      65             :     /**
      66             :      * @brief Get confidence from message
      67             :      *
      68             :      * @return uint8_t
      69             :      */
      70             :     uint8_t get_confidence() const;
      71             : 
      72             :     /**
      73             :      * @brief Get the message id available in bufffer
      74             :      *
      75             :      * @return MessageId
      76             :      */
      77         344 :     MessageId get_message_id() const { return static_cast<MessageId>(msg.id); };
      78             : 
      79             : protected:
      80             :     /**
      81             :      * @brief State for the parser logic
      82             :      *
      83             :      */
      84             :     enum class ParserState {
      85             :         HEADER1 = 0,
      86             :         HEADER2,
      87             :         LEN_L,
      88             :         LEN_H,
      89             :         MSG_ID_L,
      90             :         MSG_ID_H,
      91             :         SRC_ID,
      92             :         DST_ID,
      93             :         PAYLOAD,
      94             :         CRC_L,
      95             :         CRC_H
      96             :     };
      97             : 
      98             :     /**
      99             :      * @brief Structure holding the last message available with its state
     100             :      *
     101             :      */
     102             :     struct {
     103             :         ParserState state;      // state of incoming message processing
     104             :         bool done;              // inform if the message is complete or not
     105             :         uint8_t payload[20];    // payload
     106             :         uint16_t payload_len;   // latest message payload length
     107             :         uint16_t id;            // latest message's message id
     108             :         uint16_t payload_recv;  // number of message's payload bytes received so far
     109             :         uint16_t crc;           // latest message's crc
     110             :         uint16_t crc_expected;  // latest message's expected crc
     111             :     } msg;
     112             : };
     113             : 
     114             : /**
     115             :  * @brief Class for Blue Robotics Ping1D sensor
     116             :  *
     117             :  */
     118             : class AP_RangeFinder_BLPing : public AP_RangeFinder_Backend_Serial
     119             : {
     120             :     static constexpr uint16_t _sensor_rate_ms = 50; // initialise sensor at no more than 20hz
     121             : 
     122             : public:
     123             : 
     124           1 :     static AP_RangeFinder_Backend_Serial *create(
     125             :         RangeFinder::RangeFinder_State &_state,
     126             :         AP_RangeFinder_Params &_params) {
     127           1 :         return NEW_NOTHROW AP_RangeFinder_BLPing(_state, _params);
     128             :     }
     129             : 
     130             :     /**
     131             :      * @brief Update class state
     132             :      *
     133             :      */
     134             :     void update(void) override;
     135             : 
     136             :     /**
     137             :      * @brief Get the reading confidence
     138             :      * 100 is best quality, 0 is worst
     139             :      *
     140             :      */
     141             :     int8_t get_signal_quality_pct() const override WARN_IF_UNUSED;
     142             : 
     143             : protected:
     144             :     /**
     145             :      * @brief Return the sensor type
     146             :      *
     147             :      * @return MAV_DISTANCE_SENSOR
     148             :      */
     149         339 :     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
     150         339 :         return MAV_DISTANCE_SENSOR_ULTRASOUND;
     151             :     }
     152             : 
     153             :     /**
     154             :      * @brief Sensor protocol class
     155             :      *
     156             :      */
     157             :     PingProtocol protocol;
     158             : 
     159             : private:
     160             : 
     161             :     using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
     162             : 
     163             :     /**
     164             :      * @brief Do the necessary sensor initiation
     165             :      *
     166             :      */
     167             :     void init_sensor();
     168             : 
     169             :     /**
     170             :      * @brief Read serial interface and calculate new distance
     171             :      *
     172             :      * @param reading_m
     173             :      * @return true
     174             :      * @return false
     175             :      */
     176             :     bool get_reading(float &reading_m) override;
     177             : 
     178             :     /**
     179             :      * @brief Timeout between messages
     180             :      *
     181             :      * @return uint16_t
     182             :      */
     183        1016 :     uint16_t read_timeout_ms() const override { return 1000; }
     184             : 
     185             :     /**
     186             :      * @brief system time that sensor was last initialised
     187             :      *
     188             :      */
     189             :     uint32_t last_init_ms;
     190             : };
     191             : 
     192             : #endif