AP_Proximity: Add a low pass filter per face for distances

libraries/AP_Proximity/AP_Proximity.cpp

@@ -175,6 +175,13 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
     AP_GROUPINFO("2_YAW_CORR", 18, AP_Proximity, _yaw_correction[1], 0),
 #endif
 
+    // @Param: _LOG_RAW
+    // @DisplayName: Proximity raw distances log
+    // @Description: Set this parameter to one if logging unfiltered(raw) distances from sensor should be enabled
+    // @Values: 0:Off, 1:On
+    // @User: Advanced
+    AP_GROUPINFO("_LOG_RAW", 19, AP_Proximity, _raw_log_enable, 0),
+
     AP_GROUPEND
 };
 
@@ -359,6 +366,16 @@ bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_a
     return drivers[primary_instance]->get_horizontal_distances(prx_dist_array);
 }
 
+// get raw and filtered distances in 8 directions per layer. used for logging
+bool AP_Proximity::get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
+{
+    if (!valid_instance(primary_instance)) {
+        return false;
+    }
+    // get distances from backend
+    return drivers[primary_instance]->get_active_layer_distances(layer, prx_dist_array, prx_filt_dist_array);
+}
+
 // get total number of obstacles, used in GPS based Simple Avoidance
 uint8_t AP_Proximity::get_obstacle_count() const
 {   
@@ -368,6 +385,15 @@ uint8_t AP_Proximity::get_obstacle_count() const
     return drivers[primary_instance]->get_obstacle_count();
 }
 
+// get number of layers.
+uint8_t AP_Proximity::get_num_layers() const
+{
+    if (!valid_instance(primary_instance)) {
+        return 0;
+    }
+    return drivers[primary_instance]->get_num_layers();
+}
+
 // get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
 bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
 {

libraries/AP_Proximity/AP_Proximity.h

@@ -94,6 +94,9 @@ public:
     // get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
     bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
 
+    // get raw and filtered distances in 8 directions per layer. used for logging
+    bool get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
+
     // get total number of obstacles, used in GPS based Simple Avoidance
     uint8_t get_obstacle_count() const;
     
@@ -112,6 +115,9 @@ public:
     uint8_t get_object_count() const;
     bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
 
+    // get number of layers
+    uint8_t get_num_layers() const;
+
     // get maximum and minimum distances (in meters) of primary sensor
     float distance_max() const;
     float distance_min() const;
@@ -135,6 +141,9 @@ public:
 
     Type get_type(uint8_t instance) const;
 
+    // true if raw distances should be logged
+    bool get_raw_log_enable() const { return _raw_log_enable; }
+
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -166,6 +175,7 @@ private:
     AP_Int16 _yaw_correction[PROXIMITY_MAX_INSTANCES];
     AP_Int16 _ignore_angle_deg[PROXIMITY_MAX_IGNORE];   // angle (in degrees) of area that should be ignored by sensor (i.e. leg shows up)
     AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE];    // width of beam (in degrees) that should be ignored
+    AP_Int8 _raw_log_enable;                            // enable logging raw distances
 
     void detect_instance(uint8_t instance);
 };

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -37,22 +37,13 @@ static_assert(PROXIMITY_MAX_DIRECTION <= 8,
 // get distances in PROXIMITY_MAX_DIRECTION directions horizontally. used for sending distances to ground station
 bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const
 {
-    // cycle through all sectors filling in distances and orientations
-    // see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
-    bool valid_distances = false;
-    prx_dist_array.offset_valid = 0;
-    for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
-        prx_dist_array.orientation[i] = i;
-        const AP_Proximity_Boundary_3D::Face face(PROXIMITY_MIDDLE_LAYER, i);
-        if (boundary.get_distance(face, prx_dist_array.distance[i])) {
-            prx_dist_array.offset_valid |= (1U << i);
-            valid_distances = true;
-        } else {
-            prx_dist_array.distance[i] = distance_max();
-        }
-    }
-
-    return valid_distances;
+    AP_Proximity::Proximity_Distance_Array prx_filt_dist_array; // unused
+    return boundary.get_layer_distances(PROXIMITY_MIDDLE_LAYER, distance_max(), prx_dist_array, prx_filt_dist_array);
+}
+// get distances in PROXIMITY_MAX_DIRECTION directions at a layer. used for logging
+bool AP_Proximity_Backend::get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
+{
+    return boundary.get_layer_distances(layer, distance_max(), prx_dist_array, prx_filt_dist_array);
 }
 
 // set status and update valid count

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -64,6 +64,12 @@ public:
     // get distances in 8 directions. used for sending distances to ground station
     bool get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const;
 
+    // get raw and filtered distances in 8 directions per layer. used for logging
+    bool get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
+
+    // get number of layers
+    uint8_t get_num_layers() const { return boundary.get_num_layers(); }
+
 protected:
 
     // set status and update valid_count

libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp

@@ -21,6 +21,7 @@ void AP_Proximity_Boundary_3D::init()
             const float angle_rad = ((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f));
             _sector_edge_vector[layer][sector].offset_bearing(angle_rad, pitch, 100.0f);
             _boundary_points[layer][sector] = _sector_edge_vector[layer][sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
+            _filtered_distance[layer][sector].set_cutoff_frequency(0.25f);
         }
     }
 }
@@ -49,6 +50,9 @@ void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch
     _distance[face.layer][face.sector] = distance;
     _distance_valid[face.layer][face.sector] = true;
 
+    // apply filter
+    set_filtered_distance(face, distance);
+
     // update boundary used for simple avoidance
     update_boundary(face);
 }
@@ -67,7 +71,26 @@ void AP_Proximity_Boundary_3D::add_distance(float pitch, float yaw, float distan
         _distance_valid[face.layer][face.sector] = true;
         _angle[face.layer][face.sector] = yaw;
         _pitch[face.layer][face.sector] = pitch;
+        set_filtered_distance(face, distance);
+    }
+}
+
+// Apply low pass filter on the raw distance
+void AP_Proximity_Boundary_3D::set_filtered_distance(const Face &face, float distance)
+{
+    if (!face.valid()) {
+        return;
     }
+
+    const uint32_t now_ms = AP_HAL::millis();
+    const uint32_t dt = now_ms - _last_update_ms[face.layer][face.sector];
+    if (dt < PROXIMITY_FILT_RESET_TIME) {
+        _filtered_distance[face.layer][face.sector].apply(distance, dt* 0.001f);
+    } else {
+        // reset filter since last distance was passed a long time back
+        _filtered_distance[face.layer][face.sector].reset(distance);
+    }
+    _last_update_ms[face.layer][face.sector] = now_ms;
 }
 
 // update boundary points used for object avoidance based on a single sector and pitch distance changing
@@ -89,11 +112,11 @@ void AP_Proximity_Boundary_3D::update_boundary(const Face &face)
     // boundary point lies on the line between the two sectors at the shorter distance found in the two sectors
     float shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
     if (_distance_valid[layer][sector] && _distance_valid[layer][next_sector]) {
-        shortest_distance = MIN(_distance[layer][sector], _distance[layer][next_sector]);
+        shortest_distance = MIN(_filtered_distance[layer][sector].get(), _filtered_distance[layer][next_sector].get());
     } else if (_distance_valid[layer][sector]) {
-        shortest_distance = _distance[layer][sector];
+        shortest_distance = _filtered_distance[layer][sector].get();
     } else if (_distance_valid[layer][next_sector]) {
-        shortest_distance = _distance[layer][next_sector];
+        shortest_distance = _filtered_distance[layer][next_sector].get();
     }
     if (shortest_distance < PROXIMITY_BOUNDARY_DIST_MIN) {
         shortest_distance = PROXIMITY_BOUNDARY_DIST_MIN;
@@ -109,11 +132,11 @@ void AP_Proximity_Boundary_3D::update_boundary(const Face &face)
     const uint8_t prev_sector = get_prev_sector(sector);
     shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
     if (_distance_valid[layer][prev_sector] && _distance_valid[layer][sector]) {
-        shortest_distance = MIN(_distance[layer][prev_sector], _distance[layer][sector]);
+        shortest_distance = MIN(_filtered_distance[layer][prev_sector].get(), _filtered_distance[layer][sector].get());
     } else if (_distance_valid[layer][prev_sector]) {
-        shortest_distance = _distance[layer][prev_sector];
+        shortest_distance = _filtered_distance[layer][prev_sector].get();
     } else if (_distance_valid[layer][sector]) {
-        shortest_distance = _distance[layer][sector];
+        shortest_distance = _filtered_distance[layer][sector].get();
     }
     _boundary_points[layer][prev_sector] = _sector_edge_vector[layer][prev_sector] * shortest_distance;
 
@@ -286,8 +309,51 @@ bool AP_Proximity_Boundary_3D::get_horizontal_object_angle_and_distance(uint8_t
 {
     if ((object_number < PROXIMITY_NUM_SECTORS) && _distance_valid[PROXIMITY_MIDDLE_LAYER][object_number]) {
         angle_deg = _angle[PROXIMITY_MIDDLE_LAYER][object_number];
-        distance = _distance[PROXIMITY_MIDDLE_LAYER][object_number];
+        distance = _filtered_distance[PROXIMITY_MIDDLE_LAYER][object_number].get();
         return true;
     }
     return false;
 }
+
+// Return filtered distance for the passed in face
+bool AP_Proximity_Boundary_3D::get_filtered_distance(const Face &face, float &distance) const
+{
+    if (!face.valid()) {
+        return false;
+    }
+
+    if (!_distance_valid[face.layer][face.sector]) {
+        // invalid distace
+        return false;
+    }
+
+    distance = _filtered_distance[face.layer][face.sector].get();
+    return true;
+}
+
+// Get raw and filtered distances in 8 directions per layer
+bool AP_Proximity_Boundary_3D::get_layer_distances(uint8_t layer_number, float dist_max, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
+{
+    // cycle through all sectors filling in distances and orientations
+    // see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
+    bool valid_distances = false;
+    prx_dist_array.offset_valid = 0;
+    prx_filt_dist_array.offset_valid = 0;
+    for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
+        prx_dist_array.orientation[i] = i;
+        const AP_Proximity_Boundary_3D::Face face(layer_number, i);
+        if (!face.valid()) {
+            return false;
+        }
+        if (get_distance(face, prx_dist_array.distance[i]) && get_filtered_distance(face, prx_filt_dist_array.distance[i])) {
+            valid_distances = true;
+            prx_dist_array.offset_valid |= (1U << i);
+            prx_filt_dist_array.offset_valid |= (1U << i);
+        } else {
+            prx_dist_array.distance[i] = dist_max;
+            prx_filt_dist_array.distance[i] = dist_max;
+        }
+    }
+
+    return valid_distances;
+}

libraries/AP_Proximity/AP_Proximity_Boundary_3D.h

@@ -15,6 +15,8 @@
 
 #pragma once
 
+#include <Filter/LowPassFilter.h>
+
 #define PROXIMITY_NUM_SECTORS         8       // number of sectors
 #define PROXIMITY_NUM_LAYERS          5       // num of layers in a sector
 #define PROXIMITY_MIDDLE_LAYER        2       // middle layer
@@ -22,6 +24,7 @@
 #define PROXIMITY_SECTOR_WIDTH_DEG    (360.0f/PROXIMITY_NUM_SECTORS)   // width of sectors in degrees
 #define PROXIMITY_BOUNDARY_DIST_MIN   0.6f    // minimum distance for a boundary point.  This ensures the object avoidance code doesn't think we are outside the boundary.
 #define PROXIMITY_BOUNDARY_DIST_DEFAULT 100   // if we have no data for a sector, boundary is placed 100m out
+#define PROXIMITY_FILT_RESET_TIME     1000    // reset filter if last distance was pushed more than this many ms away
 
 class AP_Proximity_Boundary_3D 
 { 
@@ -105,6 +108,12 @@ public:
     uint8_t get_horizontal_object_count() const;
     bool get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
 
+    // get number of layers
+    uint8_t get_num_layers() const { return PROXIMITY_NUM_LAYERS; }
+
+    // get raw and filtered distances in 8 directions per layer.
+    bool get_layer_distances(uint8_t layer_number, float dist_max, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
+
     // sectors
     static_assert(PROXIMITY_NUM_SECTORS == 8, "PROXIMITY_NUM_SECTOR must be 8");
     const uint16_t _sector_middle_deg[PROXIMITY_NUM_SECTORS] {0, 45, 90, 135, 180, 225, 270, 315};    // middle angle of each sector
@@ -130,6 +139,12 @@ private:
     // The resultant is packed into a Boundary Location object and returned by reference as "face"
     bool convert_obstacle_num_to_face(uint8_t obstacle_num, Face& face) const WARN_IF_UNUSED;
 
+    // Apply low pass filter on the raw distance
+    void set_filtered_distance(const Face &face, float distance);
+
+    // Return filtered distance for the passed in face
+    bool get_filtered_distance(const Face &face, float &distance) const;
+
     Vector3f _sector_edge_vector[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
     Vector3f _boundary_points[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
 
@@ -137,5 +152,7 @@ private:
     float _pitch[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];          // pitch angle in degrees to the closest object within each sector and layer
     float _distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];       // distance to closest object within each sector and layer
     bool _distance_valid[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];  // true if a valid distance received for each sector and layer
+    uint32_t _last_update_ms[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // time when distance was last updated
+    LowPassFilterFloat _filtered_distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // low pass filter
 };