Plane: added FENCE_AUTOENABLE=3 option

this enables the fence when arming. If the vehicle is outside the
fence or it can't be enabled then arming fails

ArduPlane/AP_Arming.cpp

@@ -145,6 +145,19 @@ bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
         gcs().send_text(MAV_SEVERITY_WARNING, "watchdog: Bypassing arming checks");
         return true;
     }
+
+    if (plane.g.fence_autoenable == 3) {
+        if (!plane.geofence_set_enabled(true, AUTO_TOGGLED)) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Fence: cannot enable for arming");
+            return false;
+        } else if (!plane.geofence_prearm_check()) {
+            plane.geofence_set_enabled(false, AUTO_TOGGLED);
+            return false;
+        } else {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Fence: auto-enabled for arming");
+        }
+    }
+    
     // call parent class checks
     return AP_Arming::arm_checks(method);
 }
@@ -205,6 +218,10 @@ bool AP_Arming_Plane::disarm(void)
 
     gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed");
 
+    if (plane.g.fence_autoenable == 3) {
+        plane.geofence_set_enabled(false, AUTO_TOGGLED);
+    }
+    
     return true;
 }
 

ArduPlane/Parameters.cpp

@@ -328,8 +328,8 @@ const AP_Param::Info Plane::var_info[] = {
 
     // @Param: FENCE_AUTOENABLE
     // @DisplayName: Fence automatic enable
-    // @Description: When set to 1, geofence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing.  When on the ground before takeoff the fence is disabled.  When set to 2, the fence autoenables after an auto takeoff, but only disables the fence floor during landing. It is highly recommended to not use this option for line of sight flying and use a fence enable channel instead.
-    // @Values: 0:NoAutoEnable,1:AutoEnable,2:AutoEnableDisableFloorOnly
+    // @Description: When set to 1, geofence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing.  When on the ground before takeoff the fence is disabled.  When set to 2, the fence autoenables after an auto takeoff, but only disables the fence floor during landing. It is highly recommended to not use this option for line of sight flying and use a fence enable channel instead. When set to 3 the fence auto-enables when te vehicle is armed, and arming will fail if the fence cannot be enabled or is outside the fence. Option 3 cannot be used with a non-zero FENCE_MINALT
+    // @Values: 0:NoAutoEnable,1:AutoEnable,2:AutoEnableDisableFloorOnly,3:AutoEnableArming
     // @User: Standard
     GSCALAR(fence_autoenable,       "FENCE_AUTOENABLE", 0),
 

ArduPlane/Plane.h

@@ -906,6 +906,7 @@ private:
     bool geofence_check_minalt(void);
     bool geofence_check_maxalt(void);
     void geofence_check(bool altitude_check_only);
+    bool geofence_prearm_check(void);
     bool geofence_stickmixing(void);
     void geofence_send_status(mavlink_channel_t chan);
     bool geofence_breached(void);

ArduPlane/geofence.cpp

@@ -280,6 +280,52 @@ bool Plane::geofence_check_maxalt(void)
     return (adjusted_altitude_cm() > (g.fence_maxalt*100.0f) + home.alt);
 }
 
+/*
+  pre-arm check for being inside the fence
+ */
+bool Plane::geofence_prearm_check(void)
+{
+    if (!geofence_enabled()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: Fence not enabled");
+        return false;
+    }
+
+    /* allocate the geo-fence state if need be */
+    if (geofence_state == nullptr || !geofence_state->boundary_uptodate) {
+        geofence_load();
+        if (!geofence_enabled()) {
+            // may have been disabled by load
+            gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: Fence load failed");
+            return false;
+        }
+    }
+
+    if (geofence_state->floor_enabled && g.fence_minalt != 0) {
+        // can't use minalt with prearm check
+        gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: Fence floor enabled");
+        return false;
+    }
+    if (geofence_check_maxalt()) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: maxalt breached");
+        return false;
+    }
+    struct Location loc;
+    if (!ahrs.get_position(loc)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: no position available");
+        // must have position
+        return false;
+    }
+    Vector2l location;
+    location.x = loc.lat;
+    location.y = loc.lng;
+    bool outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
+    if (outside) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: outside fence");
+        return false;
+    }
+    return true;
+}
+
 
 /*
  *  check if we have breached the geo-fence