From 35db0a81ebcd0b9110541a05379db848846bb331 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 4 Dec 2023 16:58:40 +0000 Subject: [PATCH] AC_Fence: add ability to auto-enable fence floor while doing fence checks 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 --- libraries/AC_Fence/AC_Fence.cpp | 70 +++++++++++++++++++++++++-------- libraries/AC_Fence/AC_Fence.h | 7 +++- 2 files changed, 59 insertions(+), 18 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index aaec44932bd4ed..0e9c02512cba90 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -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(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE), + AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER), // @Param{Plane}: OPTIONS // @DisplayName: Fence options @@ -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 @@ -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; @@ -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; @@ -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; } @@ -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) @@ -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; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 1e8bd1d37c5b6f..b8fbecca239997 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -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, @@ -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; } @@ -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);