AP_Proximity: LD06/LD19 gets mode filter

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,

libraries/AP_Proximity/AP_Proximity_LD06.cpp

@@ -33,16 +33,17 @@
 
 // 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
 
+// confidence for each measurement must be this value or higher
+#define CONFIDENCE_THRESHOLD 20
+
  /* ------------------------------------------
     Data Packet Structure:
     Start Character : 1 Byte
@@ -169,25 +170,30 @@ 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];
 
-        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);
 
+            // 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;
             _dist_2deg_m = distance_m;
         }

libraries/AP_Proximity/AP_Proximity_LD06.h

@@ -29,6 +29,7 @@
 #if AP_PROXIMITY_LD06_ENABLED
 
 #include "AP_Proximity_Backend_Serial.h"
+#include <Filter/ModeFilter.h>
 
 #define MESSAGE_LENGTH_LD06         47
 
@@ -62,12 +63,16 @@ 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
     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;
 };

libraries/Filter/ModeFilter.cpp

@@ -99,3 +99,4 @@ void ModeFilter<T,FILTER_SIZE>::isort(T new_sample, bool drop_high)
 template class ModeFilter<float,5>;
 template class ModeFilter<int16_t,3>;
 template class ModeFilter<int16_t,5>;
+template class ModeFilter<uint16_t,3>;