diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index b05c602feb..77c987264f 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -153,8 +153,8 @@ bool AP_OADijkstra::create_polygon_fence_with_margin(float margin_cm) return false; } - // fail if fence is too large - if (num_points > OA_DIJKSTRA_POLYGON_FENCE_PTS) { + // expand fence point array if required + if (!_polyfence_pts.expand_to_hold(num_points)) { return false; } @@ -416,6 +416,11 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d // check OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX is defined correct static_assert(OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX > OA_DIJKSTRA_POLYGON_FENCE_PTS, "check OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX > OA_DIJKSTRA_POLYGON_FENCE_PTS"); + // expand _short_path_data if necessary + if (!_short_path_data.expand_to_hold(2 + _polyfence_numpoints)) { + return false; + } + // add origin and destination (node_type, id, visited, distance_from_idx, distance_cm) to short_path_data array _short_path_data[0] = {{AP_OAVisGraph::OATYPE_SOURCE, 0}, false, 0, 0}; _short_path_data[1] = {{AP_OAVisGraph::OATYPE_DESTINATION, 0}, false, OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX, FLT_MAX}; @@ -460,9 +465,14 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d } _path_numpoints = 0; while (true) { - // fail if out of space or newest node has invalid distance or from index - if ((_path_numpoints >= OA_DIJKSTRA_POLYGON_SHORTPATH_PTS) || - (_short_path_data[nidx].distance_from_idx == OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) || + // fail if out of space + if (_path_numpoints >= _path.max_items()) { + if (!_path.expand()) { + break; + } + } + // fail if newest node has invalid distance_from_index + if ((_short_path_data[nidx].distance_from_idx == OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) || (_short_path_data[nidx].distance_cm >= FLT_MAX)) { break; } else { diff --git a/libraries/AC_Avoidance/AP_OADijkstra.h b/libraries/AC_Avoidance/AP_OADijkstra.h index d2c38ad99e..38227d46dc 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/libraries/AC_Avoidance/AP_OADijkstra.h @@ -62,7 +62,7 @@ private: // polygon fence (with margin) related variables float _polyfence_margin = 10; - Vector2f _polyfence_pts[OA_DIJKSTRA_POLYGON_FENCE_PTS]; + AP_ExpandingArray _polyfence_pts; uint8_t _polyfence_numpoints; uint32_t _polyfence_update_ms; // system time of boundary update from AC_Fence (used to detect changes to polygon fence) @@ -83,7 +83,8 @@ private: bool visited; // true if all this node's neighbour's distances have been updated node_index distance_from_idx; // index into _short_path_data from where distance was updated (or 255 if not set) float distance_cm; // distance from source (number is tentative until this node is the current node and/or visited = true) - } _short_path_data[OA_DIJKSTRA_POLYGON_SHORTPATH_PTS]; + }; + AP_ExpandingArray _short_path_data; node_index _short_path_data_numpoints; // number of elements in _short_path_data array // update total distance for all nodes visible from current node @@ -99,10 +100,10 @@ private: bool find_closest_node_idx(node_index &node_idx) const; // final path variables and functions - AP_OAVisGraph::OAItemID _path[OA_DIJKSTRA_POLYGON_SHORTPATH_PTS]; // ids of points on return path in reverse order (i.e. destination is first element) - uint8_t _path_numpoints; // number of points on return path - Vector2f _path_source; // source point used in shortest path calculations (offset in cm from EKF origin) - Vector2f _path_destination; // destination position used in shortest path calculations (offset in cm from EKF origin) + AP_ExpandingArray _path; // ids of points on return path in reverse order (i.e. destination is first element) + uint8_t _path_numpoints; // number of points on return path + Vector2f _path_source; // source point used in shortest path calculations (offset in cm from EKF origin) + Vector2f _path_destination; // destination position used in shortest path calculations (offset in cm from EKF origin) // return point from final path as an offset (in cm) from the ekf origin bool get_shortest_path_point(uint8_t point_num, Vector2f& pos); diff --git a/libraries/AC_Avoidance/AP_OAVisGraph.cpp b/libraries/AC_Avoidance/AP_OAVisGraph.cpp index 76bbf28113..a9dcce7df7 100644 --- a/libraries/AC_Avoidance/AP_OAVisGraph.cpp +++ b/libraries/AC_Avoidance/AP_OAVisGraph.cpp @@ -24,27 +24,17 @@ AP_OAVisGraph::AP_OAVisGraph(uint8_t size) // initialise array to given size bool AP_OAVisGraph::init(uint8_t size) { - // only allow init once - if (_items != nullptr) { - return false; - } - - // allocate array - _items = (VisGraphItem *)calloc(size, sizeof(VisGraphItem)); - if (_items != nullptr) { - _num_items_max = size; - return true; - } - - return false; + return _items.expand_to_hold(size); } // add item to visiblity graph, returns true on success, false if graph is full bool AP_OAVisGraph::add_item(const OAItemID &id1, const OAItemID &id2, float distance_cm) { // check there is space - if (_num_items >= _num_items_max) { - return false; + if (_num_items >= _items.max_items()) { + if (!_items.expand(1)) { + return false; + } } // add item diff --git a/libraries/AC_Avoidance/AP_OAVisGraph.h b/libraries/AC_Avoidance/AP_OAVisGraph.h index decd9b64c0..4f54770d59 100644 --- a/libraries/AC_Avoidance/AP_OAVisGraph.h +++ b/libraries/AC_Avoidance/AP_OAVisGraph.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include /* @@ -57,7 +58,6 @@ public: private: - VisGraphItem *_items; - uint8_t _num_items_max; + AP_ExpandingArray _items; uint8_t _num_items; };