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
  • Loading branch information
andyp1per committed Dec 21, 2023
1 parent 35a7043 commit 35db0a8
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 18 deletions.
70 changes: 54 additions & 16 deletions libraries/AC_Fence/AC_Fence.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -126,14 +126,15 @@ 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),

// @Param{Plane}: OPTIONS
// @DisplayName: Fence options
Expand Down Expand Up @@ -470,6 +471,33 @@ 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();

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 +568,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 +580,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) {
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 +607,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 +722,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 +784,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
7 changes: 5 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

0 comments on commit 35db0a8

Please sign in to comment.