aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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)
{
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) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
return false;
@ -314,10 +322,6 @@ bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OAD
// update total number of 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;
}
@ -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)
{
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) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
return false;
@ -401,10 +413,6 @@ bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OAD
// update total number of 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;
}