Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: boundary_points requires only one valid distance

Browse Source 
Object avoidance is possible with just one valid distance
Boundary is initialised in new init_boundary function to be 100m from
vehicle
If sectors do not have valid distance measurements, we use the distance
from adjacent sectors.  This conveniently leads to a concave shaped
boundary that keeps the vehicle from travelling into the dataless sector.
This commit is contained in:
Randy Mackay 2017-01-10 14:02:43 +09:00
parent 20ca021cab
commit 74f1899aeb
3 changed files with 59 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -26,6 +26,8 @@ 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 in meters in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
@ -142,19 +144,36 @@ const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points)
return nullptr;
}
// check all sectors have valid data, if not, exit
// check at least one sector has valid data, if not, exit
bool some_valid = false;
for (uint8_t i=0; i<_num_sectors; i++) {
if (!_distance_valid[i]) {
num_points = 0;
return nullptr;
if (_distance_valid[i]) {
some_valid = true;
break;
}
}
if (!some_valid) {
num_points = 0;
return nullptr;
}
// return boundary points
num_points = _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 < _num_sectors; sector++) {
float angle_rad = radians((float)_sector_middle_deg[sector]+(float)_sector_width_deg[sector]/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
@ -165,13 +184,6 @@ void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
return;
}
// initialise sector_edge_vector if necessary
if (_sector_edge_vector[sector].is_zero()) {
float angle_rad = radians((float)_sector_middle_deg[sector]+(float)_sector_width_deg[sector]/2.0f);
_sector_edge_vector[sector].x = cosf(angle_rad) * 100.0f;
_sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f;
}
// find adjacent sector (clockwise)
uint8_t next_sector = sector + 1;
if (next_sector >= _num_sectors) {
@ -179,19 +191,40 @@ void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
}
// 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]) {
float shortest_distance = MIN(_distance[sector], _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;
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) ? _num_sectors-1 : sector-1;
shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
if (_distance_valid[prev_sector] && _distance_valid[sector]) {
float shortest_distance = MIN(_distance[prev_sector], _distance[sector]);
_boundary_point[prev_sector] = _sector_edge_vector[prev_sector] * shortest_distance;
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) ? _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;
}
}

5
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -20,6 +20,7 @@
#define PROXIMITY_SECTORS_MAX 12 // maximum number of sectors
#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
class AP_Proximity_Backend
{
@ -68,6 +69,10 @@ protected:
// find which sector a given angle falls into
bool convert_angle_to_sector(float angle_degrees, uint8_t &sector) 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

3
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

@ -162,6 +162,9 @@ void AP_Proximity_LightWareSF40C::init_sectors()
// set num sectors
_num_sectors = sector;
// re-initialise boundary because sector locations have changed
init_boundary();
// record success
_sector_initialised = true;
}