Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 01:23:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: Skip unnecessary retries to build inner fences

Browse Source
This commit is contained in:
Rishabh 2020-03-09 02:00:02 +05:30 committed by Randy Mackay
parent 4ec8602de2
commit 15f25aa2a5
1 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -250,6 +250,14 @@ bool AP_OADijkstra::check_inclusion_polygon_updated() const
bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id) bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id)
{ {
const AC_Fence *fence = AC_Fence::get_singleton(); const AC_Fence *fence = AC_Fence::get_singleton();
// skip unnecessary retry to build inclusion polygon if previous fence points have not changed
if (_inclusion_polygon_update_ms == fence->polyfence().get_inclusion_polygon_update_ms()) {
return false;
}
_inclusion_polygon_update_ms = fence->polyfence().get_inclusion_polygon_update_ms();
if (fence == nullptr) { if (fence == nullptr) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED; err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
return false; return false;
@ -314,10 +322,6 @@ bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OAD
// update total number of points // update total number of points
_inclusion_polygon_numpoints += num_points; _inclusion_polygon_numpoints += num_points;
} }
// record fence update time so we don't process these inclusion polygons again
_inclusion_polygon_update_ms = fence->polyfence().get_inclusion_polygon_update_ms();
return true; return true;
} }
@ -337,6 +341,14 @@ bool AP_OADijkstra::check_exclusion_polygon_updated() const
bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id) bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id)
{ {
const AC_Fence *fence = AC_Fence::get_singleton(); const AC_Fence *fence = AC_Fence::get_singleton();
// skip unnecessary retry to build exclusion polygon if previous fence points have not changed
if (_exclusion_polygon_update_ms == fence->polyfence().get_exclusion_polygon_update_ms()) {
return false;
}
_exclusion_polygon_update_ms = fence->polyfence().get_exclusion_polygon_update_ms();
if (fence == nullptr) { if (fence == nullptr) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED; err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
return false; return false;
@ -401,10 +413,6 @@ bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OAD
// update total number of points // update total number of points
_exclusion_polygon_numpoints += num_points; _exclusion_polygon_numpoints += num_points;
} }
// record fence update time so we don't process these exclusion polygons again
_exclusion_polygon_update_ms = fence->polyfence().get_exclusion_polygon_update_ms();
return true; return true;
} }