Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: Dijkstra's exits early if polygon fence disabled

Browse Source
This commit is contained in:
Randy Mackay 2019-06-13 21:12:11 +09:00
parent 23f5fabbcf
commit bba81b2794
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -37,6 +37,11 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
return false; return false;
} }
// no avoidance required if fence is disabled
if (!polygon_fence_enabled()) {
return false;
}
// check for fence updates // check for fence updates
if (check_polygon_fence_updated()) { if (check_polygon_fence_updated()) {
_polyfence_with_margin_ok = false; _polyfence_with_margin_ok = false;
@ -110,6 +115,16 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
return false; return false;
} }
// returns true if polygon fence is enabled
bool AP_OADijkstra::polygon_fence_enabled() const
{
const AC_Fence *fence = AC_Fence::get_singleton();
if (fence == nullptr) {
return false;
}
return ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) > 0);
}
// check if polygon fence has been updated since we created the inner fence. returns true if changed // check if polygon fence has been updated since we created the inner fence. returns true if changed
bool AP_OADijkstra::check_polygon_fence_updated() const bool AP_OADijkstra::check_polygon_fence_updated() const
{ {

3
libraries/AC_Avoidance/AP_OADijkstra.h
View File

@ -31,6 +31,9 @@ public:
private: private:
// returns true if polygon fence is enabled
bool polygon_fence_enabled() const;
// check if polygon fence has been updated since we created the inner fence. returns true if changed // check if polygon fence has been updated since we created the inner fence. returns true if changed
bool check_polygon_fence_updated() const; bool check_polygon_fence_updated() const;