Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-20 14:54:09 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: LD06: correct data collection

Browse Source 
incorrect distances+angles being returned
This commit is contained in:
Marco Walther 2024-05-11 18:29:27 -07:00 committed by Peter Barker
parent c57672a529
commit adf05ea0e6
2 changed files with 61 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
libraries/AP_Proximity/AP_Proximity_LD06.cpp
View File

@ -121,15 +121,10 @@ void AP_Proximity_LD06::get_readings()
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;
}
_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;
@ -148,12 +143,6 @@ void AP_Proximity_LD06::parse_response_data()
// 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];
@ -161,49 +150,68 @@ void AP_Proximity_LD06::parse_response_data()
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;
// 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;
// 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);
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);
}
// 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;
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;
// 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;
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;
}
sampled_counts ++;
distance_avg += distance_meas;
// 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;
}
}
// 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_LD06_ENABLED

10
libraries/AP_Proximity/AP_Proximity_LD06.h
View File

@ -62,7 +62,13 @@ private:
// Store for error-tracking purposes
uint32_t _last_distance_received_ms;
// Boundary to store the measurements
AP_Proximity_Temp_Boundary _temp_boundary;
// 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
uint16_t _angle_2deg;
float _dist_2deg_m;
};
#endif // AP_PROXIMITY_LD06_ENABLED