AC_Avoidance: AP_OADijkstra and OAVisGraph use AP_ExpandingArray

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
+    // expand fence point array if required
-    if (num_points > OA_DIJKSTRA_POLYGON_FENCE_PTS) {
+    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
+        // fail if out of space
-        if ((_path_numpoints >= OA_DIJKSTRA_POLYGON_SHORTPATH_PTS) ||
+        if (_path_numpoints >= _path.max_items()) {
-            (_short_path_data[nidx].distance_from_idx == OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) ||
+            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 {

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<Vector2f> _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<ShortPathNode> _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)
+    AP_ExpandingArray<AP_OAVisGraph::OAItemID> _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
+    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_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)
+    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);

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
+    return _items.expand_to_hold(size);
-    if (_items != nullptr) {
-        return false;
-    }
-
-    // allocate array
-    _items = (VisGraphItem *)calloc(size, sizeof(VisGraphItem));
-    if (_items != nullptr) {
-        _num_items_max = size;
-        return true;
-    }
-
-    return false;
 }
 
 // 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) {
+    if (_num_items >= _items.max_items()) {
-        return false;
+        if (!_items.expand(1)) {
+            return false;
+        }
     }
 
     // add item

libraries/AC_Avoidance/AP_OAVisGraph.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_Common/AP_Common.h>
+#include <AP_Common/AP_ExpandingArray.h>
 #include <AP_HAL/AP_HAL.h>
 
 /*
@@ -57,7 +58,6 @@ public:
 
 private:
 
-    VisGraphItem *_items;
+    AP_ExpandingArray<VisGraphItem> _items;
-    uint8_t _num_items_max;
     uint8_t _num_items;
 };