AC_Fence: AC_Polygon_loader loads points from eeprom

Thanks to Peter Barker for his work to library-ify Plane's geofence which was the basis for this work.

libraries/AC_Fence/AC_PolyFence_loader.cpp

@@ -0,0 +1,144 @@
+#include "AC_PolyFence_loader.h"
+
+extern const AP_HAL::HAL& hal;
+
+static const StorageAccess fence_storage(StorageManager::StorageFence);
+
+/*
+  maximum number of fencepoints
+ */
+uint8_t AC_PolyFence_loader::max_points() const
+{
+    return MIN(255U, fence_storage.size() / sizeof(Vector2l));
+}
+
+// create buffer to hold copy of eeprom points in RAM
+// returns NULL if not enough memory can be allocated
+void* AC_PolyFence_loader::create_point_array(uint8_t element_size)
+{
+    uint32_t array_size = max_points() * element_size;
+    if (hal.util->available_memory() < 100U + array_size) {
+        // too risky to enable as we could run out of stack
+        return NULL;
+    }
+
+    return calloc(1, array_size);
+}
+
+// load boundary point from eeprom, returns true on successful load
+bool AC_PolyFence_loader::load_point_from_eeprom(uint16_t i, Vector2l& point)
+{
+    // sanity check index
+    if (i >= max_points()) {
+        return false;
+    }
+
+    // read fence point
+    point.x = fence_storage.read_uint32(i * sizeof(Vector2l));
+    point.y = fence_storage.read_uint32(i * sizeof(Vector2l) + 4);
+    return true;
+}
+
+// save a fence point to eeprom, returns true on successful save
+bool AC_PolyFence_loader::save_point_to_eeprom(uint16_t i, const Vector2l& point)
+{
+    // sanity check index
+    if (i >= max_points()) {
+        return false;
+    }
+
+    // write point to eeprom
+    fence_storage.write_uint32(i * sizeof(Vector2l), point.x);
+    fence_storage.write_uint32(i * sizeof(Vector2l)+4, point.y);
+    return true;
+}
+
+// validate array of boundary points (expressed as either floats or long ints)
+//   contains_return_point should be true for plane which stores the return point as the first point in the array
+//   returns true if boundary is valid
+bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2l* points, bool contains_return_point) const
+{
+    // exit immediate if no points
+    if (points == NULL) {
+        return false;
+    }
+
+    // start from 2nd point if boundary contains return point (as first point)
+    uint8_t start_num = contains_return_point ? 1 : 0;
+
+    // a boundary requires at least 4 point (a triangle and last point equals first)
+    if (num_points < start_num + 4) {
+        return false;
+    }
+
+    // point 1 and last point must be the same.  Note: 0th point is reserved as the return point
+    if (!Polygon_complete(&points[start_num], num_points-start_num)) {
+        return false;
+    }
+
+    // check return point is within the fence
+    if (contains_return_point && Polygon_outside(points[0], &points[1], num_points-start_num)) {
+        return false;
+    }
+
+    return true;
+}
+
+bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2f* points, bool contains_return_point) const
+{
+    // exit immediate if no points
+    if (points == NULL) {
+        return false;
+    }
+
+    // start from 2nd point if boundary contains return point (as first point)
+    uint8_t start_num = contains_return_point ? 1 : 0;
+
+    // a boundary requires at least 4 point (a triangle and last point equals first)
+    if (num_points < start_num + 4) {
+        return false;
+    }
+
+    // point 1 and last point must be the same.  Note: 0th point is reserved as the return point
+    if (!Polygon_complete(&points[start_num], num_points-start_num)) {
+        return false;
+    }
+
+    // check return point is within the fence
+    if (contains_return_point && Polygon_outside(points[0], &points[1], num_points-start_num)) {
+        return false;
+    }
+
+    return true;
+}
+
+// check if a location (expressed as either floats or long ints) is within the boundary
+//   contains_return_point should be true for plane which stores the return point as the first point in the array
+//   returns true if location is outside the boundary
+bool AC_PolyFence_loader::boundary_breached(const Vector2l& location, uint16_t num_points, const Vector2l* points, bool contains_return_point) const
+{
+    // exit immediate if no points
+    if (points == NULL) {
+        return false;
+    }
+
+    // start from 2nd point if boundary contains return point (as first point)
+    uint8_t start_num = contains_return_point ? 1 : 0;
+
+    // check location is within the fence
+    return Polygon_outside(location, &points[start_num], num_points-start_num);
+}
+
+bool AC_PolyFence_loader::boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points, bool contains_return_point) const
+{
+    // exit immediate if no points
+    if (points == NULL) {
+        return false;
+    }
+
+    // start from 2nd point if boundary contains return point (as first point)
+    uint8_t start_num = contains_return_point ? 1 : 0;
+
+    // check location is within the fence
+    return Polygon_outside(location, &points[start_num], num_points-start_num);
+}

libraries/AC_Fence/AC_PolyFence_loader.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+
+class AC_PolyFence_loader
+{
+
+public:
+
+    // maximum number of fence points we can store in eeprom
+    uint8_t max_points() const;
+
+    // create buffer to hold copy of eeprom points in RAM
+    // returns NULL if not enough memory can be allocated
+    void* create_point_array(uint8_t element_size);
+
+    // load boundary point from eeprom, returns true on successful load
+    bool load_point_from_eeprom(uint16_t i, Vector2l& point);
+
+    // save a fence point to eeprom, returns true on successful save
+    bool save_point_to_eeprom(uint16_t i, const Vector2l& point);
+
+    // validate array of boundary points (expressed as either floats or long ints)
+    //   contains_return_point should be true for plane which stores the return point as the first point in the array
+    //   returns true if boundary is valid
+    bool boundary_valid(uint16_t num_points, const Vector2l* points, bool contains_return_point) const;
+    bool boundary_valid(uint16_t num_points, const Vector2f* points, bool contains_return_point) const;
+
+    // check if a location (expressed as either floats or long ints) is within the boundary
+    //   contains_return_point should be true for plane which stores the return point as the first point in the array
+    //   returns true if location is outside the boundary
+    bool boundary_breached(const Vector2l& location, uint16_t num_points, const Vector2l* points, bool contains_return_point) const;
+    bool boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points, bool contains_return_point) const;
+
+};
+