Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_RangeFinder_config.h"
       4             : 
       5             : #if AP_RANGEFINDER_LEDDARONE_ENABLED
       6             : 
       7             : #include "AP_RangeFinder.h"
       8             : #include "AP_RangeFinder_Backend_Serial.h"
       9             : 
      10             : // defines
      11             : #define LEDDARONE_DEFAULT_ADDRESS 0x01
      12             : #define LEDDARONE_MODOBUS_FUNCTION_CODE 0x04
      13             : #define LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS 20
      14             : #define LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER 10
      15             : 
      16             : #define LEDDARONE_SERIAL_PORT_MAX 250
      17             : #define LEDDARONE_READ_BUFFER_SIZE 25
      18             : 
      19             : #define LEDDARONE_DETECTIONS_MAX 3
      20             : #define LEDDARONE_DETECTION_DATA_NUMBER_INDEX 10
      21             : #define LEDDARONE_DETECTION_DATA_INDEX_OFFSET 11
      22             : #define LEDDARONE_DETECTION_DATA_OFFSET 4
      23             : 
      24             : // LeddarOne status
      25             : enum LeddarOne_Status {
      26             :     LEDDARONE_STATE_OK = 0,
      27             :     LEDDARONE_STATE_READING_BUFFER = 1,
      28             :     LEDDARONE_STATE_ERR_BAD_CRC = -1,
      29             :     LEDDARONE_STATE_ERR_NO_RESPONSES = -2,
      30             :     LEDDARONE_STATE_ERR_BAD_RESPONSE = -3,
      31             :     LEDDARONE_STATE_ERR_SHORT_RESPONSE = -4,
      32             :     LEDDARONE_STATE_ERR_SERIAL_PORT = -5,
      33             :     LEDDARONE_STATE_ERR_NUMBER_DETECTIONS = -6
      34             : };
      35             : 
      36             : // LeddarOne Modbus status
      37             : enum LeddarOne_ModbusStatus {
      38             :     LEDDARONE_MODBUS_STATE_INIT = 0,
      39             :     LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST,
      40             :     LEDDARONE_MODBUS_STATE_SENT_REQUEST,
      41             :     LEDDARONE_MODBUS_STATE_AVAILABLE
      42             : };
      43             : 
      44             : class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend_Serial
      45             : {
      46             : 
      47             : public:
      48             : 
      49           1 :     static AP_RangeFinder_Backend_Serial *create(
      50             :         RangeFinder::RangeFinder_State &_state,
      51             :         AP_RangeFinder_Params &_params) {
      52           1 :         return NEW_NOTHROW AP_RangeFinder_LeddarOne(_state, _params);
      53             :     }
      54             : 
      55             : protected:
      56             : 
      57         335 :     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
      58         335 :         return MAV_DISTANCE_SENSOR_LASER;
      59             :     }
      60             : 
      61             : private:
      62             : 
      63             :     using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
      64             : 
      65             :     // get a reading
      66             :     bool get_reading(float &reading_m) override;
      67             : 
      68             :     // CRC16
      69             :     bool CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck);
      70             : 
      71             :     // parse a response message from ModBus
      72             :     LeddarOne_Status parse_response(uint8_t &number_detections);
      73             : 
      74             :     uint32_t last_sending_request_ms;
      75             :     uint32_t last_available_ms;
      76             : 
      77             :     uint32_t sum_distance_mm;
      78             : 
      79             :     LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_STATE_INIT;
      80             :     uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];
      81             :     uint32_t read_len;
      82             : 
      83             :     // Modbus send request buffer
      84             :     // read input register (function code 0x04)
      85             :     const uint8_t send_request_buffer[8] = {
      86             :         LEDDARONE_DEFAULT_ADDRESS,
      87             :         LEDDARONE_MODOBUS_FUNCTION_CODE,
      88             :         0,
      89             :         LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS,   // 20: Address of first register to read
      90             :         0,
      91             :         LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER,        // 10: The number of consecutive registers to read
      92             :         0x30,   // CRC Lo
      93             :         0x09    // CRC Hi
      94             :     };
      95             : };
      96             : 
      97             : #endif  // AP_RANGEFINDER_LEDDARONE_ENABLED