From e1534f519e7c37fcb8dce17020ae43fec8850b32 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 make auto-disabling for minimum altitude fence an option output message when fence floor auto-enabled --- libraries/AC_Fence/AC_Fence.cpp | 75 +++++++++++++++++++++++++-------- libraries/AC_Fence/AC_Fence.h | 8 +++- 2 files changed, 63 insertions(+), 20 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index b537085e42113f..f177822725b9d4 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -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 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 + // @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 | 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(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE), @@ -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 over 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 @@ -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; @@ -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; @@ -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; } @@ -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) @@ -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; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 1e8bd1d37c5b6f..da192d4c550b7f 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); @@ -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;