Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: add reset_fence_floor_enable() and use it in plane when landing is aborted

Browse Source
This commit is contained in:
Andy Piper 2024-05-31 08:20:13 +01:00 committed by Peter Barker
parent cedccdb8fe
commit d5c6f3fe06
2 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_Fence/AC_Fence.cpp
View File

@ -609,7 +609,9 @@ bool AC_Fence::check_fence_alt_min()
bool AC_Fence::auto_enable_fence_floor() bool AC_Fence::auto_enable_fence_floor()
{ {
// altitude fence check // altitude fence check
if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || !_enabled) { if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN)
|| (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)
|| (!_enabled && (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED))) {
// not enabled // not enabled
return false; return false;
} }
@ -620,7 +622,7 @@ bool AC_Fence::auto_enable_fence_floor()
// check if we are over the altitude fence // check if we are over the altitude fence
if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) { if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) {
enable(true, AC_FENCE_TYPE_ALT_MIN); enable(true, AC_FENCE_TYPE_ALT_MIN, false);
gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)"); gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)");
return true; return true;
} }
@ -628,6 +630,30 @@ bool AC_Fence::auto_enable_fence_floor()
return false; return false;
} }
/// reset fence floor auto-enablement
bool AC_Fence::reset_fence_floor_enable()
{
// altitude fence check
if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || (!_enabled && !_auto_enabled)) {
// not enabled
return false;
}
float alt;
AP::ahrs().get_relative_position_D_home(alt);
_curr_alt = -alt; // translate Down to Up
// check if we are under the altitude fence
if ((floor_enabled() || _floor_disabled_for_landing) && _curr_alt <= _alt_min - _margin) {
enable(false, AC_FENCE_TYPE_ALT_MIN, false);
_floor_disabled_for_landing = false;
gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence disabled (auto disable)");
return true;
}
return false;
}
// check_fence_polygon - returns true if the poly fence is freshly // check_fence_polygon - returns true if the poly fence is freshly
// breached. That includes being inside exclusion zones and outside // breached. That includes being inside exclusion zones and outside
// inclusions zones // inclusions zones

3
libraries/AC_Fence/AC_Fence.h
View File

@ -96,6 +96,9 @@ public:
/// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached.
bool auto_enable_fence_floor(); bool auto_enable_fence_floor();
/// reset_fence_floor_enable - auto disables the fence floor if below the desired altitude.
bool reset_fence_floor_enable();
/// enabled - returns whether fencing is enabled or not /// enabled - returns whether fencing is enabled or not
bool enabled() const { return _enabled_fences; } bool enabled() const { return _enabled_fences; }