mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-19 15:18:28 -04:00
220 lines
8.1 KiB
C++
220 lines
8.1 KiB
C++
/*
|
|
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 Inno-Maker LD06 LiDAR
|
|
*
|
|
* ALL INFORMATION REGARDING PROTOCOL WAS DERIVED FROM InnoMaker DATASHEET:
|
|
*
|
|
* http://wiki.inno-maker.com/display/HOMEPAGE/LD06?preview=/6949506/6949511/LDROBOT_LD06_Development%20manual_v1.0_en.pdf
|
|
*
|
|
* Author: Adithya Patil, Georgia Institute of Technology
|
|
* Based on the SLAMTEC RPLiDAR code written by Steven Josefs, IAV GmbH and CYGBOT D1 LiDAR code
|
|
*
|
|
*/
|
|
|
|
#include "AP_Proximity_config.h"
|
|
|
|
#if AP_PROXIMITY_LD06_ENABLED
|
|
#include "AP_Proximity_LD06.h"
|
|
|
|
#define LD_START_CHAR 0x54
|
|
#define PROXIMITY_LD06_TIMEOUT_MS 50
|
|
|
|
// Indices in data array where each value starts being recorded
|
|
// See comment below about data payload for more info about formatting
|
|
#define START_BEGIN_CHARACTER 0
|
|
#define START_DATA_LENGTH 1
|
|
#define START_RADAR_SPEED 2
|
|
#define START_BEGIN_ANGLE 4
|
|
#define START_PAYLOAD 6
|
|
#define START_END_ANGLE 42
|
|
#define START_CHECK_SUM 46
|
|
#define MEASUREMENT_PAYLOAD_LENGTH 3
|
|
#define PAYLOAD_COUNT 12
|
|
|
|
/* ------------------------------------------
|
|
Data Packet Structure:
|
|
Start Character : 1 Byte
|
|
Data Length : 1 Byte
|
|
Radar Speed : 2 Bytes
|
|
Start Angle : 2 Bytes
|
|
Data Measurements : 36 Bytes
|
|
Contains 12 measurements of 3 Bytes each
|
|
Each measurement has 2 Bytes for distance to closest object
|
|
Each measurement has the 3rd Byte as measurement Confidence
|
|
End Angle : 2 Bytes
|
|
Timestamp : 2 Bytes
|
|
Checksum : 1 Byte
|
|
------------------------------------------ */
|
|
// ----> 47 data bytes in total for one packet
|
|
|
|
// Update the sensor readings
|
|
void AP_Proximity_LD06::update(void)
|
|
{
|
|
// Escape if no connection detected/supported while running
|
|
if (_uart == nullptr) {
|
|
return;
|
|
}
|
|
|
|
// Begin getting sensor readings
|
|
// Calls method that repeatedly reads through UART channel
|
|
get_readings();
|
|
|
|
// Check if the data is being received correctly and sets Proximity Status
|
|
if (_last_distance_received_ms == 0 || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_LD06_TIMEOUT_MS)) {
|
|
set_status(AP_Proximity::Status::NoData);
|
|
} else {
|
|
set_status(AP_Proximity::Status::Good);
|
|
}
|
|
}
|
|
|
|
// Called repeatedly to get the readings at the current instant
|
|
void AP_Proximity_LD06::get_readings()
|
|
{
|
|
if (_uart == nullptr) {
|
|
return;
|
|
}
|
|
|
|
// Store the number of bytes available on the UART input
|
|
uint32_t nbytes = MIN((uint16_t) 4000, _uart->available());
|
|
|
|
// Loops through all bytes that were received
|
|
while (nbytes-- > 0) {
|
|
|
|
uint8_t c;
|
|
if (!_uart->read(c)) {
|
|
break;
|
|
}
|
|
|
|
// Stores the byte in an array if the byte is a start byte or we have already read a start byte
|
|
if (c == LD_START_CHAR || _byte_count) {
|
|
// Stores the next byte in an array
|
|
_response[_byte_count] = c;
|
|
if (_byte_count < START_DATA_LENGTH) {
|
|
_byte_count++;
|
|
continue;
|
|
}
|
|
|
|
// total_packet_length = sizeof(header) + datalength + sizeof(footer):
|
|
const uint32_t total_packet_length = 6 + 3*_response[START_DATA_LENGTH] + 5;
|
|
if (_response[START_DATA_LENGTH] != PAYLOAD_COUNT ||
|
|
total_packet_length > ARRAY_SIZE(_response)) {
|
|
// invalid packet received; throw away all data and
|
|
// start again.
|
|
_byte_count = 0;
|
|
_uart->discard_input();
|
|
break;
|
|
}
|
|
|
|
_byte_count++;
|
|
|
|
if (_byte_count == total_packet_length) {
|
|
|
|
const uint32_t current_ms = AP_HAL::millis();
|
|
|
|
_last_distance_received_ms = current_ms;
|
|
|
|
// Updates the temporary boundary and passes off the completed data
|
|
parse_response_data();
|
|
|
|
// Resets the bytes read and whether or not we are reading data to accept a new payload
|
|
_byte_count = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Parses the data packet received from the LiDAR
|
|
void AP_Proximity_LD06::parse_response_data()
|
|
{
|
|
|
|
// Data interpretation based on:
|
|
// http://wiki.inno-maker.com/display/HOMEPAGE/LD06?preview=/6949506/6949511/LDROBOT_LD06_Development%20manual_v1.0_en.pdf
|
|
|
|
// Second byte in array stores length of data - not used but stored for debugging
|
|
// const uint8_t data_length = _response[START_DATA_LENGTH];
|
|
|
|
// Verify the checksum that is stored in the last element of the response array
|
|
// Return if checksum is incorrect - i.e. bad data, bad readings, etc.
|
|
const uint8_t check_sum = _response[START_CHECK_SUM];
|
|
if (check_sum != crc8_generic(&_response[0], sizeof(_response) / sizeof(_response[0]) - 1, 0x4D)) {
|
|
return;
|
|
}
|
|
|
|
// Respective bits store the radar speed, start/end angles
|
|
// Use bitwise operations to correctly obtain correct angles
|
|
// Divide angles by 100 as per manual
|
|
const float start_angle = float(UINT16_VALUE(_response[START_BEGIN_ANGLE + 1], _response[START_BEGIN_ANGLE])) * 0.01;
|
|
const float end_angle = float(UINT16_VALUE(_response[START_END_ANGLE + 1], _response[START_END_ANGLE])) * 0.01;
|
|
|
|
float angle_step;
|
|
if (start_angle < end_angle) {
|
|
angle_step = (end_angle - start_angle) / (PAYLOAD_COUNT - 1);
|
|
} else {
|
|
angle_step = (end_angle + 360 - start_angle) / (PAYLOAD_COUNT - 1);
|
|
}
|
|
// Each recording point is three bytes long, goes through all of that and updates database
|
|
for (uint16_t i = START_PAYLOAD; i < START_PAYLOAD + MEASUREMENT_PAYLOAD_LENGTH * PAYLOAD_COUNT; i += MEASUREMENT_PAYLOAD_LENGTH) {
|
|
|
|
// Gets the distance recorded and converts to meters
|
|
const float angle_deg = correct_angle_for_orientation(start_angle + angle_step * (i / MEASUREMENT_PAYLOAD_LENGTH));
|
|
const float distance_m = UINT16_VALUE(_response[i + 1], _response[i]) * 0.001;
|
|
|
|
if (distance_m < distance_min() || distance_m > distance_max() || _response[i + 2] < 20) { // XXX 20 good?
|
|
continue;
|
|
}
|
|
if (ignore_reading(angle_deg, distance_m)) {
|
|
continue;
|
|
}
|
|
|
|
uint16_t a2d = (int)(angle_deg / 2.0) * 2;
|
|
if (_angle_2deg == a2d) {
|
|
if (distance_m < _dist_2deg_m) {
|
|
_dist_2deg_m = distance_m;
|
|
}
|
|
} else {
|
|
// New 2deg angle, record the old one
|
|
|
|
const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face((float)_angle_2deg);
|
|
|
|
if (face != _last_face) {
|
|
// distance is for a new face, the previous one can be updated now
|
|
if (_last_distance_valid) {
|
|
frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance);
|
|
} else {
|
|
// reset distance from last face
|
|
frontend.boundary.reset_face(face, state.instance);
|
|
}
|
|
|
|
// initialize the new face
|
|
_last_face = face;
|
|
_last_distance_valid = false;
|
|
}
|
|
|
|
// update shortest distance
|
|
if (!_last_distance_valid || (_dist_2deg_m < _last_distance_m)) {
|
|
_last_distance_m = _dist_2deg_m;
|
|
_last_distance_valid = true;
|
|
_last_angle_deg = (float)_angle_2deg;
|
|
}
|
|
// update OA database
|
|
database_push(_last_angle_deg, _last_distance_m);
|
|
|
|
_angle_2deg = a2d;
|
|
_dist_2deg_m = distance_m;
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_PROXIMITY_LD06_ENABLED
|