mirror of https://github.com/ArduPilot/ardupilot
AC_Fence: add ability to auto-enable fence floor while doing fence checks
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 re-use fence floor auto-enable/disable from plane auto-disable on landing do not update enable parameter when controlling through mavlink make sure get_enabled_fences() actually returns enabled fences. make current fences enabled internal state rather than persistent implement auto options correctly and on copter add fence names utility use ExpandingString for constructing fence names correctly check whether fences are enabled or not and disable min alt for landing in all auto modes add enable_configured() for use by mavlink and rc add events for all fence types make sure that auto fences are no longer candidates after manual updates add fence debug make sure rc switch is the ultimate authority on fences reset auto mask when enabling or disabling fencing ensure auto-enable on arming works as intended simplify printing fence notices reset autofences when auot-enablement is changed
This commit is contained in:
Andy Piper
Peter Barker
parent
370b0d7b9c
commit
f0f8187c7f
|
|
@ -40,22 +40,24 @@ extern const AP_HAL::HAL& hal;
|
|||
#define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0 // after fence is broken we recreate the fence 20m further out
|
||||
#endif
|
||||
|
||||
//#define AC_FENCE_DEBUG
|
||||
|
||||
const AP_Param::GroupInfo AC_Fence::var_info[] = {
|
||||
|
||||
// @Param: ENABLE
|
||||
// @DisplayName: Fence enable/disable
|
||||
// @Description: Allows you to enable (1) or disable (0) the fence functionality
|
||||
// @Description: Allows you to enable (1) or disable (0) the fence functionality. Fences can still be enabled and disabled via mavlink or an RC option, but these changes are not persisted.
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("ENABLE", 0, AC_Fence, _enabled, 0),
|
||||
|
||||
// @Param: TYPE
|
||||
// @DisplayName: Fence Type
|
||||
// @Description: Enabled fence types held as bitmask
|
||||
// @Description: Configured fence types held as bitmask. Max altitide, Circle and Polygon fences will be immediately enabled if configured. Min altitude fence will only be enabled once the minimum altitude is reached.
|
||||
// @Bitmask{Rover}: 1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons
|
||||
// @Bitmask{Copter, Plane, Sub}: 0:Max altitude,1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons,3:Min altitude
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TYPE", 1, AC_Fence, _enabled_fences, AC_FENCE_TYPE_DEFAULT),
|
||||
AP_GROUPINFO("TYPE", 1, AC_Fence, _configured_fences, AC_FENCE_TYPE_DEFAULT),
|
||||
|
||||
// @Param: ACTION
|
||||
// @DisplayName: Fence Action
|
||||
|
|
@ -126,18 +128,18 @@ 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 fences. AutoEnableOnTakeoff enables all configured fences after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences, but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings.
|
||||
// @Values: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed
|
||||
// @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences on arming, except the minimum altitude fence (which is enabled when the minimum altitude is reached), but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings.
|
||||
// @Values{Plane, Copter}: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed
|
||||
// @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.
|
||||
// @Description: When bit 0 is set sisable 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
|
||||
// @User: Standard
|
||||
AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast<uint16_t>(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE),
|
||||
|
|
@ -155,49 +157,156 @@ AC_Fence::AC_Fence()
|
|||
#endif
|
||||
_singleton = this;
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
if (_enabled) {
|
||||
_enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN;
|
||||
}
|
||||
}
|
||||
|
||||
// get a user-friendly list of fences
|
||||
void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg)
|
||||
{
|
||||
if (!fences) {
|
||||
return;
|
||||
}
|
||||
static const char* FENCE_NAMES[] = {
|
||||
"Max Alt",
|
||||
"Circle",
|
||||
"Polygon",
|
||||
"Min Alt",
|
||||
};
|
||||
uint8_t i = 0;
|
||||
uint8_t nfences = 0;
|
||||
while (fences !=0) {
|
||||
if (fences & 0x1) {
|
||||
if (nfences > 0) {
|
||||
if (!(fences & ~1U)) {
|
||||
msg.printf(" and ");
|
||||
} else {
|
||||
msg.printf(", ");
|
||||
}
|
||||
}
|
||||
msg.printf("%s", FENCE_NAMES[i]);
|
||||
nfences++;
|
||||
}
|
||||
fences >>= 1;
|
||||
i++;
|
||||
}
|
||||
msg.printf(" fence");
|
||||
if (nfences>1) {
|
||||
msg.printf("s");
|
||||
}
|
||||
}
|
||||
|
||||
// print a message about the passed in fences
|
||||
void AC_Fence::print_fence_message(const char* message, uint8_t fences) const
|
||||
{
|
||||
if (!fences) {
|
||||
return;
|
||||
}
|
||||
|
||||
char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
|
||||
ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
|
||||
AC_Fence::get_fence_names(fences, e);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s %s", e.get_writeable_string(), message);
|
||||
}
|
||||
|
||||
// should be called @10Hz to handle loading from eeprom
|
||||
void AC_Fence::update()
|
||||
{
|
||||
_poly_loader.update();
|
||||
// if someone changes the parameter we want to enable or disable everything
|
||||
if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) {
|
||||
// reset the auto mask since we just reconfigured all of fencing
|
||||
_auto_enable_mask = AC_FENCE_ALL_FENCES;
|
||||
_last_enabled = _enabled;
|
||||
_last_auto_enabled = _auto_enabled;
|
||||
if (_enabled) {
|
||||
_enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN;
|
||||
} else {
|
||||
_enabled_fences = 0;
|
||||
}
|
||||
}
|
||||
#ifdef AC_FENCE_DEBUG
|
||||
static uint32_t last_msg_count = 0;
|
||||
if (get_enabled_fences() && last_msg_count++ % 10 == 0) {
|
||||
print_fence_message("fences active", get_enabled_fences());
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/// enable the Fence code generally; a master switch for all fences
|
||||
void AC_Fence::enable(bool value)
|
||||
uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
|
||||
{
|
||||
uint8_t fences = _configured_fences.get() & fence_types;
|
||||
uint8_t enabled_fences = _enabled_fences;
|
||||
if (value) {
|
||||
enabled_fences |= fences;
|
||||
} else {
|
||||
enabled_fences &= ~fences;
|
||||
}
|
||||
|
||||
// fences that were manually changed are no longer eligible for auto-enablement or disablement
|
||||
if (update_auto_mask) {
|
||||
// if we are explicitly enabling or disabling the alt min fence and it was auto-enabled then make sure
|
||||
// it doesn't get re-enabled or disabled
|
||||
if (fence_types & _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) {
|
||||
_floor_disabled_for_landing = !value;
|
||||
}
|
||||
_auto_enable_mask &= ~fences;
|
||||
}
|
||||
|
||||
uint8_t fences_to_change = _enabled_fences ^ enabled_fences;
|
||||
|
||||
if (!fences_to_change) {
|
||||
return 0;
|
||||
}
|
||||
#if HAL_LOGGING_ENABLED
|
||||
if (_enabled && !value) {
|
||||
AP::logger().Write_Event(LogEvent::FENCE_DISABLE);
|
||||
} else if (!_enabled && value) {
|
||||
AP::logger().Write_Event(LogEvent::FENCE_ENABLE);
|
||||
AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE);
|
||||
if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) {
|
||||
AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MAX_ENABLE : LogEvent::FENCE_ALT_MAX_DISABLE);
|
||||
}
|
||||
if (fences_to_change & AC_FENCE_TYPE_CIRCLE) {
|
||||
AP::logger().Write_Event(value ? LogEvent::FENCE_CIRCLE_ENABLE : LogEvent::FENCE_CIRCLE_DISABLE);
|
||||
}
|
||||
if (fences_to_change & AC_FENCE_TYPE_ALT_MIN) {
|
||||
AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MIN_ENABLE : LogEvent::FENCE_ALT_MIN_DISABLE);
|
||||
}
|
||||
if (fences_to_change & AC_FENCE_TYPE_POLYGON) {
|
||||
AP::logger().Write_Event(value ? LogEvent::FENCE_POLYGON_ENABLE : LogEvent::FENCE_POLYGON_DISABLE);
|
||||
}
|
||||
#endif
|
||||
_enabled.set(value);
|
||||
|
||||
_enabled_fences = enabled_fences;
|
||||
|
||||
if (!value) {
|
||||
clear_breach(AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON);
|
||||
disable_floor();
|
||||
} else {
|
||||
enable_floor();
|
||||
clear_breach(fences_to_change);
|
||||
}
|
||||
|
||||
return fences_to_change;
|
||||
}
|
||||
|
||||
/// enable/disable fence floor only
|
||||
void AC_Fence::enable_floor()
|
||||
{
|
||||
#if HAL_LOGGING_ENABLED
|
||||
if (!_floor_enabled) {
|
||||
// Floor is currently disabled, enable it
|
||||
AP::logger().Write_Event(LogEvent::FENCE_FLOOR_ENABLE);
|
||||
}
|
||||
#endif
|
||||
_floor_enabled = true;
|
||||
enable(true, AC_FENCE_TYPE_ALT_MIN);
|
||||
}
|
||||
|
||||
void AC_Fence::disable_floor()
|
||||
{
|
||||
#if HAL_LOGGING_ENABLED
|
||||
if (_floor_enabled) {
|
||||
// Floor is currently enabled, disable it
|
||||
AP::logger().Write_Event(LogEvent::FENCE_FLOOR_DISABLE);
|
||||
enable(false, AC_FENCE_TYPE_ALT_MIN);
|
||||
}
|
||||
|
||||
/*
|
||||
called on arming
|
||||
*/
|
||||
void AC_Fence::auto_enable_fence_on_arming(void)
|
||||
{
|
||||
if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
_floor_enabled = false;
|
||||
clear_breach(AC_FENCE_TYPE_ALT_MIN);
|
||||
|
||||
const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false);
|
||||
print_fence_message("auto-enabled", fences);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -205,15 +314,18 @@ void AC_Fence::disable_floor()
|
|||
*/
|
||||
void AC_Fence::auto_enable_fence_after_takeoff(void)
|
||||
{
|
||||
if (_enabled) {
|
||||
return;
|
||||
}
|
||||
switch(auto_enabled()) {
|
||||
case AC_Fence::AutoEnable::ALWAYS_ENABLED:
|
||||
case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
|
||||
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
|
||||
enable(true);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence enabled (auto enabled)");
|
||||
case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: {
|
||||
// auto-enable fences that aren't currently auto-enabled
|
||||
if (_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) {
|
||||
_floor_disabled_for_landing = false;
|
||||
}
|
||||
const uint8_t fences = enable(true, _auto_enable_mask, false);
|
||||
print_fence_message("auto-enabled", fences);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
// fence does not auto-enable in other takeoff conditions
|
||||
break;
|
||||
|
|
@ -226,13 +338,20 @@ void AC_Fence::auto_enable_fence_after_takeoff(void)
|
|||
void AC_Fence::auto_disable_fence_for_landing(void)
|
||||
{
|
||||
switch (auto_enabled()) {
|
||||
case AC_Fence::AutoEnable::ALWAYS_ENABLED:
|
||||
enable(false);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)");
|
||||
case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: {
|
||||
// disable only those fences which are allowed to be disabled
|
||||
if (_auto_enable_mask & _enabled_fences & AC_FENCE_TYPE_ALT_MIN) {
|
||||
_floor_disabled_for_landing = true;
|
||||
}
|
||||
const uint8_t fences = enable(false, _auto_enable_mask & _enabled_fences, false);
|
||||
print_fence_message("auto-disabled", fences);
|
||||
break;
|
||||
}
|
||||
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
|
||||
case AC_Fence::AutoEnable::ONLY_WHEN_ARMED:
|
||||
disable_floor();
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
|
||||
_floor_disabled_for_landing = true;
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled");
|
||||
break;
|
||||
default:
|
||||
// fence does not auto-disable in other landing conditions
|
||||
|
|
@ -240,36 +359,26 @@ void AC_Fence::auto_disable_fence_for_landing(void)
|
|||
}
|
||||
}
|
||||
|
||||
bool AC_Fence::present() const
|
||||
uint8_t AC_Fence::present() const
|
||||
{
|
||||
const auto enabled_fences = _enabled_fences.get();
|
||||
// A fence is present if any of the conditions are true.
|
||||
// * tin can (circle) is enabled
|
||||
// * min or max alt is enabled
|
||||
// * polygon fences are enabled and any fence has been uploaded
|
||||
if (enabled_fences & AC_FENCE_TYPE_CIRCLE ||
|
||||
enabled_fences & AC_FENCE_TYPE_ALT_MIN ||
|
||||
enabled_fences & AC_FENCE_TYPE_ALT_MAX ||
|
||||
((enabled_fences & AC_FENCE_TYPE_POLYGON) && _poly_loader.total_fence_count() > 0)) {
|
||||
return true;
|
||||
uint8_t mask = AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX;
|
||||
if (_poly_loader.total_fence_count() > 0) {
|
||||
mask |= AC_FENCE_TYPE_POLYGON;
|
||||
}
|
||||
|
||||
return false;
|
||||
return _configured_fences.get() & mask;
|
||||
}
|
||||
|
||||
/// get_enabled_fences - returns bitmask of enabled fences
|
||||
uint8_t AC_Fence::get_enabled_fences() const
|
||||
{
|
||||
if (!_enabled && !_auto_enabled) {
|
||||
return 0;
|
||||
}
|
||||
return _enabled_fences;
|
||||
return _enabled_fences & present();
|
||||
}
|
||||
|
||||
// additional checks for the polygon fence:
|
||||
bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
|
||||
{
|
||||
if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) {
|
||||
if (!(_configured_fences & AC_FENCE_TYPE_POLYGON)) {
|
||||
// not enabled; all good
|
||||
return true;
|
||||
}
|
||||
|
|
@ -324,20 +433,20 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
|
|||
fail_msg = nullptr;
|
||||
|
||||
// if fences are enabled but none selected fail pre-arm check
|
||||
if (enabled() && !present()) {
|
||||
if (_enabled && !present()) {
|
||||
fail_msg = "Fences enabled, but none selected";
|
||||
return false;
|
||||
}
|
||||
|
||||
// if not enabled or not fence set-up always return true
|
||||
if ((!_enabled && !_auto_enabled) || !_enabled_fences) {
|
||||
if ((!enabled() && !_auto_enabled) || !_configured_fences) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// if we have horizontal limits enabled, check we can get a
|
||||
// relative position from the AHRS
|
||||
if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) ||
|
||||
(_enabled_fences & AC_FENCE_TYPE_POLYGON)) {
|
||||
if ((_configured_fences & AC_FENCE_TYPE_CIRCLE) ||
|
||||
(_configured_fences & AC_FENCE_TYPE_POLYGON)) {
|
||||
Vector2f position;
|
||||
if (!AP::ahrs().get_relative_position_NE_home(position)) {
|
||||
fail_msg = "Fence requires position";
|
||||
|
|
@ -389,13 +498,14 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
|
|||
bool AC_Fence::check_fence_alt_max()
|
||||
{
|
||||
// altitude fence check
|
||||
if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MAX)) {
|
||||
if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) {
|
||||
// not enabled; no breach
|
||||
return false;
|
||||
}
|
||||
|
||||
AP::ahrs().get_relative_position_D_home(_curr_alt);
|
||||
_curr_alt = -_curr_alt; // translate Down to Up
|
||||
float alt;
|
||||
AP::ahrs().get_relative_position_D_home(alt);
|
||||
_curr_alt = -alt; // translate Down to Up
|
||||
|
||||
// check if we are over the altitude fence
|
||||
if (_curr_alt >= _alt_max) {
|
||||
|
|
@ -437,13 +547,14 @@ bool AC_Fence::check_fence_alt_max()
|
|||
bool AC_Fence::check_fence_alt_min()
|
||||
{
|
||||
// altitude fence check
|
||||
if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MIN)) {
|
||||
if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)) {
|
||||
// not enabled; no breach
|
||||
return false;
|
||||
}
|
||||
|
||||
AP::ahrs().get_relative_position_D_home(_curr_alt);
|
||||
_curr_alt = -_curr_alt; // translate Down to Up
|
||||
float alt;
|
||||
AP::ahrs().get_relative_position_D_home(alt);
|
||||
_curr_alt = -alt; // translate Down to Up
|
||||
|
||||
// check if we are under the altitude fence
|
||||
if (_curr_alt <= _alt_min) {
|
||||
|
|
@ -479,13 +590,37 @@ bool AC_Fence::check_fence_alt_min()
|
|||
return false;
|
||||
}
|
||||
|
||||
|
||||
/// auto enable fence floor
|
||||
bool AC_Fence::auto_enable_fence_floor()
|
||||
{
|
||||
// altitude fence check
|
||||
if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || !_enabled) {
|
||||
// not enabled
|
||||
return false;
|
||||
}
|
||||
|
||||
float alt;
|
||||
AP::ahrs().get_relative_position_D_home(alt);
|
||||
_curr_alt = -alt; // translate Down to Up
|
||||
|
||||
// check if we are over the altitude fence
|
||||
if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) {
|
||||
enable(true, AC_FENCE_TYPE_ALT_MIN);
|
||||
gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)");
|
||||
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
|
||||
bool AC_Fence::check_fence_polygon()
|
||||
{
|
||||
const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON;
|
||||
const bool breached = ((_enabled_fences & AC_FENCE_TYPE_POLYGON) &&
|
||||
const bool breached = ((_configured_fences & AC_FENCE_TYPE_POLYGON) &&
|
||||
_poly_loader.breached());
|
||||
if (breached) {
|
||||
if (!was_breached) {
|
||||
|
|
@ -506,7 +641,7 @@ bool AC_Fence::check_fence_polygon()
|
|||
/// fence is breached.
|
||||
bool AC_Fence::check_fence_circle()
|
||||
{
|
||||
if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) {
|
||||
if (!(get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) {
|
||||
// not enabled; no breach
|
||||
return false;
|
||||
}
|
||||
|
|
@ -554,34 +689,29 @@ uint8_t AC_Fence::check()
|
|||
uint8_t ret = 0;
|
||||
|
||||
// clear any breach from a non-enabled fence
|
||||
clear_breach(~_enabled_fences);
|
||||
clear_breach(~_configured_fences);
|
||||
|
||||
// return immediately if disabled
|
||||
if ((!_enabled && !_auto_enabled) || !_enabled_fences) {
|
||||
if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) {
|
||||
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;
|
||||
}
|
||||
|
||||
// minimum altitude fence check
|
||||
if (_floor_enabled && check_fence_alt_min()) {
|
||||
// minimum altitude fence check, do this before auto-disabling (e.g. because falling)
|
||||
// so that any action can be taken
|
||||
if (floor_enabled() && check_fence_alt_min()) {
|
||||
ret |= AC_FENCE_TYPE_ALT_MIN;
|
||||
}
|
||||
|
||||
// auto enable floor unless auto enable has been set (which means other behaviour is required)
|
||||
if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && (_configured_fences & AC_FENCE_TYPE_ALT_MIN)) {
|
||||
auto_enable_fence_floor();
|
||||
}
|
||||
|
||||
// circle fence check
|
||||
if (check_fence_circle()) {
|
||||
ret |= AC_FENCE_TYPE_CIRCLE;
|
||||
|
|
@ -592,6 +722,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;
|
||||
}
|
||||
|
|
@ -695,6 +837,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)
|
||||
|
|
@ -712,7 +856,7 @@ bool AC_Fence::sys_status_enabled() const
|
|||
return false;
|
||||
}
|
||||
// Fence is only enabled when the flag is enabled
|
||||
return _enabled;
|
||||
return enabled();
|
||||
}
|
||||
|
||||
bool AC_Fence::sys_status_failed() const
|
||||
|
|
@ -742,14 +886,17 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { AP_GROUPEND };
|
|||
|
||||
AC_Fence::AC_Fence() {};
|
||||
|
||||
void AC_Fence::enable(bool value) {};
|
||||
uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable) { return 0; }
|
||||
|
||||
void AC_Fence::disable_floor() {};
|
||||
void AC_Fence::enable_floor() {}
|
||||
void AC_Fence::disable_floor() {}
|
||||
void AC_Fence::update() {}
|
||||
|
||||
void AC_Fence::auto_enable_fence_after_takeoff() {};
|
||||
void AC_Fence::auto_disable_fence_for_landing() {};
|
||||
void AC_Fence::auto_enable_fence_after_takeoff() {}
|
||||
void AC_Fence::auto_disable_fence_for_landing() {}
|
||||
void AC_Fence::auto_enable_fence_on_arming() {}
|
||||
|
||||
bool AC_Fence::present() const { return false; }
|
||||
uint8_t AC_Fence::present() const { return 0; }
|
||||
|
||||
uint8_t AC_Fence::get_enabled_fences() const { return 0; }
|
||||
|
||||
|
|
@ -758,6 +905,8 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; }
|
|||
uint8_t AC_Fence::check() { 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; }
|
||||
void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { }
|
||||
void AC_Fence::print_fence_message(const char* msg, uint8_t fences) const {}
|
||||
|
||||
void AC_Fence::manual_recovery_start() {}
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
|
||||
#include <inttypes.h>
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Common/ExpandingString.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AC_Fence/AC_PolyFence_loader.h>
|
||||
|
|
@ -15,6 +16,8 @@
|
|||
#define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL)
|
||||
#define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence
|
||||
#define AC_FENCE_TYPE_ALT_MIN 8 // low alt fence which usually initiates an RTL
|
||||
#define AC_FENCE_ARMING_FENCES (AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)
|
||||
#define AC_FENCE_ALL_FENCES (AC_FENCE_ARMING_FENCES | AC_FENCE_TYPE_ALT_MIN)
|
||||
|
||||
// valid actions should a fence be breached
|
||||
#define AC_FENCE_ACTION_REPORT_ONLY 0 // report to GCS that boundary has been breached but take no further action
|
||||
|
|
@ -34,12 +37,19 @@ class AC_Fence
|
|||
public:
|
||||
friend class AC_PolyFence_loader;
|
||||
|
||||
enum class AutoEnable
|
||||
enum class AutoEnable : uint8_t
|
||||
{
|
||||
ALWAYS_DISABLED = 0,
|
||||
ALWAYS_ENABLED = 1,
|
||||
ENABLE_DISABLE_FLOOR_ONLY = 2,
|
||||
ONLY_WHEN_ARMED = 3
|
||||
ENABLE_ON_AUTO_TAKEOFF = 1, // enable on auto takeoff
|
||||
ENABLE_DISABLE_FLOOR_ONLY = 2, // enable on takeoff but disable floor on landing
|
||||
ONLY_WHEN_ARMED = 3 // enable on arming
|
||||
};
|
||||
|
||||
enum class MavlinkFenceActions : uint16_t
|
||||
{
|
||||
DISABLE_FENCE = 0,
|
||||
ENABLE_FENCE = 1,
|
||||
DISABLE_ALT_MIN_FENCE = 2
|
||||
};
|
||||
|
||||
AC_Fence();
|
||||
|
|
@ -55,7 +65,12 @@ public:
|
|||
static AC_Fence *get_singleton() { return _singleton; }
|
||||
|
||||
/// enable - allows fence to be enabled/disabled.
|
||||
void enable(bool value);
|
||||
/// returns a bitmask of fences that were changed
|
||||
uint8_t enable(bool value, uint8_t fence_types, bool update_auto_mask = true);
|
||||
|
||||
/// enable_configured - allows configured fences to be enabled/disabled.
|
||||
/// returns a bitmask of fences that were changed
|
||||
uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); }
|
||||
|
||||
/// auto_enabled - automaticaly enable/disable fence depending of flight status
|
||||
AutoEnable auto_enabled() { return static_cast<AutoEnable>(_auto_enabled.get()); }
|
||||
|
|
@ -72,19 +87,23 @@ public:
|
|||
/// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing.
|
||||
void auto_disable_fence_for_landing();
|
||||
|
||||
/// enabled - returns true if fence is enabled
|
||||
bool enabled() const { return _enabled; }
|
||||
/// auto_enable_fences_on_arming - auto enables all applicable fences on arming
|
||||
void auto_enable_fence_on_arming();
|
||||
|
||||
/// present - returns true if fence is present
|
||||
bool present() const;
|
||||
/// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached.
|
||||
bool auto_enable_fence_floor();
|
||||
|
||||
/// enabled - returns whether fencing is enabled or not
|
||||
bool enabled() const { return _enabled_fences; }
|
||||
|
||||
/// present - returns true if any of the provided types is present
|
||||
uint8_t present() const;
|
||||
|
||||
/// get_enabled_fences - returns bitmask of enabled fences
|
||||
uint8_t get_enabled_fences() const;
|
||||
|
||||
// should be called @10Hz to handle loading from eeprom
|
||||
void update() {
|
||||
_poly_loader.update();
|
||||
}
|
||||
void update();
|
||||
|
||||
/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
|
||||
bool pre_arm_check(const char* &fail_msg) const;
|
||||
|
|
@ -133,6 +152,12 @@ public:
|
|||
/// get_return_rally - returns whether returning to fence return point or rally point
|
||||
float get_return_altitude() const { return _ret_altitude; }
|
||||
|
||||
/// get a user-friendly list of fences
|
||||
static void get_fence_names(uint8_t fences, ExpandingString& msg);
|
||||
|
||||
// print a message about the fences to the GCS
|
||||
void print_fence_message(const char* msg, uint8_t fences) const;
|
||||
|
||||
/// 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
|
||||
|
|
@ -190,11 +215,16 @@ private:
|
|||
bool pre_arm_check_polygon(const char* &fail_msg) const;
|
||||
bool pre_arm_check_circle(const char* &fail_msg) const;
|
||||
bool pre_arm_check_alt(const char* &fail_msg) const;
|
||||
// fence floor is enabled
|
||||
bool floor_enabled() const { return _enabled_fences & AC_FENCE_TYPE_ALT_MIN; }
|
||||
|
||||
// parameters
|
||||
AP_Int8 _enabled; // fence enable/disable control
|
||||
uint8_t _enabled_fences; // fences that are currently enabled/disabled
|
||||
bool _last_enabled; // value of enabled last time we checked
|
||||
AP_Int8 _enabled; // overall feature control
|
||||
AP_Int8 _auto_enabled; // top level flag for auto enabling fence
|
||||
AP_Int8 _enabled_fences; // bit mask holding which fences are enabled
|
||||
uint8_t _last_auto_enabled; // value of auto_enabled last time we checked
|
||||
AP_Int8 _configured_fences; // bit mask holding which fences are enabled
|
||||
AP_Int8 _action; // recovery action specified by user
|
||||
AP_Float _alt_max; // altitude upper limit in meters
|
||||
AP_Float _alt_min; // altitude lower limit in meters
|
||||
|
|
@ -216,7 +246,8 @@ private:
|
|||
float _circle_breach_distance; // distance beyond the circular fence
|
||||
|
||||
// other internal variables
|
||||
bool _floor_enabled; // fence floor is enabled
|
||||
bool _floor_disabled_for_landing; // fence floor is disabled for landing
|
||||
uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled
|
||||
float _home_distance; // distance from home in meters (provided by main code)
|
||||
float _curr_alt;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue