From e7db5be1d774e8ac42bc67dd92f1605458184780 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 May 2025 12:52:41 +0900 Subject: [PATCH 1/5] AP_Proximity: LD06 comment and definition improvements add confidence threshold definition remove unused definitions --- libraries/AP_Proximity/AP_Proximity_LD06.cpp | 22 +++++++++++++------- libraries/AP_Proximity/AP_Proximity_LD06.h | 1 + 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp index 428445c08b078..a249901bca7a7 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -33,9 +33,7 @@ // 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 @@ -43,6 +41,9 @@ #define MEASUREMENT_PAYLOAD_LENGTH 3 #define PAYLOAD_COUNT 12 +// confidence for each measurement must be this value or higher +#define CONFIDENCE_THRESHOLD 20 + /* ------------------------------------------ Data Packet Structure: Start Character : 1 Byte @@ -170,24 +171,29 @@ void AP_Proximity_LD06::parse_response_data() // 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; + const float confidence = _response[i + 2]; - if (distance_m < distance_min() || distance_m > distance_max() || _response[i + 2] < 20) { // XXX 20 good? + // ignore distance that are out-of-range or have low confidence + if (distance_m < distance_min() || distance_m > distance_max() || confidence < CONFIDENCE_THRESHOLD) { continue; } + + // ignore distances that are out-of-range (based on user parameters) or within ignore areas if (ignore_reading(angle_deg, distance_m)) { continue; } - uint16_t a2d = (int)(angle_deg / 2.0) * 2; + // use the shortest distance within 2 degree sectors + const 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 + // new 2 degree sector, process the old one + // check if new sector is also a new face 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) { @@ -202,15 +208,17 @@ void AP_Proximity_LD06::parse_response_data() _last_distance_valid = false; } - // update shortest distance + // update face's 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); + // advance to the next 2 degree sector _angle_2deg = a2d; _dist_2deg_m = distance_m; } diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.h b/libraries/AP_Proximity/AP_Proximity_LD06.h index 949e56240437b..1d4345c1aa255 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.h +++ b/libraries/AP_Proximity/AP_Proximity_LD06.h @@ -68,6 +68,7 @@ class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial float _last_distance_m; ///< shortest distance for _last_face bool _last_distance_valid; ///< true if _last_distance_m is valid + // angle and distance for the latest 2 degree sector uint16_t _angle_2deg; float _dist_2deg_m; }; From f671f652304c51a539e89d78bcfc7a6695f21b2b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 May 2025 13:08:19 +0900 Subject: [PATCH 2/5] Filter: define mode filter for uint16 --- libraries/Filter/ModeFilter.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/Filter/ModeFilter.cpp b/libraries/Filter/ModeFilter.cpp index 25fac4304c34a..878b5222e07c6 100644 --- a/libraries/Filter/ModeFilter.cpp +++ b/libraries/Filter/ModeFilter.cpp @@ -99,3 +99,4 @@ void ModeFilter::isort(T new_sample, bool drop_high) template class ModeFilter; template class ModeFilter; template class ModeFilter; +template class ModeFilter; From 81f5007854f7de0cf9df3642a3b4851d584380ff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 May 2025 12:55:43 +0900 Subject: [PATCH 3/5] AP_Proximity: LD06 gets mode filter --- libraries/AP_Proximity/AP_Proximity_LD06.cpp | 2 +- libraries/AP_Proximity/AP_Proximity_LD06.h | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp index a249901bca7a7..e5b2238ebedb2 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -170,7 +170,7 @@ void AP_Proximity_LD06::parse_response_data() // 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; + const float distance_m = _dist_filt_mm.apply(UINT16_VALUE(_response[i + 1], _response[i])) * 0.001; const float confidence = _response[i + 2]; // ignore distance that are out-of-range or have low confidence diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.h b/libraries/AP_Proximity/AP_Proximity_LD06.h index 1d4345c1aa255..3d4c98db7dbf6 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.h +++ b/libraries/AP_Proximity/AP_Proximity_LD06.h @@ -29,6 +29,7 @@ #if AP_PROXIMITY_LD06_ENABLED #include "AP_Proximity_Backend_Serial.h" +#include #define MESSAGE_LENGTH_LD06 47 @@ -62,6 +63,9 @@ class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial // Store for error-tracking purposes uint32_t _last_distance_received_ms; + // distance filter applies to raw measurements + ModeFilterUInt16_Size3 _dist_filt_mm {1}; + // 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 From c364958438032b0301200de838b973f96b94d309 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 May 2025 12:57:40 +0900 Subject: [PATCH 4/5] AP_Proximity: LD06 updates OA database with 2deg sector dist --- libraries/AP_Proximity/AP_Proximity_LD06.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp index e5b2238ebedb2..22dad642a5731 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -215,8 +215,8 @@ void AP_Proximity_LD06::parse_response_data() _last_angle_deg = (float)_angle_2deg; } - // update OA database - database_push(_last_angle_deg, _last_distance_m); + // update OA database with the 2 degree sectors distance + database_push(_angle_2deg, _dist_2deg_m); // advance to the next 2 degree sector _angle_2deg = a2d; From fde25e3e11d981a355c3a9e8cbaaf14bd9a5aa56 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 21 May 2025 20:39:25 +0900 Subject: [PATCH 5/5] Tools: correct Copter.Proximity distances for ld06 test --- Tools/autotest/arducopter.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 6f3518ff1f889..e5be5394b25e5 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7963,14 +7963,14 @@ def ProximitySensors(self): }) sensors = [ # tuples of name, prx_type ('ld06', 16, { - mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 273, + mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 275, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 696, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1200, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 625, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 967, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 760, - mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 771, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 765, }), ('sf45b', 8, { mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 270,