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

AC_Fence: disable fences for landing by suppressing in the fence check rather than using a state machine 

simplify takeoff auto-enablement
This commit is contained in:
Andy Piper 2024-05-31 14:12:38 +01:00 committed by Peter Barker
parent c216536a94
commit 04dd7de1ed
2 changed files with 43 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
libraries/AC_Fence/AC_Fence.cpp
View File

@ -248,11 +248,6 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
// fences that were manually changed are no longer eligible for auto-enablement or disablement
if (update_auto_mask) {
// if we are explicitly enabling or disabling the alt min fence and it was auto-enabled then make sure
// it doesn't get re-enabled or disabled
if (fence_types & _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) {
_floor_disabled_for_landing = !value;
}
_auto_enable_mask &= ~fences;
}
@ -328,49 +323,30 @@ void AC_Fence::auto_disable_fence_on_disarming(void)
*/
void AC_Fence::auto_enable_fence_after_takeoff(void)
{
switch(auto_enabled()) {
case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: {
// auto-enable fences that aren't currently auto-enabled
if (_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) {
_floor_disabled_for_landing = false;
if (auto_enabled() != AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF) {
return;
}
const uint8_t fences = enable(true, _auto_enable_mask, false);
print_fence_message("auto-enabled", fences);
break;
}
default:
// fence does not auto-enable in other takeoff conditions
break;
}
}
/*
called when performing an auto landing
*/
void AC_Fence::auto_disable_fence_for_landing(void)
// return fences that should be auto-disabled when requested
uint8_t AC_Fence::get_auto_disable_fences(void) const
{
uint8_t auto_disable = 0;
switch (auto_enabled()) {
case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: {
// disable only those fences which are allowed to be disabled
if (_auto_enable_mask & _enabled_fences & AC_FENCE_TYPE_ALT_MIN) {
_floor_disabled_for_landing = true;
}
const uint8_t fences = enable(false, _auto_enable_mask & _enabled_fences, false);
print_fence_message("auto-disabled", fences);
case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
auto_disable = _auto_enable_mask;
break;
}
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
case AC_Fence::AutoEnable::ONLY_WHEN_ARMED:
enable(false, AC_FENCE_TYPE_ALT_MIN, false);
_floor_disabled_for_landing = true;
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled");
auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN;
break;
default:
// fence does not auto-disable in other landing conditions
break;
}
return auto_disable;
}
uint8_t AC_Fence::present() const
@ -609,9 +585,11 @@ bool AC_Fence::check_fence_alt_min()
bool AC_Fence::auto_enable_fence_floor()
{
// altitude fence check
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))) {
if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured
|| (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled
|| !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) // has been manually disabled
|| (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED
|| auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) {
// not enabled
return false;
}
@ -621,7 +599,7 @@ bool AC_Fence::auto_enable_fence_floor()
_curr_alt = -alt; // translate Down to Up
// check if we are over the altitude fence
if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) {
if (!floor_enabled() && _curr_alt >= _alt_min + _margin) {
enable(true, AC_FENCE_TYPE_ALT_MIN, false);
gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)");
return true;
@ -630,30 +608,6 @@ bool AC_Fence::auto_enable_fence_floor()
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
// breached. That includes being inside exclusion zones and outside
// inclusions zones
@ -728,41 +682,53 @@ 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 disable_auto_fences)
{
uint8_t ret = 0;
uint8_t disabled_fences = disable_auto_fences ? get_auto_disable_fences() : 0;
uint8_t fences_to_disable = disabled_fences & _enabled_fences;
// clear any breach from a non-enabled fence
clear_breach(~_configured_fences);
// clear any breach from disabled fences
clear_breach(fences_to_disable);
// report on any fences that were auto-disabled
if (fences_to_disable) {
print_fence_message("auto-disabled", fences_to_disable);
}
// return immediately if disabled
if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) {
return 0;
}
// disable the (temporarily) disabled fences
enable(false, disabled_fences, false);
// maximum altitude fence check
if (check_fence_alt_max()) {
if (!(disabled_fences & AC_FENCE_TYPE_ALT_MAX) && check_fence_alt_max()) {
ret |= AC_FENCE_TYPE_ALT_MAX;
}
// minimum altitude fence check, do this before auto-disabling (e.g. because falling)
// so that any action can be taken
if (floor_enabled() && check_fence_alt_min()) {
if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN) && check_fence_alt_min()) {
ret |= AC_FENCE_TYPE_ALT_MIN;
}
// auto enable floor unless auto enable has been set (which means other behaviour is required)
if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && (_configured_fences & AC_FENCE_TYPE_ALT_MIN)) {
// auto enable floor unless auto enable on auto takeoff has been set (which means other behaviour is required)
if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN)) {
auto_enable_fence_floor();
}
// circle fence check
if (check_fence_circle()) {
if (!(disabled_fences & AC_FENCE_TYPE_CIRCLE) && check_fence_circle()) {
ret |= AC_FENCE_TYPE_CIRCLE;
}
// polygon fence check
if (check_fence_polygon()) {
if (!(disabled_fences & AC_FENCE_TYPE_POLYGON) && check_fence_polygon()) {
ret |= AC_FENCE_TYPE_POLYGON;
}
@ -937,7 +903,6 @@ void AC_Fence::disable_floor() {}
void AC_Fence::update() {}
void AC_Fence::auto_enable_fence_after_takeoff() {}
void AC_Fence::auto_disable_fence_for_landing() {}
void AC_Fence::auto_enable_fence_on_arming() {}
void AC_Fence::auto_disable_fence_on_disarming() {}
@ -947,7 +912,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 disable_auto_fences) { 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; }
void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { }

14
libraries/AC_Fence/AC_Fence.h
View File

@ -73,7 +73,7 @@ public:
uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); }
/// auto_enabled - automaticaly enable/disable fence depending of flight status
AutoEnable auto_enabled() { return static_cast<AutoEnable>(_auto_enabled.get()); }
AutoEnable auto_enabled() const { return static_cast<AutoEnable>(_auto_enabled.get()); }
/// enable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value
void enable_floor();
@ -84,21 +84,17 @@ public:
/// auto_enable_fence_on_takeoff - auto enables the fence. Called after takeoff conditions met
void auto_enable_fence_after_takeoff();
/// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing.
void auto_disable_fence_for_landing();
/// auto_enable_fences_on_arming - auto enables all applicable fences on arming
void auto_enable_fence_on_arming();
/// auto_disable_fences_on_disarming - auto disables all applicable fences on disarming
void auto_disable_fence_on_disarming();
uint8_t get_auto_disable_fences(void) const;
/// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached.
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
bool enabled() const { return _enabled_fences; }
@ -119,7 +115,8 @@ public:
///
/// check - returns the fence type that has been breached (if any)
uint8_t check();
/// disabled_fences can be used to disable fences for certain conditions (e.g. landing)
uint8_t check(bool disable_auto_fence = 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);
@ -252,7 +249,6 @@ private:
float _circle_breach_distance; // distance beyond the circular fence
// other internal variables
bool _floor_disabled_for_landing; // fence floor is disabled for landing
uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled
float _home_distance; // distance from home in meters (provided by main code)
float _curr_alt;