diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index aaec44932bd4ed..dc5aec2097c439 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 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), @@ -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 @@ -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;