-
Notifications
You must be signed in to change notification settings - Fork 17.1k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
4957fc3
commit d634155
Showing
5 changed files
with
237 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,174 @@ | ||
|
||
#include "AP_Proximity_config.h" | ||
|
||
#if AP_PROXIMITY_LD19_ENABLED | ||
#include "AP_Proximity_LD19.h" | ||
|
||
#define LD_START_CHAR 0x54 | ||
#define PROXIMITY_LD19_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_LD19::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_LD19_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_LD19::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) { | ||
|
||
// Gets and logs the current byte being read | ||
const uint8_t c = _uart->read(); | ||
|
||
// 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 || _response_data) { | ||
|
||
// Sets to true if a start byte has been read, default false otherwise | ||
_response_data = true; | ||
|
||
// Stores the next byte in an array | ||
_response[_byte_count] = c; | ||
_byte_count++; | ||
|
||
if (_byte_count == _response[START_DATA_LENGTH] + 3) { | ||
|
||
const uint32_t current_ms = AP_HAL::millis(); | ||
|
||
// Stores the last distance taken, used to reduce number of readings taken | ||
if (_last_distance_received_ms != current_ms) { | ||
_last_distance_received_ms = current_ms; | ||
} | ||
|
||
// Updates the temporary boundary and passes off the completed data | ||
parse_response_data(); | ||
_temp_boundary.update_3D_boundary(state.instance, frontend.boundary); | ||
_temp_boundary.reset(); | ||
|
||
// Resets the bytes read and whether or not we are reading data to accept a new payload | ||
_byte_count = 0; | ||
_response_data = false; | ||
} | ||
} | ||
} | ||
} | ||
|
||
// Parses the data packet received from the LiDAR | ||
void AP_Proximity_LD19::parse_response_data() | ||
{ | ||
|
||
// Data interpretation based on: | ||
// http://wiki.inno-maker.com/display/HOMEPAGE/LD19?preview=/6949506/6949511/LDROBOT_LD19_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]; | ||
|
||
// 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; | ||
|
||
// 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; | ||
} | ||
|
||
// Calculates the angle that this point was sampled at | ||
float sampled_counts = 0; | ||
const float angle_step = (end_angle - start_angle) / (PAYLOAD_COUNT - 1); | ||
float uncorrected_angle = start_angle + (end_angle - start_angle) * 0.5; | ||
|
||
// Handles the case that the angles read went from 360 to 0 (jumped) | ||
if (angle_step < 0) { | ||
uncorrected_angle = wrap_360(start_angle + (end_angle + 360 - start_angle) * 0.5); | ||
} | ||
|
||
// Takes the angle in the middle of the readings to be pushed to the database | ||
const float push_angle = correct_angle_for_orientation(uncorrected_angle); | ||
|
||
float distance_avg = 0.0; | ||
|
||
// 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 distance_meas = UINT16_VALUE(_response[i + 1], _response[i]) * 0.001; | ||
|
||
// Validates data and checks if it should be included | ||
if (distance_meas > distance_min() && distance_meas < distance_max()) { | ||
if (ignore_reading(push_angle, distance_meas)) { | ||
continue; | ||
} | ||
|
||
sampled_counts ++; | ||
distance_avg += distance_meas; | ||
} | ||
} | ||
|
||
// Convert angle to appropriate face and adds to database | ||
// Since angle increments are only about 3 degrees, ignore readings if there were only 1 or 2 measurements | ||
// (likely outliers) recorded in the range | ||
if (sampled_counts > 2) { | ||
// Gets the average distance read | ||
distance_avg /= sampled_counts; | ||
|
||
// Pushes the average distance and angle to the obstacle avoidance database | ||
const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(push_angle); | ||
_temp_boundary.add_distance(face, push_angle, distance_avg); | ||
database_push(push_angle, distance_avg); | ||
} | ||
} | ||
#endif // AP_PROXIMITY_LD19_ENABLED |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,44 @@ | ||
|
||
#pragma once | ||
#include "AP_Proximity_config.h" | ||
|
||
#if AP_PROXIMITY_LD19_ENABLED | ||
#include "AP_Proximity_Backend_Serial.h" | ||
|
||
#define MESSAGE_LENGTH_LD19 47 | ||
|
||
// Minimum and maximum distance that the sensor can read in meters | ||
#define MAX_READ_DISTANCE_LD19 12.0f | ||
#define MIN_READ_DISTANCE_LD19 0.02f | ||
|
||
class AP_Proximity_LD19 : public AP_Proximity_Backend_Serial | ||
{ | ||
public: | ||
|
||
using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial; | ||
|
||
// Update the state of the sensor | ||
void update(void) override; | ||
|
||
// Get the max and min distances for the sensor being used | ||
float distance_max() const override { return MAX_READ_DISTANCE_LD19; } | ||
float distance_min() const override { return MIN_READ_DISTANCE_LD19; } | ||
|
||
private: | ||
|
||
// Get and parse the sensor data | ||
void parse_response_data(); | ||
void get_readings(); | ||
|
||
// Store and keep track of the bytes being read from the sensor | ||
uint8_t _response[MESSAGE_LENGTH_LD19]; | ||
bool _response_data; | ||
uint16_t _byte_count; | ||
|
||
// Store for error-tracking purposes | ||
uint32_t _last_distance_received_ms; | ||
|
||
// Boundary to store the measurements | ||
AP_Proximity_Temp_Boundary _temp_boundary; | ||
}; | ||
#endif // AP_PROXIMITY_LD19_ENABLED |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters