From 2225c87486851fbb9933ef62d536d73c08bbfec5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 12:02:21 +0100 Subject: [PATCH] AC_Fence: always disable Min Alt fence on landing allow precise pre-arm messages --- libraries/AC_Fence/AC_Fence.cpp | 48 ++++++++++++++++----------------- libraries/AC_Fence/AC_Fence.h | 8 +++--- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index e28ad0b6e23964..df03fd493b9ba4 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -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; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index aa5c4c254d3c2b..3165d7b73d4d33 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -108,7 +108,7 @@ class AC_Fence 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 @@ class AC_Fence 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; }