AP_Proximity: Check for stray faces and add new param for filter cut off freq

libraries/AP_Proximity/AP_Proximity.cpp

@@ -166,6 +166,14 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_LOG_RAW", 17, AP_Proximity, _raw_log_enable, 0),
 
+    // @Param: _FILT
+    // @DisplayName: Proximity filter cutoff frequency
+    // @Description: Cutoff frequency for low pass filter applied to each face in the proximity boundary
+    // @Units: Hz
+    // @Range: 0 20
+    // @User: Advanced
+    AP_GROUPINFO("_FILT", 18, AP_Proximity, _filt_freq, 0.25f),
+
     AP_GROUPEND
 };
 
@@ -209,6 +217,7 @@ void AP_Proximity::update(void)
             continue;
         }
         drivers[i]->update();
+        drivers[i]->boundary_3D_checks();
     }
 
     // work out primary instance - first sensor returning good data

libraries/AP_Proximity/AP_Proximity.h

@@ -89,6 +89,7 @@ public:
     // return sensor orientation and yaw correction
     uint8_t get_orientation(uint8_t instance) const;
     int16_t get_yaw_correction(uint8_t instance) const;
+    float get_filter_freq() const { return _filt_freq; }
 
     // return sensor health
     Status get_status(uint8_t instance) const;
@@ -188,6 +189,7 @@ private:
     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
     AP_Int8 _ign_gnd_enable;                           // true if land detection should be enabled
+    AP_Float _filt_freq;                               // cutoff frequency for low pass filter
 
     void detect_instance(uint8_t instance);
 };

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -55,6 +55,21 @@ void AP_Proximity_Backend::set_status(AP_Proximity::Status status)
     state.status = status;
 }
 
+// timeout faces that have not received data recently and update filter frequencies
+void AP_Proximity_Backend::boundary_3D_checks()
+{
+    // set the cutoff freq for low pass filter
+    boundary.set_filter_freq(frontend.get_filter_freq());
+
+    // check if any face has valid distance when it should not
+    const uint32_t now_ms = AP_HAL::millis();
+    // run this check every PROXIMITY_BOUNDARY_3D_TIMEOUT_MS
+    if ((now_ms - _last_timeout_check_ms) > PROXIMITY_BOUNDARY_3D_TIMEOUT_MS) {
+        _last_timeout_check_ms = now_ms;
+        boundary.check_face_timeout();
+    }
+}
+
 // correct an angle (in degrees) based on the orientation and yaw correction parameters
 float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) const
 {

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -24,6 +24,7 @@
 
 #define PROXIMITY_GND_DETECT_THRESHOLD 1.0f // set ground detection threshold to be 1 meters
 #define PROXIMITY_ALT_DETECT_TIMEOUT_MS 500 // alt readings should arrive within this much time
+#define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 1000 // we should check the 3D boundary faces after every this many ms
 
 class AP_Proximity_Backend
 {
@@ -38,6 +39,9 @@ public:
     // update the state structure
     virtual void update() = 0;
 
+    // timeout faces that have not received data recently and update filter frequencies
+    void boundary_3D_checks();
+
     // get maximum and minimum distances (in meters) of sensor
     virtual float distance_max() const = 0;
     virtual float distance_min() const = 0;
@@ -108,6 +112,8 @@ protected:
     };
     static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned);
 
+    uint32_t _last_timeout_check_ms;  // time when boundary was checked for non-updated valid faces
+
     // used for ground detection
     uint32_t _last_downward_update_ms;
     bool     _rangefinder_use;

libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp

@@ -38,7 +38,6 @@ 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);
         }
     }
 }
@@ -92,12 +91,26 @@ void AP_Proximity_Boundary_3D::add_distance(float pitch, float yaw, float distan
     }
 }
 
+// apply a new cutoff_freq to low-pass filter
+void AP_Proximity_Boundary_3D::apply_filter_freq(float cutoff_freq)
+{
+    for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
+        for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
+            _filtered_distance[layer][sector].set_cutoff_frequency(cutoff_freq);
+        }
+    }
+}
+
 // 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;
     }
+    if (!is_equal(_filtered_distance[face.layer][face.sector].get_cutoff_freq(), _filter_freq)) {
+        // cutoff freq has changed
+        apply_filter_freq(_filter_freq);
+    }
 
     const uint32_t now_ms = AP_HAL::millis();
     const uint32_t dt = now_ms - _last_update_ms[face.layer][face.sector];
@@ -195,6 +208,22 @@ void AP_Proximity_Boundary_3D::reset_face(const Face &face)
     update_boundary(face);
 }
 
+// check if a face has valid distance even if it was updated a long time back
+void AP_Proximity_Boundary_3D::check_face_timeout()
+{
+    for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
+        for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
+            if (_distance_valid[layer][sector]) {
+                if ((AP_HAL::millis() - _last_update_ms[layer][sector]) > PROXIMITY_FACE_RESET_MS) {
+                    // this face has a valid distance but wasn't updated for a long time, reset it
+                    AP_Proximity_Boundary_3D::Face face{layer, sector};
+                    reset_face(face);
+                }
+            }
+        }
+    }
+}
+
 // get distance for a face.  returns true on success and fills in distance argument with distance in meters
 bool AP_Proximity_Boundary_3D::get_distance(const Face &face, float &distance) const
 {

libraries/AP_Proximity/AP_Proximity_Boundary_3D.h

@@ -29,6 +29,7 @@
 #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
+#define PROXIMITY_FACE_RESET_MS       1250    // face will be reset if not updated within this many ms
 
 class AP_Proximity_Boundary_3D
 {
@@ -90,6 +91,9 @@ public:
     // i.e Distance is marked as not-valid
     void reset_face(const Face &face);
 
+    // check if a face has valid distance even if it was updated a long time back
+    void check_face_timeout();
+
     // get distance for a face.  returns true on success and fills in distance argument with distance in meters
     bool get_distance(const Face &face, float &distance) const;
 
@@ -118,6 +122,9 @@ public:
     // 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;
 
+    // pass down filter cut-off freq from params
+    void set_filter_freq(float filt_freq) { _filter_freq = filt_freq; }
+
     // 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
@@ -143,6 +150,9 @@ 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 a new cutoff_freq to low-pass filter
+    void apply_filter_freq(float cutoff_freq);
+
     // Apply low pass filter on the raw distance
     void set_filtered_distance(const Face &face, float distance);
 
@@ -158,6 +168,7 @@ private:
     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
+    float _filter_freq;                                                 // cutoff freq of low pass filter
 };
 
 #endif // HAL_PROXIMITY_ENABLED