Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_RangeFinder_config.h"
       4             : 
       5             : #if AP_RANGEFINDER_LEDDARVU8_ENABLED
       6             : 
       7             : #include "AP_RangeFinder.h"
       8             : #include "AP_RangeFinder_Backend_Serial.h"
       9             : 
      10             : #define LEDDARVU8_PAYLOAD_LENGTH (8*2)
      11             : 
      12             : class AP_RangeFinder_LeddarVu8 : public AP_RangeFinder_Backend_Serial
      13             : {
      14             : 
      15             : public:
      16             : 
      17           0 :     static AP_RangeFinder_Backend_Serial *create(
      18             :         RangeFinder::RangeFinder_State &_state,
      19             :         AP_RangeFinder_Params &_params) {
      20           0 :         return NEW_NOTHROW AP_RangeFinder_LeddarVu8(_state, _params);
      21             :     }
      22             : 
      23             : protected:
      24             : 
      25             :     // baudrate used during object construction:
      26           0 :     uint32_t initial_baudrate(uint8_t serial_instance) const override {
      27           0 :         return 115200;
      28             :     }
      29             : 
      30             :     // return sensor type as laser
      31           0 :     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
      32           0 :         return MAV_DISTANCE_SENSOR_LASER;
      33             :     }
      34             : 
      35             :     // get a reading, distance returned in reading_cm
      36             :     bool get_reading(float &reading_m) override;
      37             : 
      38             :     // maximum time between readings before we change state to NoData:
      39           0 :     uint16_t read_timeout_ms() const override { return 500; }
      40             : 
      41             : private:
      42             : 
      43             :     using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
      44             : 
      45             :     // function codes
      46             :     enum class FunctionCode : uint8_t {
      47             :         READ_HOLDING_REGISTER = 0x03,
      48             :         READ_INPUT_REGISTER = 0x04,
      49             :         WRITE_HOLDING_REGISTER = 0x06,
      50             :         WRITE_MULTIPLE_REGISTER = 0x10,
      51             :         READ_WRITE_MULTIPLE_REGISTER = 0x17
      52             :     };
      53             : 
      54             :     // register numbers for reading input registers
      55             :     enum class RegisterNumber : uint8_t {
      56             :         REGISTER_STATUS = 1,    // 0 = detections not ready, 1 = ready
      57             :         NUMBER_OF_SEGMENTS = 2,
      58             :         NUMBER_OF_DETECTIONS = 11,
      59             :         PERCENTAGE_OF_LIGHT_SOURCE_POWER = 12,
      60             :         TIMESTAMP_LOW = 14,
      61             :         TIMESTAMP_HIGH = 15,
      62             :         FIRST_DISTANCE0 = 16,   // distance of first detection for 1st segment, zero if no detection
      63             :         FIRST_AMPLITUDE0 = 24,  // amplitude of first detection * 64 for 1st segment
      64             :         FIRST_FLAG0 = 32,       // flags of first detection for 1st segment.  Bit0:Valid, Bit1:Result of object demerging, Bit2:Reserved, Bit3:Saturated
      65             :         // registers exist for distance, amplitude and flags for subsequent detections but are not used in this driver
      66             :     };
      67             : 
      68             :     // parsing state
      69             :     enum class ParseState : uint8_t {
      70             :         WAITING_FOR_ADDRESS,
      71             :         WAITING_FOR_FUNCTION_CODE,
      72             :         WAITING_FOR_PAYLOAD_LEN,
      73             :         WAITING_FOR_PAYLOAD,
      74             :         WAITING_FOR_CRC_LOW,
      75             :         WAITING_FOR_CRC_HIGH,
      76             :     };
      77             : 
      78             :     // get sensor address from RNGFNDx_ADDR parameter
      79             :     uint8_t get_sensor_address() const;
      80             : 
      81             :     // send request to device to provide distances
      82             :     void request_distances();
      83             : 
      84             :     // process one byte received on serial port
      85             :     // returns true if successfully parsed a message
      86             :     // if distances are valid, valid_readings is set to true and distance is stored in reading_cm
      87             :     bool parse_byte(uint8_t b, bool &valid_reading, uint16_t &reading_cm);
      88             : 
      89             :     // structure holding latest message contents
      90             :     // the order of fields matches the incoming message so it can be used to calculate the crc
      91             :     struct PACKED {
      92             :         uint8_t address;                            // device address (required for calculating crc)
      93             :         uint8_t function_code;                      // function code (always 0x04 but required for calculating crc)
      94             :         uint8_t payload_len;                        // message payload length
      95             :         uint8_t payload[LEDDARVU8_PAYLOAD_LENGTH];  // payload
      96             :         uint16_t crc;                               // latest message's crc
      97             :         uint16_t payload_recv;                      // number of message's payload bytes received so far
      98             :         ParseState state;                           // state of incoming message processing
      99             :     } parsed_msg;
     100             :     uint32_t last_distance_ms;                      // system time of last successful distance sensor read
     101             :     uint32_t last_distance_request_ms;              // system time of last request to sensor to send distances
     102             : };
     103             : 
     104             : #endif  // AP_RANGEFINDER_LEDDARVU8_ENABLED