From 5e5ca6e02abcbc04db1a9645e03be675f6c10a65 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Mon, 15 Mar 2021 23:31:53 +0530 Subject: [PATCH] AP_Proximity: Check for stray faces and add new param for filter cut off freq --- libraries/AP_Proximity/AP_Proximity.cpp | 9 ++++++ libraries/AP_Proximity/AP_Proximity.h | 2 ++ .../AP_Proximity/AP_Proximity_Backend.cpp | 15 +++++++++ libraries/AP_Proximity/AP_Proximity_Backend.h | 6 ++++ .../AP_Proximity/AP_Proximity_Boundary_3D.cpp | 31 ++++++++++++++++++- .../AP_Proximity/AP_Proximity_Boundary_3D.h | 15 +++++++-- 6 files changed, 75 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 1c006eec16..5879511d17 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/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 diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 17d46f4155..5e054a9ada 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/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); }; diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 6ba7858803..4be559b3a6 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/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 { diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index cddfe43f46..efd71b1598 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/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 ¤t_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; diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp index 2935dc2870..61bdf01d75 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp +++ b/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 { diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h index 95a72e3c14..d2f30fa2b8 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h @@ -29,9 +29,10 @@ #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 -{ +class AP_Proximity_Boundary_3D +{ public: // constructor. This incorporates initialisation as well. 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