Skip to content

Commit

Permalink
AC_Fence: add ability to auto-enable fence floor while doing fence ch…
Browse files Browse the repository at this point in the history
…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
make auto-disabling for minimum altitude fence an option
output message when fence floor auto-enabled
  • Loading branch information
andyp1per committed Jan 1, 2024
1 parent 9200240 commit 1a0c9ad
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 20 deletions.
75 changes: 57 additions & 18 deletions libraries/AC_Fence/AC_Fence.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -126,19 +126,20 @@ 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 | 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.
// @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas
// @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. When bit 2 is set any active minimum altitude floor will be disabled.
// @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas, 2:Auto-disable floor fence on disarming
// @User: Standard
AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast<uint16_t>(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE),

Expand Down Expand Up @@ -470,6 +471,34 @@ 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();
gcs().send_text(MAV_SEVERITY_INFO, "Fence floor enabled (auto enabled)");

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
Expand Down Expand Up @@ -540,7 +569,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;

Expand All @@ -552,22 +581,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 && option_enabled(AC_Fence::OPTIONS::AUTO_DISABLE_FLOOR_ON_DISARM)) {
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;
Expand All @@ -583,6 +608,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;
}
Expand Down Expand Up @@ -686,6 +723,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)
Expand Down Expand Up @@ -746,7 +785,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; }

Expand Down
8 changes: 6 additions & 2 deletions libraries/AC_Fence/AC_Fence.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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; }

Expand All @@ -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);
Expand Down Expand Up @@ -149,6 +152,7 @@ class AC_Fence
enum class OPTIONS {
DISABLE_MODE_CHANGE = 1U << 0,
INCLUSION_UNION = 1U << 1,
AUTO_DISABLE_FLOOR_ON_DISARM = 1U << 2,
};
static bool option_enabled(OPTIONS opt, const AP_Int16 &options) {
return (options.get() & int16_t(opt)) != 0;
Expand Down

0 comments on commit 1a0c9ad

Please sign in to comment.