diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index f5150cd6ea32c..8fd4d18fb2445 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -313,7 +313,7 @@ bool AP_OADijkstra::check_inclusion_polygon_updated() const // create polygons inside the existing inclusion polygons // returns true on success. returns false on failure and err_id is updated -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(); @@ -324,6 +324,8 @@ bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OAD // 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 the error code we returned last time we were called with this fence set: + err_id = last_create_inclusion_polygon_with_margin_error_id; return false; } @@ -399,6 +401,12 @@ bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OAD } return true; } +bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id) +{ + const bool ret = _create_inclusion_polygon_with_margin(margin_cm, err_id); + last_create_inclusion_polygon_with_margin_error_id = err_id; + return ret; +} // check if exclusion polygons have been updated since create_exclusion_polygon_with_margin was run // returns true if changed diff --git a/libraries/AC_Avoidance/AP_OADijkstra.h b/libraries/AC_Avoidance/AP_OADijkstra.h index b0beeea2113fe..f8afe8db4c7d2 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/libraries/AC_Avoidance/AP_OADijkstra.h @@ -76,6 +76,10 @@ class AP_OADijkstra { // create polygons inside the existing inclusion polygons // returns true on success. returns false on failure and err_id is updated bool create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id); + // create polygons inside the existing inclusion polygons + // returns true on success. returns false on failure and err_id is updated. This function is wrapped so we can remember the error ID. + bool _create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id); + AP_OADijkstra_Error last_create_inclusion_polygon_with_margin_error_id; // // exclusion polygon methods