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 fence
-    // @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;