diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 38d20273297..014bfa3e1db 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/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(); +} diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 7993a3a2bb4..305233e38a3 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/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