Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_Proximity_config.h"
       4             : 
       5             : #if AP_PROXIMITY_CYGBOT_ENABLED
       6             : 
       7             : #include "AP_Proximity_Backend_Serial.h"
       8             : 
       9             : #define CYGBOT_MAX_MSG_SIZE            350
      10             : #define CYGBOT_PACKET_HEADER_0         0x5A
      11             : #define CYGBOT_PACKET_HEADER_1         0x77
      12             : #define CYGBOT_PACKET_HEADER_2         0xFF
      13             : #define CYGBOT_PAYLOAD_HEADER              0x01
      14             : #define CYGBOT_2D_START_ANGLE              -60.0f  // Starting 2-D horizontal angle of distances received in payload
      15             : #define CYGBOT_2D_ANGLE_STEP               0.75f   // Angle step size of each distance received. Starts from CYGBOT_2D_START_ANGLE
      16             : 
      17             : #define CYGBOT_TIMEOUT_MS              500    // Driver will report "unhealthy" if valid sensor readings not received within this many ms
      18             : #define CYGBOT_INIT_TIMEOUT_MS         1000   // Timeout this many ms after init
      19             : #define CYGBOT_MAX_RANGE_M             7.0f   // max range of the sensor in meters
      20             : #define CYGBOT_MIN_RANGE_M             0.2f   // min range of the sensor in meters
      21             : 
      22             : class AP_Proximity_Cygbot_D1 : public AP_Proximity_Backend_Serial
      23             : {
      24             : 
      25             : public:
      26             : 
      27             :     using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
      28             : 
      29             :     // update the state of the sensor
      30             :     void update(void) override;
      31             : 
      32             :     // get maximum and minimum distances (in meters) of sensor
      33           0 :     float distance_max_m() const override { return CYGBOT_MAX_RANGE_M; }
      34           0 :     float distance_min_m() const override { return CYGBOT_MIN_RANGE_M; }
      35             : 
      36             : private:
      37             : 
      38             :     // send message to the sensor to start streaming 2-D data
      39             :     void send_sensor_start();
      40             : 
      41             :     // read bytes from the sensor
      42             :     void read_sensor_data();
      43             : 
      44             :     // parse one byte from the sensor. Return false on error.
      45             :     bool parse_byte(uint8_t data);
      46             : 
      47             :     // parse payload, to pick out distances, and feed them to the correct faces
      48             :     void parse_payload();
      49             : 
      50             :     // Checksum
      51             :     uint8_t calc_checksum(uint8_t *buff, int buffSize);
      52             : 
      53             :     // reset all variables and flags
      54             :     void reset();
      55             : 
      56             :     // expected bytes from the sensor
      57             :     enum PacketList {
      58             :         Header1 = 0,
      59             :         Header2,
      60             :         Header3,
      61             :         Length1,
      62             :         Length2,
      63             :         Payload_Header,
      64             :         Payload_Data,
      65             :         CheckSum
      66             :     } _parse_state;
      67             : 
      68             :     struct {
      69             :         uint8_t payload_len_flags_low;          // low byte for payload size
      70             :         uint8_t payload_len_flags_high;         // high byte for payload size
      71             :         uint16_t payload_len;                   // latest message expected payload length
      72             :         uint16_t payload_counter;              // counter of the number of payload bytes received
      73             :         uint8_t payload[CYGBOT_MAX_MSG_SIZE];   // payload
      74             :     } _msg;
      75             : 
      76             :     bool _initialized;
      77             :     uint32_t _last_init_ms;                 // system time of last sensor init
      78             :     uint32_t _last_distance_received_ms;    // system time of last distance measurement received from sensor
      79             : 
      80             :     AP_Proximity_Temp_Boundary _temp_boundary; // temporary boundary to store incoming payload
      81             : 
      82             : };
      83             : 
      84             : #endif // AP_PROXIMITY_CYGBOT_ENABLED