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

AP_Proximity: Shift methods to Proximity_Boundary_3D class and support 3D Boundary

Browse Source
This commit is contained in:
Rishabh 2020-12-06 17:51:40 +05:30 committed by Randy Mackay
parent 7569d8bf8e
commit 4fce715a9a
2 changed files with 31 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

167
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -29,50 +29,6 @@ AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity
frontend(_frontend),
state(_state)
{
// initialise sector edge vector used for building the boundary fence
init_boundary();
}
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance) const
{
bool sector_found = false;
uint8_t sector = 0;
// check all sectors for shorter distance
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) {
if (!sector_found || (_distance[i] < _distance[sector])) {
sector = i;
sector_found = true;
}
}
}
if (sector_found) {
angle_deg = _angle[sector];
distance = _distance[sector];
}
return sector_found;
}
// get number of objects, used for non-GPS avoidance
uint8_t AP_Proximity_Backend::get_object_count() const
{
return PROXIMITY_NUM_SECTORS;
}
// get an object's angle and distance, used for non-GPS avoidance
// returns false if no angle or distance could be returned for some reason
bool AP_Proximity_Backend::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{
if (object_number < PROXIMITY_NUM_SECTORS && _distance_valid[object_number]) {
angle_deg = _angle[object_number];
distance = _distance[object_number];
return true;
}
return false;
}
// get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
@ -83,9 +39,10 @@ bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Dist
bool valid_distances = false;
for (uint8_t i=0; i<PROXIMITY_MAX_DIRECTION; i++) {
prx_dist_array.orientation[i] = i;
if (_distance_valid[i]) {
const boundary_location bnd_loc{i};
if (boundary.check_distance_valid(bnd_loc)) {
valid_distances = true;
prx_dist_array.distance[i] = _distance[i];
prx_dist_array.distance[i] = boundary.get_distance(bnd_loc);
} else {
prx_dist_array.distance[i] = distance_max();
}
@ -94,104 +51,6 @@ bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Dist
return valid_distances;
}
// get boundary points around vehicle for use by avoidance
// returns nullptr and sets num_points to zero if no boundary can be returned
const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points) const
{
// high-level status check
if (state.status != AP_Proximity::Status::Good) {
num_points = 0;
return nullptr;
}
// check at least one sector has valid data, if not, exit
bool some_valid = false;
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) {
some_valid = true;
break;
}
}
if (!some_valid) {
num_points = 0;
return nullptr;
}
// return boundary points
num_points = PROXIMITY_NUM_SECTORS;
return _boundary_point;
}
// initialise the boundary and sector_edge_vector array used for object avoidance
// should be called if the sector_middle_deg or _setor_width_deg arrays are changed
void AP_Proximity_Backend::init_boundary()
{
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
float angle_rad = radians((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f));
_sector_edge_vector[sector].x = cosf(angle_rad) * 100.0f;
_sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f;
_boundary_point[sector] = _sector_edge_vector[sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
}
}
// update boundary points used for object avoidance based on a single sector's 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
void AP_Proximity_Backend::update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB)
{
// sanity check
if (sector >= PROXIMITY_NUM_SECTORS) {
return;
}
if (push_to_OA_DB && _distance_valid[sector]) {
database_push(_angle[sector], _distance[sector]);
}
// find adjacent sector (clockwise)
uint8_t next_sector = sector + 1;
if (next_sector >= PROXIMITY_NUM_SECTORS) {
next_sector = 0;
}
// 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[sector] && _distance_valid[next_sector]) {
shortest_distance = MIN(_distance[sector], _distance[next_sector]);
} else if (_distance_valid[sector]) {
shortest_distance = _distance[sector];
} else if (_distance_valid[next_sector]) {
shortest_distance = _distance[next_sector];
}
if (shortest_distance < PROXIMITY_BOUNDARY_DIST_MIN) {
shortest_distance = PROXIMITY_BOUNDARY_DIST_MIN;
}
_boundary_point[sector] = _sector_edge_vector[sector] * shortest_distance;
// if the next sector (clockwise) has an invalid distance, set boundary to create a cup like boundary
if (!_distance_valid[next_sector]) {
_boundary_point[next_sector] = _sector_edge_vector[next_sector] * shortest_distance;
}
// repeat for edge between sector and previous sector
uint8_t prev_sector = (sector == 0) ? PROXIMITY_NUM_SECTORS-1 : sector-1;
shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
if (_distance_valid[prev_sector] && _distance_valid[sector]) {
shortest_distance = MIN(_distance[prev_sector], _distance[sector]);
} else if (_distance_valid[prev_sector]) {
shortest_distance = _distance[prev_sector];
} else if (_distance_valid[sector]) {
shortest_distance = _distance[sector];
}
_boundary_point[prev_sector] = _sector_edge_vector[prev_sector] * shortest_distance;
// if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup like boundary
uint8_t prev_sector_ccw = (prev_sector == 0) ? PROXIMITY_NUM_SECTORS - 1 : prev_sector - 1;
if (!_distance_valid[prev_sector_ccw]) {
_boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance;
}
}
// set status and update valid count
void AP_Proximity_Backend::set_status(AP_Proximity::Status status)
{
@ -205,12 +64,6 @@ float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) c
return wrap_360(angle_degrees * angle_sign + frontend.get_yaw_correction(state.instance));
}
// find which sector a given angle falls into
uint8_t AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees) const
{
return wrap_360(angle_degrees + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f;
}
// check if a reading should be ignored because it falls into an ignore area
bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg) const
{
@ -254,19 +107,17 @@ void AP_Proximity_Backend::database_push(float angle, float distance)
}
// update Object Avoidance database with Earth-frame point
void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned)
// pitch can be optionally provided if needed
void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned, float pitch)
{
AP_OADatabase *oaDb = AP::oadatabase();
if (oaDb == nullptr || !oaDb->healthy()) {
return;
}
//Assume object is angle bearing and distance meters away from the vehicle
Vector2f object_2D = {0.0f,0.0f};
object_2D.offset_bearing(wrap_180(angle), distance);
//rotate that vector to a 3D vector in NED frame
const Vector3f object_3D = {object_2D.x,object_2D.y,0.0f};
//Assume object is angle and pitch bearing and distance meters away from the vehicle
Vector3f object_3D;
object_3D.offset_bearing(wrap_180(angle), wrap_180(pitch), distance);
const Vector3f rotated_object_3D = body_to_ned * object_3D;
//Calculate the position vector from origin
@ -275,4 +126,4 @@ void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t t
temp_pos.z = temp_pos.z * -1.0f;
oaDb->queue_push(temp_pos, timestamp_ms, distance);
}
}

61
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -18,11 +18,7 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity.h"
#include <AP_Common/Location.h>
#define PROXIMITY_NUM_SECTORS 8 // number of sectors
#define PROXIMITY_SECTOR_WIDTH_DEG 45.0f // 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
#include "AP_Proximity_Boundary_3D.h"
class AP_Proximity_Backend
{
@ -47,17 +43,23 @@ public:
// handle mavlink DISTANCE_SENSOR messages
virtual void handle_msg(const mavlink_message_t &msg) {}
// get boundary points around vehicle for use by avoidance
// returns nullptr and sets num_points to zero if no boundary can be returned
const Vector2f* get_boundary_points(uint16_t& num_points) const;
// get total number of obstacles, used in GPS based Simple Avoidance
uint8_t get_obstacle_count() { return boundary.get_obstacle_count(); }
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
void get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const { return boundary.get_obstacle(obstacle_num, vec_to_obstacle); }
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// used in GPS based Simple Avoidance
float distance_to_obstacle(const uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const { return boundary.distance_to_obstacle(obstacle_num , seg_start, seg_end, closest_point); }
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool get_closest_object(float& angle_deg, float &distance) const;
bool get_closest_object(float& angle_deg, float &distance) const { return boundary.get_closest_object(angle_deg, distance); }
// get number of objects, angle and distance - used for non-GPS avoidance
uint8_t get_object_count() const;
bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
uint8_t get_horizontal_object_count() const {return boundary.get_horizontal_object_count(); }
bool get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const { return boundary.get_horizontal_object_angle_and_distance(object_number, angle_deg, distance); }
// 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;
@ -69,39 +71,20 @@ protected:
// correct an angle (in degrees) based on the orientation and yaw correction parameters
float correct_angle_for_orientation(float angle_degrees) const;
// find which sector a given angle falls into
uint8_t convert_angle_to_sector(float angle_degrees) const;
// initialise the boundary and sector_edge_vector array used for object avoidance
// should be called if the sector_middle_deg or _setor_width_deg arrays are changed
void init_boundary();
// update boundary points used for object avoidance based on a single sector's 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
void update_boundary_for_sector(const uint8_t sector, const bool push_to_OA_DB);
// check if a reading should be ignored because it falls into an ignore area
// angles should be in degrees and in the range of 0 to 360
bool ignore_reading(uint16_t angle_deg) const;
// database helpers. all angles are in degrees
bool database_prepare_for_push(Vector3f &current_pos, Matrix3f &body_to_ned);
void database_push(float angle, float distance);
void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned);
// database helpers. All angles are in degrees
static bool database_prepare_for_push(Vector3f &current_pos, Matrix3f &body_to_ned);
// Note: "angle" refers to yaw (in body frame) towards the obstacle
static void database_push(float angle, float distance);
static void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned, float pitch = 0.0f);
AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state
// sectors
const uint16_t _sector_middle_deg[PROXIMITY_NUM_SECTORS] = {0, 45, 90, 135, 180, 225, 270, 315}; // middle angle of each sector
// sensor data
float _angle[PROXIMITY_NUM_SECTORS]; // angle to closest object within each sector
float _distance[PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector
bool _distance_valid[PROXIMITY_NUM_SECTORS]; // true if a valid distance received for each sector
// fence boundary
Vector2f _sector_edge_vector[PROXIMITY_NUM_SECTORS]; // vector for right-edge of each sector, used to speed up calculation of boundary
Vector2f _boundary_point[PROXIMITY_NUM_SECTORS]; // bounding polygon around the vehicle calculated conservatively for object avoidance
// Methods to manipulate 3D boundary in this class
AP_Proximity_Boundary_3D boundary;
};