aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Rishabh 2021-03-15 23:31:53 +05:30 committed by Randy Mackay
parent e31ea2f3ee
commit 5e5ca6e02a
6 changed files with 75 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Proximity/AP_Proximity.cpp
View File

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

2
libraries/AP_Proximity/AP_Proximity.h
View File

@ -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);
};

15
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -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
{

6
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -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;

31
libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp
View File

@ -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
{

15
libraries/AP_Proximity/AP_Proximity_Boundary_3D.h
View File

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