AC_Fence: add support for polygon fences

libraries/AC_Fence/AC_Fence.cpp

@@ -1,6 +1,8 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 #include <AP_HAL/AP_HAL.h>
 #include "AC_Fence.h"
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -15,9 +17,10 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Fence Type
     // @Description: Enabled fence types held as bitmask
-    // @Values: 0:None,1:Altitude,2:Circle,3:Altitude and Circle
+    // @Values: 0:None,1:Altitude,2:Circle,3:Altitude and Circle,4:Polygon,5:Altitude and Polygon,6:Circle and Polygon,7:All
+    // @Bitmask: 0:Altitude,1:Circle,2:Polygon
     // @User: Standard
-    AP_GROUPINFO("TYPE",        1,  AC_Fence,   _enabled_fences,  AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE),
+    AP_GROUPINFO("TYPE",        1,  AC_Fence,   _enabled_fences,  AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON),
 
     // @Param: ACTION
     // @DisplayName: Fence Action
@@ -50,7 +53,14 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
     // @Range: 1 10
     // @User: Standard
     AP_GROUPINFO("MARGIN",      5,  AC_Fence,   _margin, AC_FENCE_MARGIN_DEFAULT),
-    
+
+    // @Param: TOTAL
+    // @DisplayName: Fence polygon point total
+    // @Description: Number of polygon points saved in eeprom (do not update manually)
+    // @Range: 1 20
+    // @User: Standard
+    AP_GROUPINFO("TOTAL",       6,  AC_Fence,   _total, 0),
+
     AP_GROUPEND
 };
 
@@ -190,11 +200,32 @@ uint8_t AC_Fence::check_fence(float curr_alt)
         }
     }
 
+    // polygon fence check
+    if ((_enabled_fences & AC_FENCE_TYPE_POLYGON) != 0 ) {
+        // load fence if necessary
+        if (!_boundary_loaded) {
+            load_polygon_from_eeprom();
+        } else if (_boundary_valid) {
+            // check if vehicle is outside the polygon fence
+            const Vector3f& position = _inav.get_position();
+            if (_poly_loader.boundary_breached(Vector2f(position.x, position.y), _boundary_num_points, _boundary, true)) {
+                // check if this is a new breach
+                if ((_breached_fences & AC_FENCE_TYPE_POLYGON) == 0) {
+                    // record that we have breached the polygon
+                    record_breach(AC_FENCE_TYPE_POLYGON);
+                    ret = ret | AC_FENCE_TYPE_POLYGON;
+                }
+            } else {
+                // clear breach if present
+                if ((_breached_fences & AC_FENCE_TYPE_POLYGON) != 0) {
+                    clear_breach(AC_FENCE_TYPE_POLYGON);
+                }
+            }
+        }
+    }
+
     // return any new breaches that have occurred
     return ret;
-
-    // To-Do: add min alt and polygon check
-    //outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
 }
 
 // returns true if the destination is within fence (used to reject waypoints outside the fence)
@@ -272,3 +303,110 @@ void AC_Fence::manual_recovery_start()
     // record time pilot began manual recovery
     _manual_recovery_start_ms = AP_HAL::millis();
 }
+
+/// returns pointer to array of polygon points and num_points is filled in with the total number
+Vector2f* AC_Fence::get_polygon_points(uint16_t& num_points) const
+{
+    num_points = _boundary_num_points;
+    return _boundary;
+}
+
+/// 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, true);
+}
+
+/// handler for polygon fence messages with GCS
+void AC_Fence::handle_msg(mavlink_channel_t chan, mavlink_message_t* msg)
+{
+    // exit immediately if null message
+    if (msg == NULL) {
+        return;
+    }
+
+    switch (msg->msgid) {
+        // receive a fence point from GCS and store in EEPROM
+        case MAVLINK_MSG_ID_FENCE_POINT: {
+            mavlink_fence_point_t packet;
+            mavlink_msg_fence_point_decode(msg, &packet);
+            if (!check_latlng(packet.lat,packet.lng)) {
+                GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_WARNING, chan, "Invalid fence point, lat or lng too large");
+            } else {
+                Vector2l point;
+                point.x = packet.lat*1.0e7f;
+                point.y = packet.lng*1.0e7f;
+                if (!_poly_loader.save_point_to_eeprom(packet.idx, point)) {
+                    GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_WARNING, chan, "Failed to save polygon point, too many points?");
+                }
+            }
+            break;
+        }
+
+        // send a fence point to GCS
+        case MAVLINK_MSG_ID_FENCE_FETCH_POINT: {
+            mavlink_fence_fetch_point_t packet;
+            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)) {
+                mavlink_msg_fence_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, _total, point.x*1.0e-7f, point.y*1.0e-7f);
+            } else {
+                GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_WARNING, chan, "Bad fence point");
+            }
+            break;
+        }
+
+        default:
+            // do nothing
+            break;
+    }
+}
+
+/// load polygon points stored in eeprom into boundary array and perform validation
+bool AC_Fence::load_polygon_from_eeprom(bool force_reload)
+{
+    // exit immediately if already loaded
+    if (_boundary_loaded && !force_reload) {
+        return true;
+    }
+
+    // check if we need to create array
+    if (!_boundary_create_attempted) {
+        _boundary = (Vector2f *)_poly_loader.create_point_array(sizeof(Vector2f));
+        _boundary_create_attempted = true;
+    }
+
+    // exit if we could not allocate RAM for the boundary
+    if (_boundary == NULL) {
+        return false;
+    }
+
+    // get current location from EKF
+    Location temp_loc;
+    if (!_inav.get_location(temp_loc)) {
+        return false;
+    }
+    const struct Location &ekf_origin = _inav.get_origin();
+
+    // sanity check total
+    _total = constrain_int16(_total, 0, _poly_loader.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
+        _poly_loader.load_point_from_eeprom(index, temp_latlon);
+        // move into location structure and convert to offset from ekf origin
+        temp_loc.lat = temp_latlon.x;
+        temp_loc.lng = temp_latlon.y;
+        _boundary[index] = location_diff(ekf_origin, temp_loc) * 100.0f;
+    }
+    _boundary_num_points = _total;
+    _boundary_loaded = true;
+
+    // update validity of polygon
+    _boundary_valid = _poly_loader.boundary_valid(_boundary_num_points, _boundary, true);
+
+    return true;
+}

libraries/AC_Fence/AC_Fence.h

@@ -5,12 +5,15 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_InertialNav/AP_InertialNav.h>     // Inertial Navigation library
+#include <AP_Fence/AP_PolyFence_loader.h>
 
 // bit masks for enabled fence types.  Used for TYPE parameter
 #define AC_FENCE_TYPE_NONE                          0       // fence disabled
 #define AC_FENCE_TYPE_ALT_MAX                       1       // high alt fence which usually initiates an RTL
 #define AC_FENCE_TYPE_CIRCLE                        2       // circular horizontal fence (usually initiates an RTL)
+#define AC_FENCE_TYPE_POLYGON                       4       // polygon horizontal fence
 
 // valid actions should a fence be breached
 #define AC_FENCE_ACTION_REPORT_ONLY                 0       // report to GCS that boundary has been breached but take no further action
@@ -93,6 +96,19 @@ public:
     /// set_home_distance - update vehicle's distance from home in meters - required for circular horizontal fence monitoring
     void set_home_distance(float distance) { _home_distance = distance; }
 
+    ///
+    /// polygon related methods
+    ///
+
+    /// returns pointer to array of polygon points and num_points is filled in with the total number
+    Vector2f* get_polygon_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(mavlink_channel_t chan, mavlink_message_t* msg);
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -103,6 +119,9 @@ private:
     /// clear_breach - update breach bitmask, time and count
     void clear_breach(uint8_t fence_type);
 
+    /// load polygon points stored in eeprom into boundary array and perform validation.  returns true if load successfully completed
+    bool load_polygon_from_eeprom(bool force_reload = false);
+
     // pointers to other objects we depend upon
     const AP_InertialNav& _inav;
 
@@ -113,6 +132,7 @@ private:
     AP_Float        _alt_max;               // altitude upper limit in meters
     AP_Float        _circle_radius;         // circle fence radius in meters
     AP_Float        _margin;                // distance in meters that autopilot's should maintain from the fence to avoid a breach
+    AP_Int8         _total;                 // number of polygon points saved in eeprom
 
     // backup fences
     float           _alt_max_backup;        // backup altitude upper limit in meters used to refire the breach if the vehicle continues to move further away
@@ -131,4 +151,13 @@ private:
     uint16_t        _breach_count;          // number of times we have breached the fence
 
     uint32_t        _manual_recovery_start_ms;  // system time in milliseconds that pilot re-took manual control
+
+    // polygon fence variables
+    AP_PolyFence_loader _poly_loader;               // helper for loading/saving polygon points
+    Vector2f        *_boundary = NULL;              // 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_loaded = false;       // true if boundary array has been loaded from eeprom
+    bool            _boundary_valid = false;        // true if boundary forms a closed polygon
+    bool            _boundary_revalidate = false;   // set to true when we need to revalidate the boundary (required after a point is changed)
 };