From adf05ea0e685c3c634fef0fbe8201ff02aa3a47b Mon Sep 17 00:00:00 2001 From: Marco Walther Date: Sat, 11 May 2024 18:29:27 -0700 Subject: [PATCH] AP_Proximity: LD06: correct data collection incorrect distances+angles being returned --- libraries/AP_Proximity/AP_Proximity_LD06.cpp | 98 +++++++++++--------- libraries/AP_Proximity/AP_Proximity_LD06.h | 10 +- 2 files changed, 61 insertions(+), 47 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp index 88d5f5b4f3..e18580fb6b 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -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 diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.h b/libraries/AP_Proximity/AP_Proximity_LD06.h index af8bd37edd..949e562404 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.h +++ b/libraries/AP_Proximity/AP_Proximity_LD06.h @@ -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