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.
This commit is contained in:
Randy Mackay 2014-04-27 11:08:37 +09:00
parent 20de5b3006
commit e855cfec02
2 changed files with 35 additions and 2 deletions

View File

@ -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();
}

View File

@ -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
{
@ -71,6 +72,11 @@ 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