Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_Proximity_config.h"
       4             : 
       5             : #if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
       6             : #include "AP_Proximity_Backend_Serial.h"
       7             : #include <AP_LightWareSerial/AP_LightWareSerial.h>
       8             : 
       9             : #include <Filter/Filter.h>
      10             : 
      11             : class AP_Proximity_LightWareSF45B : public AP_Proximity_Backend_Serial, public AP_LightWareSerial
      12             : {
      13             : 
      14             : public:
      15             :     using AP_Proximity_Backend_Serial::_uart; // needed to disambiguate _uart in both base classes and avoid compiler error
      16             : 
      17             :     // constructor
      18           1 :     AP_Proximity_LightWareSF45B(AP_Proximity &_frontend,
      19             :                                 AP_Proximity::Proximity_State &_state,
      20             :                                 AP_Proximity_Params& _params,
      21           1 :                                 uint8_t serial_instance) :
      22             :             AP_Proximity_Backend_Serial(_frontend, _state, _params, serial_instance),
      23           1 :             AP_LightWareSerial(AP_Proximity_Backend_Serial::_uart)
      24             :     {
      25           1 :     }
      26             : 
      27           0 :     uint16_t rxspace() const override {
      28           0 :         return 1280;
      29             :     };
      30             : 
      31             :     // update state
      32             :     void update(void) override;
      33             : 
      34             :     // get maximum and minimum distances (in meters) of sensor
      35        5546 :     float distance_max_m() const override { return 50.0f; }
      36        5731 :     float distance_min_m() const override { return 0.20f; }
      37             : 
      38             : private:
      39             : 
      40             :     // message ids
      41             :     enum class MessageID : uint8_t {
      42             :         PRODUCT_NAME = 0,
      43             :         HARDWARE_VERSION = 1,
      44             :         FIRMWARE_VERSION = 2,
      45             :         SERIAL_NUMBER = 3,
      46             :         TEXT_MESSAGE = 7,
      47             :         USER_DATA = 9,
      48             :         TOKEN = 10,
      49             :         SAVE_PARAMETERS = 12,
      50             :         RESET = 14,
      51             :         STAGE_FIRMWARE = 16,
      52             :         COMMIT_FIRMWARE = 17,
      53             :         DISTANCE_OUTPUT = 27,
      54             :         STREAM = 30,
      55             :         DISTANCE_DATA_CM = 44,
      56             :         DISTANCE_DATA_MM = 45,
      57             :         LASER_FIRING = 50,
      58             :         TEMPERATURE = 57,
      59             :         UPDATE_RATE = 66,
      60             :         NOISE = 74,
      61             :         ZERO_OFFSET = 75,
      62             :         LOST_SIGNAL_COUNTER = 76,
      63             :         BAUD_RATE = 79,
      64             :         I2C_ADDRESS = 80,
      65             :         STEPPER_STATUS = 93,
      66             :         SCAN_ON_STARTUP = 94,
      67             :         SCAN_ENABLE = 96,
      68             :         SCAN_POSITION = 97,
      69             :         SCAN_LOW_ANGLE = 98,
      70             :         SCAN_HIGH_ANGLE = 99
      71             :     };
      72             : 
      73             :     // initialise sensor
      74             :     void initialise();
      75             : 
      76             :     // request start of streaming of distances
      77             :     void request_stream_start();
      78             : 
      79             :     // check and process replies from sensor
      80             :     void process_replies();
      81             : 
      82             :     // process the latest message held in the msg structure
      83             :     void process_message();
      84             : 
      85             :     // convert an angle (in degrees) to a mini sector number
      86             :     uint8_t convert_angle_to_minisector(float angle_deg) const;
      87             : 
      88             :     // internal variables
      89             :     uint32_t _last_init_ms;                 // system time of last re-initialisation
      90             :     uint32_t _last_distance_received_ms;    // system time of last distance measurement received from sensor
      91             :     bool _init_complete;                    // true once sensor initialisation is complete
      92             :     ModeFilterInt16_Size3 _distance_filt{1};// mode filter to reduce glitches
      93             : 
      94             :     // 3D boundary face and distance for latest readings
      95             :     AP_Proximity_Boundary_3D::Face _face;   // face of most recently received distance
      96             :     float _face_distance;                   // shortest distance (in meters) on face
      97             :     float _face_yaw_deg;                    // yaw angle (in degrees) of shortest distance on face
      98             :     bool _face_distance_valid;              // true if face has at least one valid distance
      99             : 
     100             :     // mini sector (5 degrees) angles and distances (used to populate obstacle database for path planning)
     101             :     uint8_t _minisector = UINT8_MAX;        // mini sector number (from 0 to 71) of most recently received distance
     102             :     float _minisector_distance;             // shortest distance (in meters) in mini sector
     103             :     float _minisector_angle;                // angle (in degrees) of shortest distance in mini sector
     104             :     bool _minisector_distance_valid;        // true if mini sector has at least one valid distance
     105             : 
     106             :     // state of sensor
     107             :     struct {
     108             :         uint8_t update_rate;        // sensor reported update rate enum from UPDATE_RATE message
     109             :         uint32_t streaming_fields;  // sensor reported bitmask of fields sent in DISTANCE_DATA_CM message
     110             :         uint32_t stream_data_type;  // sensor reported stream value.  5 if DISTANCE_DATA_CM messages are being streamed
     111             :     } _sensor_state;
     112             : 
     113             : };
     114             : 
     115             : #endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED