Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /*
      17             :  * ArduPilot device driver for SLAMTEC RPLIDAR A2 (16m range version)
      18             :  *
      19             :  * ALL INFORMATION REGARDING PROTOCOL WAS DERIVED FROM RPLIDAR DATASHEET:
      20             :  *
      21             :  * https://www.slamtec.com/en/Lidar
      22             :  * http://bucket.download.slamtec.com/63ac3f0d8c859d3a10e51c6b3285fcce25a47357/LR001_SLAMTEC_rplidar_protocol_v1.0_en.pdf
      23             :  *
      24             :  * Author: Steven Josefs, IAV GmbH
      25             :  * Based on the LightWare SF40C ArduPilot device driver from Randy Mackay
      26             :  *
      27             :  */
      28             : 
      29             : /*
      30             : 
      31             : # to connect device to SITL:
      32             : ./Tools/autotest/sim_vehicle.py -v Rover --gdb --debug -A --serial5=uart:/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0:115200
      33             : param set SERIAL5_PROTOCOL 11
      34             : param set SERIAL5_BAUD 115200
      35             : param set PRX1_TYPE 5
      36             : reboot
      37             : 
      38             : # short outer-two wires on JST plug to get it to spin
      39             : 
      40             : */
      41             : 
      42             : 
      43             : #pragma once
      44             : 
      45             : #include "AP_Proximity_config.h"
      46             : 
      47             : #if AP_PROXIMITY_RPLIDARA2_ENABLED
      48             : 
      49             : #include "AP_Proximity_Backend_Serial.h"
      50             : 
      51             : class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
      52             : {
      53             : 
      54             : public:
      55             : 
      56             :     using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
      57             : 
      58             :     // update state
      59             :     void update(void) override;
      60             : 
      61             :     // get maximum and minimum distances (in meters) of sensor
      62             :     float distance_max_m() const override;
      63             :     float distance_min_m() const override;
      64             : 
      65             : private:
      66             : 
      67             :     enum class State {
      68             :         RESET = 56,
      69             :         AWAITING_RESPONSE,
      70             :         AWAITING_SCAN_DATA,
      71             :         AWAITING_HEALTH,
      72             :         AWAITING_DEVICE_INFO,
      73             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
      74             :         AWAITING_EXPRESS_DATA,
      75             : #endif
      76             :     } _state = State::RESET;
      77             : 
      78             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
      79             :     // Dense capsulated block size (EXPRESS/DENSE mode)
      80             :     static constexpr uint16_t EXPRESS_BLOCK_SIZE = 84;
      81             :     // Express/Dense capsule sync nibbles (upper 4 bits)
      82             :     static constexpr uint8_t EXPRESS_SYNC1 = 0x0A;
      83             :     static constexpr uint8_t EXPRESS_SYNC2 = 0x05;
      84             : 
      85             :     // S2 Dense Express is ~4 blocks/update at 200Hz; allow 2x headroom.
      86             :     static constexpr uint16_t EXPRESS_STREAM_BUFFER_SIZE = EXPRESS_BLOCK_SIZE * 8;
      87             :     static constexpr uint16_t EXPRESS_MAX_BYTES_CONSUME  = EXPRESS_BLOCK_SIZE * 8;
      88             : #endif
      89             : 
      90             :     // send request for something from sensor
      91             :     void send_request_for_health();
      92             :     void send_scan_mode_request();
      93             :     void send_request_for_device_info();
      94             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
      95             :     void send_express_scan_request();
      96             : #endif
      97             : 
      98             :     void parse_response_data();
      99             :     void parse_response_health();
     100             :     void parse_response_device_info();
     101             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
     102             :     void parse_response_express(const uint8_t *buf);
     103             : #endif
     104             : 
     105             :     void get_readings();
     106             :     void reset_rplidar();
     107             :     void reset();
     108             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
     109             :     void handle_express_data();
     110             :     bool express_time_exceeded(uint32_t start_us) const;
     111             :     void ensure_express_stream_space(uint16_t need);
     112             : #endif
     113             : 
     114             :     // remove bytes from read buffer:
     115             :     void consume_bytes(uint16_t count);
     116             : 
     117             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
     118             :     // verify Dense capsulated (Express) block checksum
     119             :     bool verify_cabin_checksum(const uint8_t *buf, size_t len);
     120             : #endif
     121             : 
     122             :     uint8_t _sync_error;
     123             :     uint16_t _byte_count;
     124             : 
     125             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
     126             :     uint8_t _express_stream[EXPRESS_STREAM_BUFFER_SIZE];
     127             :     uint16_t _express_stream_len;
     128             : #endif
     129             : 
     130             :     // request related variables
     131             :     uint32_t  _last_distance_received_ms;     ///< system time of last distance measurement received from sensor
     132             :     uint32_t  _last_reset_ms;
     133             : 
     134             :     // face related variables
     135             :     AP_Proximity_Boundary_3D::Face _last_face;///< last face requested
     136             :     float _last_angle_deg;                    ///< yaw angle (in degrees) of _last_distance_m
     137             :     float _last_distance_m;                   ///< shortest distance for _last_face
     138             :     bool _last_distance_valid;                ///< true if _last_distance_m is valid
     139             : 
     140             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
     141             :     // use Dense EXPRESS_SCAN path
     142             :     bool _use_dense_express = false;
     143             : #endif
     144             : 
     145             :     struct PACKED _device_info {
     146             :         uint8_t model;
     147             :         uint8_t firmware_minor;
     148             :         uint8_t firmware_major;
     149             :         uint8_t hardware;
     150             :         uint8_t serial[16];
     151             :    };
     152             : 
     153             :     struct PACKED _sensor_scan {
     154             :         uint8_t startbit      : 1;            ///< on the first revolution 1 else 0
     155             :         uint8_t not_startbit  : 1;            ///< complementary to startbit
     156             :         uint8_t quality       : 6;            ///< Related the reflected laser pulse strength
     157             :         uint8_t checkbit      : 1;            ///< always set to 1
     158             :         uint16_t angle_q6     : 15;           ///< Actual heading = angle_q6/64.0 Degree
     159             :         uint16_t distance_q2  : 16;           ///< Actual Distance = distance_q2/4.0 mm
     160             :     };
     161             : 
     162             :     struct PACKED _sensor_health {
     163             :         uint8_t status;                       ///< status definition: 0 good, 1 warning, 2 error
     164             :         uint16_t error_code;                  ///< the related error code
     165             :     };
     166             : 
     167             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
     168             :     // Express/Dense capsule sync header (2 bytes)
     169             :     struct PACKED _express_header {
     170             :         uint8_t sync1_checksum_low;   // upper nibble = 0xA
     171             :         uint8_t sync2_checksum_high;  // upper nibble = 0x5
     172             :     };
     173             : #endif
     174             : 
     175             :     struct PACKED _descriptor {
     176             :         uint8_t bytes[7];
     177             :     };
     178             : 
     179             :     // we don't actually *need* to store this.  If we don't, _payload
     180             :     // can be just 7 bytes, but that doesn't make for efficient
     181             :     // reading.  It also simplifies the state machine to have the read
     182             :     // buffer at least this big.  Note that we force the buffer to a
     183             :     // larger size below anyway.
     184             :     struct PACKED _rpi_information {
     185             :         uint8_t bytes[63];
     186             :     };
     187             : 
     188             :     union PACKED {
     189      147138 :         DEFINE_BYTE_ARRAY_METHODS
     190             :         _sensor_scan sensor_scan;
     191             :         _sensor_health sensor_health;
     192             :         _descriptor descriptor;
     193             :         _rpi_information information;
     194             :         _device_info device_info;
     195             : #if AP_PROXIMITY_RPLIDAR_EXPRESSSCAN_ENABLED
     196             :         _express_header express_header;
     197             : #endif
     198             :         uint8_t forced_buffer_size[256]; // just so we read(...) efficiently
     199             :     } _payload;
     200             :     static_assert(sizeof(_payload) >= 63, "Needed for parsing out reboot data");
     201             : 
     202             :     enum class Model {
     203             :         UNKNOWN,
     204             :         A1,
     205             :         A2,
     206             :         A2M12,
     207             :         C1,
     208             :         S1,
     209             :         S2,
     210             :     } model = Model::UNKNOWN;
     211             : 
     212             :     bool make_first_byte_in_payload(uint8_t desired_byte);
     213             : };
     214             : 
     215             : #endif // AP_PROXIMITY_RPLIDARA2_ENABLED