AC_Fence: add 10sec manual recovery

This resolves issue #461 by giving the pilot a minimum of 10 seconds to
attempt to manually recover before the autopilot will attempt to retake
control to bring the copter home or land.

libraries/AC_Fence/AC_Fence.cpp

@@ -64,7 +64,8 @@ AC_Fence::AC_Fence(const AP_InertialNav* inav) :
     _home_distance(0),
     _breached_fences(AC_FENCE_TYPE_NONE),
     _breach_time(0),
-    _breach_count(0)
+    _breach_count(0),
+    _manual_recovery_start_ms(0)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -119,6 +120,17 @@ uint8_t AC_Fence::check_fence()
         return AC_FENCE_TYPE_NONE;
     }
 
+    // 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 ((hal.scheduler->millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) {
+            return AC_FENCE_TYPE_NONE;
+        } else {
+            // recovery period has passed so reset manual recovery time and continue with fence breach checks
+            _manual_recovery_start_ms = 0;
+        }
+    }
+
     // get current altitude in meters
     float curr_alt = _inav->get_altitude() * 0.01f;
 
@@ -233,3 +245,16 @@ float AC_Fence::get_breach_distance(uint8_t fence_type) const
     // we don't recognise the fence type so just return 0
     return 0;
 }
+
+/// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds
+///     has no effect if no breaches have occurred
+void AC_Fence::manual_recovery_start()
+{
+    // return immediate if we haven't breached a fence
+    if (_breached_fences == AC_FENCE_TYPE_NONE) {
+        return;
+    }
+
+    // record time pilot began manual recovery
+    _manual_recovery_start_ms = hal.scheduler->millis();
+}

libraries/AC_Fence/AC_Fence.h

@@ -26,6 +26,7 @@
 
 // give up distance
 #define AC_FENCE_GIVE_UP_DISTANCE                   100.0f  // distance outside the fence at which we should give up and just land.  Note: this is not used by library directly but is intended to be used by the main code
+#define AC_FENCE_MANUAL_RECOVERY_TIME_MIN           10000   // pilot has 10seconds to recover during which time the autopilot will not attempt to re-take control
 
 class AC_Fence
 {
@@ -70,7 +71,12 @@ public:
 
     /// get_safe_alt - returns maximum safe altitude (i.e. alt_max - margin)
     float get_safe_alt() const { return _alt_max - _margin; }
-    
+
+    /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds
+    ///     should be called whenever the pilot changes the flight mode
+    ///     has no effect if no breaches have occurred
+    void manual_recovery_start();
+
     ///
     /// time saving methods to piggy-back on main code's calculations
     ///
@@ -114,5 +120,7 @@ private:
     uint8_t         _breached_fences;       // bitmask holding the fence type that was breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
     uint32_t        _breach_time;           // time of last breach in milliseconds
     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
 };
 #endif	// AC_FENCE_H