Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Add a low pass filter per face for distances

This commit is contained in:
Rishabh 2021-02-27 22:19:27 +05:30 committed by Randy Mackay
parent 4abae23306
commit 044e1850d4
6 changed files with 139 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Proximity/AP_Proximity.cpp
View File

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

10
libraries/AP_Proximity/AP_Proximity.h
View File

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

23
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

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

6
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

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

80
libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp
View File

@ -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,9 +71,28 @@ 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
// the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
// the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
@ -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;
}

17
libraries/AP_Proximity/AP_Proximity_Boundary_3D.h
View File

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