AP_Proximity: LD06 gets mode filter

Author: rmackay9

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

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,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