You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
124 lines
4.0 KiB
124 lines
4.0 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
|
|
/* |
|
* ArduPilot device driver for SLAMTEC RPLIDAR A2 (16m range version) |
|
* |
|
* ALL INFORMATION REGARDING PROTOCOL WAS DERIVED FROM RPLIDAR DATASHEET: |
|
* |
|
* https://www.slamtec.com/en/Lidar |
|
* http://bucket.download.slamtec.com/63ac3f0d8c859d3a10e51c6b3285fcce25a47357/LR001_SLAMTEC_rplidar_protocol_v1.0_en.pdf |
|
* |
|
* Author: Steven Josefs, IAV GmbH |
|
* Based on the LightWare SF40C ArduPilot device driver from Randy Mackay |
|
* |
|
*/ |
|
|
|
|
|
#pragma once |
|
|
|
#include "AP_Proximity_Backend_Serial.h" |
|
|
|
#if HAL_PROXIMITY_ENABLED |
|
|
|
class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial |
|
{ |
|
|
|
public: |
|
|
|
using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial; |
|
|
|
// update state |
|
void update(void) override; |
|
|
|
// get maximum and minimum distances (in meters) of sensor |
|
float distance_max() const override; |
|
float distance_min() const override; |
|
|
|
private: |
|
enum rp_state { |
|
rp_unknown = 0, |
|
rp_resetted, |
|
rp_responding, |
|
rp_measurements, |
|
rp_health |
|
}; |
|
|
|
enum ResponseType { |
|
ResponseType_Descriptor = 0, |
|
ResponseType_SCAN, |
|
ResponseType_EXPRESS, |
|
ResponseType_Health |
|
}; |
|
|
|
// initialise sensor (returns true if sensor is successfully initialised) |
|
bool initialise(); |
|
void set_scan_mode(); |
|
|
|
// send request for something from sensor |
|
void send_request_for_health(); |
|
void parse_response_data(); |
|
void parse_response_descriptor(); |
|
void get_readings(); |
|
void reset_rplidar(); |
|
|
|
// reply related variables |
|
uint8_t _descriptor[7]; |
|
char _rp_systeminfo[63]; |
|
bool _descriptor_data; |
|
bool _information_data; |
|
bool _resetted; |
|
bool _initialised; |
|
|
|
uint8_t _payload_length; |
|
uint8_t _cnt; |
|
uint8_t _sync_error ; |
|
uint16_t _byte_count; |
|
|
|
// request related variables |
|
enum ResponseType _response_type; ///< response from the lidar |
|
enum rp_state _rp_state; |
|
uint32_t _last_request_ms; ///< system time of last request |
|
uint32_t _last_distance_received_ms; ///< system time of last distance measurement received from sensor |
|
uint32_t _last_reset_ms; |
|
|
|
// face related variables |
|
AP_Proximity_Boundary_3D::Face _last_face;///< last face requested |
|
float _last_angle_deg; ///< yaw angle (in degrees) of _last_distance_m |
|
float _last_distance_m; ///< shortest distance for _last_face |
|
bool _last_distance_valid; ///< true if _last_distance_m is valid |
|
|
|
struct PACKED _sensor_scan { |
|
uint8_t startbit : 1; ///< on the first revolution 1 else 0 |
|
uint8_t not_startbit : 1; ///< complementary to startbit |
|
uint8_t quality : 6; ///< Related the reflected laser pulse strength |
|
uint8_t checkbit : 1; ///< always set to 1 |
|
uint16_t angle_q6 : 15; ///< Actual heading = angle_q6/64.0 Degree |
|
uint16_t distance_q2 : 16; ///< Actual Distance = distance_q2/4.0 mm |
|
}; |
|
|
|
struct PACKED _sensor_health { |
|
uint8_t status; ///< status definition: 0 good, 1 warning, 2 error |
|
uint16_t error_code; ///< the related error code |
|
}; |
|
|
|
union PACKED { |
|
DEFINE_BYTE_ARRAY_METHODS |
|
_sensor_scan sensor_scan; |
|
_sensor_health sensor_health; |
|
} payload; |
|
}; |
|
|
|
#endif // HAL_PROXIMITY_ENABLED
|
|
|