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

AC_Fence: always disable Min Alt fence on landing 

allow precise pre-arm messages
This commit is contained in:
Andy Piper 2024-06-22 12:02:21 +01:00 committed by Peter Barker
parent 685fb7e34f
commit f0456f29dd
2 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AC_Fence/AC_Fence.cpp
View File

@ -341,10 +341,9 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const
break;
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
case AC_Fence::AutoEnable::ONLY_WHEN_ARMED:
default: // when auto disable is not set we still need to disable the altmin fence on landing
auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN;
break;
default:
break;
}
return auto_disable;
}
@ -366,7 +365,7 @@ uint8_t AC_Fence::get_enabled_fences() const
}
// additional checks for the polygon fence:
bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
bool AC_Fence::pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const
{
if (!(_configured_fences & AC_FENCE_TYPE_POLYGON)) {
// not enabled; all good
@ -374,12 +373,12 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
}
if (! _poly_loader.loaded()) {
fail_msg = "Fences invalid";
hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence(s) invalid");
return false;
}
if (!_poly_loader.check_inclusion_circle_margin(_margin)) {
fail_msg = "Margin is less than inclusion circle radius";
hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence margin is less than inclusion circle radius");
return false;
}
@ -387,14 +386,14 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
}
// additional checks for the circle fence:
bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const
bool AC_Fence::pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const
{
if (_circle_radius < 0) {
fail_msg = "Invalid FENCE_RADIUS value";
hal.util->snprintf(failure_msg, failure_msg_len, "Invalid Circle FENCE_RADIUS value");
return false;
}
if (_circle_radius < _margin) {
fail_msg = "FENCE_MARGIN is less than FENCE_RADIUS";
hal.util->snprintf(failure_msg, failure_msg_len, "Circle FENCE_MARGIN is less than FENCE_RADIUS");
return false;
}
@ -402,15 +401,15 @@ bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const
}
// additional checks for the alt fence:
bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const
bool AC_Fence::pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const
{
if (_alt_max < 0.0f) {
fail_msg = "Invalid FENCE_ALT_MAX value";
hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MAX value");
return false;
}
if (_alt_min < -100.0f) {
fail_msg = "Invalid FENCE_ALT_MIN value";
hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MIN value");
return false;
}
return true;
@ -418,13 +417,11 @@ bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const
/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
bool AC_Fence::pre_arm_check(const char* &fail_msg) const
bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const
{
fail_msg = nullptr;
// if fences are enabled but none selected fail pre-arm check
if (_enabled && !present()) {
fail_msg = "Fences enabled, but none selected";
hal.util->snprintf(failure_msg, failure_msg_len, "Fences enabled, but none selected");
return false;
}
@ -439,42 +436,45 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
(_configured_fences & AC_FENCE_TYPE_POLYGON)) {
Vector2f position;
if (!AP::ahrs().get_relative_position_NE_home(position)) {
fail_msg = "Fence requires position";
hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position");
return false;
}
}
if (!pre_arm_check_polygon(fail_msg)) {
if (!pre_arm_check_polygon(failure_msg, failure_msg_len)) {
return false;
}
if (!pre_arm_check_circle(fail_msg)) {
if (!pre_arm_check_circle(failure_msg, failure_msg_len)) {
return false;
}
if (!pre_arm_check_alt(fail_msg)) {
if (!pre_arm_check_alt(failure_msg, failure_msg_len)) {
return false;
}
// check no limits are currently breached
if (_breached_fences) {
fail_msg = "vehicle outside fence";
char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
AC_Fence::get_fence_names(_breached_fences, e);
hal.util->snprintf(failure_msg, failure_msg_len, "vehicle outside %s", e.get_writeable_string());
return false;
}
// validate FENCE_MARGIN parameter range
if (_margin < 0.0f) {
fail_msg = "Invalid FENCE_MARGIN value";
hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_MARGIN value");
return false;
}
if (_alt_max < _alt_min) {
fail_msg = "FENCE_ALT_MAX < FENCE_ALT_MIN";
hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_ALT_MAX < FENCE_ALT_MIN");
return false;
}
if (_alt_max - _alt_min <= 2.0f * _margin) {
fail_msg = "FENCE_MARGIN too big";
hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_MARGIN too big");
return false;
}
@ -910,7 +910,7 @@ uint8_t AC_Fence::present() const { return 0; }
uint8_t AC_Fence::get_enabled_fences() const { return 0; }
bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; }
bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { return true; }
uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; }
bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; }

8
libraries/AC_Fence/AC_Fence.h
View File

@ -108,7 +108,7 @@ public:
void update();
/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
bool pre_arm_check(const char* &fail_msg) const;
bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const;
///
/// methods to check we are within the boundaries and recover
@ -215,9 +215,9 @@ private:
void clear_breach(uint8_t fence_type);
// additional checks for the different fence types:
bool pre_arm_check_polygon(const char* &fail_msg) const;
bool pre_arm_check_circle(const char* &fail_msg) const;
bool pre_arm_check_alt(const char* &fail_msg) const;
bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const;
bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const;
bool pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const;
// fence floor is enabled
bool floor_enabled() const { return _enabled_fences & AC_FENCE_TYPE_ALT_MIN; }