AC_Fence: move polygon points into AC_Fence_Polygon

libraries/AC_Fence/AC_Fence.cpp

@@ -116,7 +116,7 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
         return true;
     }
 
-    if (!_boundary_valid) {
+    if (!_poly_loader.valid()) {
         fail_msg = "Polygon boundary invalid";
         return false;
     }
@@ -258,28 +258,10 @@ bool AC_Fence::polygon_fence_is_breached()
         return false;
     }
 
-    // check consistency of number of points
-    if (_boundary_num_points != _total) {
-        // Fence is currently not completely loaded.  Can't breach it?!
-        load_polygon_from_eeprom();
-        return false;
-    }
-    if (!_boundary_valid) {
-        // fence isn't valid - can't breach it?!
-        return false;
-    }
-
-    // check if vehicle is outside the polygon fence
-    Vector2f position;
-    if (!AP::ahrs().get_relative_position_NE_origin(position)) {
-        // we have no idea where we are; can't breach the fence
-        return false;
-    }
-
-    position = position * 100.0f;  // m to cm
-    return _poly_loader.boundary_breached(position, _boundary_num_points, _boundary);
+    return _poly_loader.breached();
 }
 
+
 bool AC_Fence::check_fence_circle()
 {
     if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) {
@@ -385,13 +367,9 @@ bool AC_Fence::check_destination_within_fence(const Location& loc)
     }
 
     // polygon fence check
-    if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON) && _boundary_num_points > 0) {
-        // check ekf has a good location
-        Vector2f posNE;
-        if (loc.get_vector_xy_from_origin_NE(posNE)) {
-            if (_poly_loader.boundary_breached(posNE, _boundary_num_points, _boundary)) {
-                return false;
-            }
+    if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) {
+        if (_poly_loader.breached(loc)) {
+            return false;
         }
     }
 
@@ -449,17 +427,17 @@ void AC_Fence::manual_recovery_start()
 }
 
 /// returns pointer to array of polygon points and num_points is filled in with the total number
-Vector2f* AC_Fence::get_boundary_points(uint16_t& num_points) const
+Vector2f* AC_PolyFence_loader::get_boundary_points(uint16_t& num_points) const
 {
     // return array minus the first point which holds the return location
     if (_boundary == nullptr) {
         return nullptr;
     }
-    if (!_boundary_valid) {
+    if (!valid()) {
         return nullptr;
     }
     // minus one for return point, minus one for closing point
-    // (_boundary_valid is not true unless we have a closing point AND
+    // (polygon().valid() is not true unless we have a closing point AND
     // we have a minumum number of points)
     if (_boundary_num_points < 2) {
         return nullptr;
@@ -468,14 +446,8 @@ Vector2f* AC_Fence::get_boundary_points(uint16_t& num_points) const
     return &_boundary[1];
 }
 
-/// returns true if we've breached the polygon boundary.  simple passthrough to underlying _poly_loader object
-bool AC_Fence::boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points) const
-{
-    return _poly_loader.boundary_breached(location, num_points, points);
-}
-
 /// handler for polygon fence messages with GCS
-void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
+void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg)
 {
     switch (msg.msgid) {
         // receive a fence point from GCS and store in EEPROM
@@ -488,7 +460,7 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
                 Vector2l point;
                 point.x = packet.lat*1.0e7f;
                 point.y = packet.lng*1.0e7f;
-                if (!_poly_loader.save_point_to_eeprom(packet.idx, point)) {
+                if (!save_point_to_eeprom(packet.idx, point)) {
                     link.send_text(MAV_SEVERITY_WARNING, "Failed to save polygon point, too many points?");
                 } else {
                     // trigger reload of points
@@ -504,7 +476,7 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
             mavlink_msg_fence_fetch_point_decode(&msg, &packet);
             // attempt to retrieve from eeprom
             Vector2l point;
-            if (_poly_loader.load_point_from_eeprom(packet.idx, point)) {
+            if (load_point_from_eeprom(packet.idx, point)) {
                 mavlink_msg_fence_point_send(link.get_chan(), msg.sysid, msg.compid, packet.idx, _total, point.x*1.0e-7f, point.y*1.0e-7f);
             } else {
                 link.send_text(MAV_SEVERITY_WARNING, "Bad fence point");
@@ -519,12 +491,12 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg)
 }
 
 /// load polygon points stored in eeprom into boundary array and perform validation
-bool AC_Fence::load_polygon_from_eeprom()
+bool AC_PolyFence_loader::load_from_eeprom()
 {
     // check if we need to create array
-    if (!_boundary_create_attempted) {
-        _boundary = (Vector2f *)_poly_loader.create_point_array(sizeof(Vector2f));
-        _boundary_create_attempted = true;
+    if (!_create_attempted) {
+        _boundary = (Vector2f *)create_point_array(sizeof(Vector2f));
+        _create_attempted = true;
     }
 
     // exit if we could not allocate RAM for the boundary
@@ -543,13 +515,13 @@ bool AC_Fence::load_polygon_from_eeprom()
     }
 
     // sanity check total
-    _total = constrain_int16(_total, 0, _poly_loader.max_points());
+    _total = constrain_int16(_total, 0, max_points());
 
     // load each point from eeprom
     Vector2l temp_latlon;
     for (uint16_t index=0; index<_total; index++) {
         // load boundary point as lat/lon point
-        if (!_poly_loader.load_point_from_eeprom(index, temp_latlon)) {
+        if (!load_point_from_eeprom(index, temp_latlon)) {
             return false;
         }
         // move into location structure and convert to offset from ekf origin
@@ -558,10 +530,10 @@ bool AC_Fence::load_polygon_from_eeprom()
         _boundary[index] = ekf_origin.get_distance_NE(temp_loc) * 100.0f;
     }
     _boundary_num_points = _total;
-    _boundary_update_ms = AP_HAL::millis();
+    _update_ms = AP_HAL::millis();
 
     // update validity of polygon
-    _boundary_valid = _poly_loader.boundary_valid(_boundary_num_points, _boundary);
+    _valid = calculate_boundary_valid();
 
     return true;
 }
@@ -593,7 +565,7 @@ bool AC_Fence::sys_status_failed() const
         return true;
     }
     if (_enabled_fences & AC_FENCE_TYPE_POLYGON) {
-        if (!_boundary_valid) {
+        if (!_poly_loader.valid()) {
             return true;
         }
     }

libraries/AC_Fence/AC_Fence.h

@@ -96,26 +96,6 @@ public:
     ///     has no effect if no breaches have occurred
     void manual_recovery_start();
 
-    ///
-    /// polygon related methods
-    ///
-
-    /// returns true if polygon fence is valid (i.e. has at least 3 sides)
-    bool is_polygon_valid() const { return _boundary_valid; }
-
-    /// returns pointer to array of polygon points and num_points is filled in with the total number
-    /// points are offsets from EKF origin in NE frame
-    Vector2f* get_boundary_points(uint16_t& num_points) const;
-
-    /// returns true if we've breached the polygon boundary.  simple passthrough to underlying _poly_loader object
-    bool boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points) const;
-
-    /// handler for polygon fence messages with GCS
-    void handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg);
-
-    /// return system time of last update to the boundary (allows external detection of boundary changes)
-    uint32_t get_boundary_update_ms() const { return _boundary_update_ms; }
-
     static const struct AP_Param::GroupInfo var_info[];
 
     // methods for mavlink SYS_STATUS message (send_sys_status)
@@ -126,6 +106,9 @@ public:
     // get singleton instance
     static AC_Fence *get_singleton() { return _singleton; }
 
+    AC_PolyFence_loader &polyfence() { return _poly_loader; }
+    const AC_PolyFence_loader &polyfence_const() const { return _poly_loader; }
+
 private:
     static AC_Fence *_singleton;
 
@@ -149,9 +132,6 @@ private:
     bool pre_arm_check_circle(const char* &fail_msg) const;
     bool pre_arm_check_alt(const char* &fail_msg) const;
 
-    /// load polygon points stored in eeprom into boundary array and perform validation.  returns true if load successfully completed
-    bool load_polygon_from_eeprom();
-
     // returns true if we have breached the fence:
     bool polygon_fence_is_breached();
 
@@ -185,13 +165,8 @@ private:
 
     uint32_t        _manual_recovery_start_ms;  // system time in milliseconds that pilot re-took manual control
 
-    // polygon fence variables
-    AC_PolyFence_loader _poly_loader;               // helper for loading/saving polygon points
-    Vector2f        *_boundary = nullptr;           // array of boundary points.  Note: point 0 is the return point
-    uint8_t         _boundary_num_points = 0;       // number of points in the boundary array (should equal _total parameter after load has completed)
-    bool            _boundary_create_attempted = false; // true if we have attempted to create the boundary array
-    bool            _boundary_valid = false;        // true if boundary forms a closed polygon
-    uint32_t        _boundary_update_ms;            // system time of last update to the boundary
+    AC_PolyFence_loader _poly_loader{_total}; // polygon fence
+
 };
 
 namespace AP {

libraries/AC_Fence/AC_PolyFence_loader.cpp

@@ -53,13 +53,12 @@ bool AC_PolyFence_loader::save_point_to_eeprom(uint16_t i, const Vector2l& point
     return true;
 }
 
-// validate array of boundary points (expressed as either floats or long ints)
+// validate array of boundary points
 //   returns true if boundary is valid
-template <typename T>
-bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>* points) const
+bool AC_PolyFence_loader::calculate_boundary_valid() const
 {
     // exit immediate if no points
-    if (points == nullptr) {
+    if (_boundary == nullptr) {
         return false;
     }
 
@@ -67,30 +66,61 @@ bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2<T>*
     uint8_t start_num = 1;
 
     // a boundary requires at least 4 point (a triangle and last point equals first)
-    if (num_points < start_num + 4) {
+    if (_boundary_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)) {
+    if (!Polygon_complete(&_boundary[start_num], _boundary_num_points-start_num)) {
         return false;
     }
 
     // check return point is within the fence
-    if (Polygon_outside(points[0], &points[1], num_points-start_num)) {
+    if (Polygon_outside(_boundary[0], &_boundary[1], _boundary_num_points-start_num)) {
         return false;
     }
 
     return true;
 }
 
-// check if a location (expressed as either floats or long ints) is within the boundary
+bool AC_PolyFence_loader::breached()
+{
+    // check if vehicle is outside the polygon fence
+    Vector2f position;
+    if (!AP::ahrs().get_relative_position_NE_origin(position)) {
+        // we have no idea where we are; can't breach the fence
+        return false;
+    }
+
+    position = position * 100.0f;  // m to cm
+    return breached(position);
+}
+
+bool AC_PolyFence_loader::breached(const Location& loc)
+{
+    Vector2f posNE;
+    if (!loc.get_vector_xy_from_origin_NE(posNE)) {
+        // not breached if we don't now where we are
+        return false;
+    }
+    return breached(posNE);
+}
+
 //   returns true if location is outside the boundary
-template <typename T>
-bool AC_PolyFence_loader::boundary_breached(const Vector2<T>& location, uint16_t num_points, const Vector2<T>* points) const
+bool AC_PolyFence_loader::breached(const Vector2f& location)
 {
+    // check consistency of number of points
+    if (_boundary_num_points != _total) {
+        // Fence is currently not completely loaded.  Can't breach it?!
+        load_from_eeprom();
+        return false;
+    }
     // exit immediate if no points
-    if (points == nullptr) {
+    if (_boundary == nullptr) {
+        return false;
+    }
+    if (!_valid) {
+        // fence isn't valid - can't breach it?!
         return false;
     }
 
@@ -98,13 +128,5 @@ bool AC_PolyFence_loader::boundary_breached(const Vector2<T>& location, uint16_t
     uint8_t start_num = 1;
 
     // check location is within the fence
-    return Polygon_outside(location, &points[start_num], num_points-start_num);
+    return Polygon_outside(location, &_boundary[start_num], _boundary_num_points-start_num);
 }
-
-// declare type specific methods
-template bool AC_PolyFence_loader::boundary_valid<int32_t>(uint16_t num_points, const Vector2l* points) const;
-template bool AC_PolyFence_loader::boundary_valid<float>(uint16_t num_points, const Vector2f* points) const;
-template bool AC_PolyFence_loader::boundary_breached<int32_t>(const Vector2l& location, uint16_t num_points,
-                                                              const Vector2l* points) const;
-template bool AC_PolyFence_loader::boundary_breached<float>(const Vector2f& location, uint16_t num_points,
-                                                            const Vector2f* points) const;

libraries/AC_Fence/AC_PolyFence_loader.h

@@ -2,12 +2,49 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
 
 class AC_PolyFence_loader
 {
 
 public:
 
+    AC_PolyFence_loader(AP_Int8 &total) :
+        _total(total) {}
+
+    AC_PolyFence_loader(const AC_PolyFence_loader &other) = delete;
+    AC_PolyFence_loader &operator=(const AC_PolyFence_loader&) = delete;
+
+    /// returns pointer to array of polygon points and num_points is
+    /// filled in with the total number.  This does not include the
+    /// return point or the closing point.
+    Vector2f* get_boundary_points(uint16_t& num_points) const;
+
+    /// handler for polygon fence messages with GCS
+    void handle_msg(GCS_MAVLINK &link, mavlink_message_t* msg);
+
+    bool breached();
+    //   returns true if location is outside the boundary
+    bool breached(const Location& loc);
+
+    //   returns true if boundary is valid
+    bool valid() const {
+        return _valid;
+    }
+
+    // returns the system in in millis when the boundary was last updated
+    uint32_t update_ms() const {
+        return _update_ms;
+    }
+
+private:
+
+    bool breached(const Vector2f& location);
+    /// load polygon points stored in eeprom into boundary array and
+    /// perform validation.  returns true if load successfully
+    /// completed
+    bool load_from_eeprom();
+
     // maximum number of fence points we can store in eeprom
     uint8_t max_points() const;
 
@@ -21,16 +58,15 @@ public:
     // save a fence point to eeprom, returns true on successful save
     bool save_point_to_eeprom(uint16_t i, const Vector2l& point);
 
+    // update the validity flag:
+    bool calculate_boundary_valid() const;
 
-    // validate array of boundary points (expressed as either floats or long ints)
-    //   returns true if boundary is valid
-    template <typename T>
-    bool boundary_valid(uint16_t num_points, const Vector2<T>* points) const;
-
-    // check if a location (expressed as either floats or long ints) is within the boundary
-    //   returns true if location is outside the boundary
-    template <typename T>
-    bool boundary_breached(const Vector2<T>& location, uint16_t num_points, const Vector2<T>* points) const;
+    Vector2f *_boundary;          // array of boundary points.  Note: point 0 is the return point
+    uint8_t _boundary_num_points; // number of points in the boundary array (should equal _total parameter after load has completed)
+    bool   _create_attempted;     // true if we have attempted to create the boundary array
+    bool   _valid;                // true if boundary forms a closed polygon
+    uint32_t        _update_ms;   // system time of last update to the boundary
 
+    AP_Int8 &_total;
 };