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

AP_Proximity: set minimum boundary distance

Browse Source 
AC_Avoidance will not stop if it thinks the vehicle is on or outside the polygon.  Setting a minimum distance ensures the vehicle is always within the polygon.
This commit is contained in:
Randy Mackay 2016-11-26 11:22:39 +09:00
parent 115bc44d3f
commit 54a87ad35c
2 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -163,6 +163,9 @@ 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 // boundary point lies on the line between the two sectors at the shorter distance found in the two sectors
if (_distance_valid[sector] && _distance_valid[next_sector]) { if (_distance_valid[sector] && _distance_valid[next_sector]) {
float shortest_distance = MIN(_distance[sector], _distance[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; _boundary_point[sector] = _sector_edge_vector[sector] * shortest_distance;
} }

1
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -19,6 +19,7 @@
#include "AP_Proximity.h" #include "AP_Proximity.h"
#define PROXIMITY_SECTORS_MAX 12 // maximum number of sectors #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.
class AP_Proximity_Backend class AP_Proximity_Backend
{ {