AC_Fence: add ability to auto-enable fence floor while doing fence ch…

…ecks

add armed boolean to check
control copter floor fence with autoenable
autoenable floor fence with a margin
check for manual recovery only after having checked the fences
make auto-disabling for minimum altitude fence an option
output message when fence floor auto-enabled

libraries/AC_Fence/AC_Fence.cpp

@@ -126,19 +126,20 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
     // @User: Standard
     AP_GROUPINFO_FRAME("RET_ALT",   9,  AC_Fence,   _ret_altitude,       0.0f, AP_PARAM_FRAME_PLANE),
 
-    // @Param{Plane}: AUTOENABLE
+    // @Param{Plane, Copter}: AUTOENABLE
     // @DisplayName: Fence Auto-Enable
     // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences after autotakeoffs reach altitude. During autolandings the fences will be disabled.  AutoEnableDisableFloorOnLanding enables all configured fences after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences, but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings.
-    // @Values: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed
+    // @Values{Plane}: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed
+    // @Values{Copter}: 0:AutoEnableOff,1:AutoEnableFloorOnTakeoff
     // @Range: 0 3
     // @Increment: 1
     // @User: Standard
-    AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast<uint8_t>(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE),
+    AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast<uint8_t>(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI),
 
     // @Param{Plane}: OPTIONS
     // @DisplayName: Fence options
-    // @Description: 0:Disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas.
-    // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas
+    // @Description: 0:Disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. When bit 2 is set any active minimum altitude floor will be disabled.
+    // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas, 2:Auto-disable floor fence on disarming
     // @User: Standard
     AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast<uint16_t>(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE),
 
@@ -470,6 +471,34 @@ bool AC_Fence::check_fence_alt_min()
     return false;
 }
 
+
+/// auto enable fence floor
+bool AC_Fence::auto_enable_fence_floor()
+{
+    if (_floor_enabled) {
+        return true; // already enabled
+    }
+
+    // altitude fence check
+    if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MIN)) {
+        // not enabled
+        return false;
+    }
+
+    AP::ahrs().get_relative_position_D_home(_curr_alt);
+    _curr_alt = -_curr_alt; // translate Down to Up
+
+    // check if we are under the altitude fence
+    if (_curr_alt >= _alt_min + _margin) {
+        enable_floor();
+        gcs().send_text(MAV_SEVERITY_INFO, "Fence floor enabled (auto enabled)");
+
+        return true;
+    }
+
+    return false;
+}
+
 // check_fence_polygon - returns true if the poly fence is freshly
 // breached.  That includes being inside exclusion zones and outside
 // inclusions zones
@@ -540,7 +569,7 @@ bool AC_Fence::check_fence_circle()
 
 
 /// check - returns bitmask of fence types breached (if any)
-uint8_t AC_Fence::check()
+uint8_t AC_Fence::check(bool armed)
 {
     uint8_t ret = 0;
 
@@ -552,22 +581,18 @@ uint8_t AC_Fence::check()
         return 0;
     }
 
-    // check if pilot is attempting to recover manually
-    if (_manual_recovery_start_ms != 0) {
-        // we ignore any fence breaches during the manual recovery period which is about 10 seconds
-        if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) {
-            return 0;
-        }
-        // recovery period has passed so reset manual recovery time
-        // and continue with fence breach checks
-        _manual_recovery_start_ms = 0;
-    }
-
     // maximum altitude fence check
     if (check_fence_alt_max()) {
         ret |= AC_FENCE_TYPE_ALT_MAX;
     }
 
+    // auto enable floor check
+    if (!armed && option_enabled(AC_Fence::OPTIONS::AUTO_DISABLE_FLOOR_ON_DISARM)) {
+        disable_floor();    // this also resets the alt min fence breach
+    } else if (!_floor_enabled && auto_enabled() == AC_Fence::AutoEnable::ALWAYS_ENABLED) {
+        auto_enable_fence_floor();
+    }
+
     // minimum altitude fence check
     if (_floor_enabled && check_fence_alt_min()) {
         ret |= AC_FENCE_TYPE_ALT_MIN;
@@ -583,6 +608,18 @@ uint8_t AC_Fence::check()
         ret |= AC_FENCE_TYPE_POLYGON;
     }
 
+    // check if pilot is attempting to recover manually
+    // this is done last so that _breached_fences is correct
+    if (_manual_recovery_start_ms != 0) {
+        // we ignore any fence breaches during the manual recovery period which is about 10 seconds
+        if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) {
+            return 0;
+        }
+        // recovery period has passed so reset manual recovery time
+        // and continue with fence breach checks
+        _manual_recovery_start_ms = 0;
+    }
+
     // return any new breaches that have occurred
     return ret;
 }
@@ -686,6 +723,8 @@ void AC_Fence::manual_recovery_start()
 
     // record time pilot began manual recovery
     _manual_recovery_start_ms = AP_HAL::millis();
+
+    gcs().send_text(MAV_SEVERITY_INFO, "Manual recovery started");
 }
 
 // methods for mavlink SYS_STATUS message (send_sys_status)
@@ -746,7 +785,7 @@ uint8_t AC_Fence::get_enabled_fences() const { return 0; }
 
 bool AC_Fence::pre_arm_check(const char* &fail_msg) const  { return true; }
 
-uint8_t AC_Fence::check() { return 0; }
+uint8_t AC_Fence::check(bool armed) { return 0; }
 bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; }
 float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; }
 

libraries/AC_Fence/AC_Fence.h

@@ -34,7 +34,7 @@ class AC_Fence
 public:
     friend class AC_PolyFence_loader;
 
-    enum class AutoEnable
+    enum class AutoEnable : uint8_t
     {
         ALWAYS_DISABLED = 0,
         ALWAYS_ENABLED = 1,
@@ -72,6 +72,9 @@ class AC_Fence
     /// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing.
     void auto_disable_fence_for_landing();
 
+    /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached.
+    bool auto_enable_fence_floor();
+
     /// enabled - returns true if fence is enabled
     bool enabled() const { return _enabled; }
 
@@ -94,7 +97,7 @@ class AC_Fence
     ///
 
     /// check - returns the fence type that has been breached (if any)
-    uint8_t check();
+    uint8_t check(bool armed = false);
 
     // returns true if the destination is within fence (used to reject waypoints outside the fence)
     bool check_destination_within_fence(const class Location& loc);
@@ -149,6 +152,7 @@ class AC_Fence
     enum class OPTIONS {
         DISABLE_MODE_CHANGE = 1U << 0,
         INCLUSION_UNION = 1U << 1,
+        AUTO_DISABLE_FLOOR_ON_DISARM = 1U << 2,
     };
     static bool option_enabled(OPTIONS opt, const AP_Int16 &options) {
         return (options.get() & int16_t(opt)) != 0;