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

libraries/AC_Fence/AC_Fence.cpp

@@ -126,14 +126,15 @@ 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),
 
     // @Param{Plane}: OPTIONS
     // @DisplayName: Fence options
@@ -470,6 +471,33 @@ 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();
+
+        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 +568,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 +580,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) {
+        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 +607,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 +722,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 +784,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);