Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: remember manual disable of fence for min-alt 

the automatic min-alt fence should not auto-enable based on altitude
if the fence has been manually disabled. This is needed to allow for a
manual landing by disabling the fence before descending
This commit is contained in:
Andrew Tridgell 2024-12-10 08:41:05 +11:00
parent 75655a787c
commit 076782f6f1
2 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_Fence/AC_Fence.cpp
View File

@ -237,9 +237,9 @@ void AC_Fence::update()
}
// enable or disable configured fences present in fence_types
// also updates the bitmask of auto enabled fences if update_auto_mask is true
// also updates the _min_alt_state enum if update_auto_enable is true
// returns a bitmask of fences that were changed
uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable)
{
uint8_t fences = _configured_fences.get() & fence_types;
uint8_t enabled_fences = _enabled_fences;
@ -249,6 +249,11 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
enabled_fences &= ~fences;
}
if (update_auto_enable && (fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
// remember that min-alt fence was manually enabled/disabled
_min_alt_state = value ? MinAltState::MANUALLY_ENABLED : MinAltState::MANUALLY_DISABLED;
}
uint8_t fences_to_change = _enabled_fences ^ enabled_fences;
if (!fences_to_change) {
@ -300,6 +305,10 @@ void AC_Fence::auto_enable_fence_on_arming(void)
return;
}
// reset min alt state, after an auto-enable the min alt fence can be auto-enabled on
// reaching altitude
_min_alt_state = MinAltState::DEFAULT;
const uint8_t fences = enable(true, AC_FENCE_ARMING_FENCES, false);
print_fence_message("auto-enabled", fences);
}
@ -327,6 +336,9 @@ void AC_Fence::auto_enable_fence_after_takeoff(void)
return;
}
// reset min-alt state
_min_alt_state = MinAltState::DEFAULT;
const uint8_t fences = enable(true, AC_FENCE_ALL_FENCES, false);
print_fence_message("auto-enabled", fences);
}
@ -345,6 +357,10 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const
auto_disable = AC_FENCE_TYPE_ALT_MIN;
break;
}
if (_min_alt_state == MinAltState::MANUALLY_ENABLED) {
// don't auto-disable min alt fence if manually enabled
auto_disable &= ~AC_FENCE_TYPE_ALT_MIN;
}
return auto_disable;
}
@ -610,6 +626,7 @@ bool AC_Fence::auto_enable_fence_floor()
// altitude fence check
if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured
|| (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled
|| _min_alt_state == MinAltState::MANUALLY_DISABLED // user has manually disabled the fence
|| (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED
|| auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) {
// not enabled
@ -715,6 +732,11 @@ uint8_t AC_Fence::check(bool disable_auto_fences)
// clear any breach from disabled fences
clear_breach(fences_to_disable);
if (_min_alt_state == MinAltState::MANUALLY_ENABLED) {
// if user has manually enabled the min-alt fence then don't auto-disable
fences_to_disable &= ~AC_FENCE_TYPE_ALT_MIN;
}
// report on any fences that were auto-disabled
if (fences_to_disable) {
print_fence_message("auto-disabled", fences_to_disable);

7
libraries/AC_Fence/AC_Fence.h
View File

@ -259,6 +259,13 @@ private:
uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control
enum class MinAltState
{
DEFAULT = 0,
MANUALLY_ENABLED,
MANUALLY_DISABLED
} _min_alt_state;
AC_PolyFence_loader _poly_loader{_total, _options}; // polygon fence
};