From f6726e66f8665999675629fc56b7ce24182a662d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 Nov 2024 13:53:22 +1100 Subject: [PATCH 01/10] AC_Fence: prevent a fetch R/C switch from disabling FENCE_AUTOENABLE=3 we should only add fence types to the no auto-enable mask if the enable actually changed that type of fence. This fixes the case where the user has both FENCE_AUTOENABLE=3 and RCn_OPTION=11. The disable triggered by the init of the aux function was preventing the fence from auto-enabling --- libraries/AC_Fence/AC_Fence.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index db214b2e494e17..ba057dc96b320e 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -250,16 +250,17 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) enabled_fences &= ~fences; } - // fences that were manually changed are no longer eligible for auto-enablement or disablement - if (update_auto_mask) { - _auto_enable_mask &= ~fences; - } - uint8_t fences_to_change = _enabled_fences ^ enabled_fences; if (!fences_to_change) { return 0; } + + // fences that were manually changed are no longer eligible for auto-enablement or disablement + if (update_auto_mask) { + _auto_enable_mask &= ~fences_to_change; + } + #if HAL_LOGGING_ENABLED AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE); if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) { From 9084b48308ab357bcb11e0594e85f0c259c79775 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 Nov 2024 13:55:07 +1100 Subject: [PATCH 02/10] AC_Fence: fixed pre-arm check for polygon fences for polygon fences we need to check if the vehicle has a position and is inside the polygon --- libraries/AC_Fence/AC_Fence.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index ba057dc96b320e..cdbf55e51fe69c 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -470,8 +470,20 @@ bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) c return false; } + auto breached_fences = _breached_fences; + if (auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { + Location loc; + if (!AP::ahrs().get_location(loc)) { + hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position"); + return false; + } + if (_poly_loader.breached(loc)) { + breached_fences |= AC_FENCE_TYPE_POLYGON; + } + } + // check no limits are currently breached - if (_breached_fences) { + if (breached_fences) { 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); From 4c40d66d7dd068ec74583cb2df95f2d7235eda39 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 19:19:48 +1100 Subject: [PATCH 03/10] autotest: fixed test suite for FENCE_AUTOENABLE=2 --- Tools/autotest/arduplane.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 8e49b0f2038cce..9e1ea7ede1dbbe 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3696,20 +3696,23 @@ def FenceAutoEnableDisableSwitch(self): self.set_rc(3, 1500) self.change_altitude(cruise_alt, relative=True) - self.progress("Fly below floor and check for no breach") - self.change_altitude(25, relative=True) + self.progress("Fly below floor and check for breach") + self.set_rc(2, 2000) + self.wait_mode("RTL") m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Got (%s)" % str(m)) - if (not (m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence Ceiling breached") + if (m.onboard_control_sensors_health & fence_bit): + raise NotAchievedException("Fence floor not breached") - self.progress("Fly above floor and check fence is not re-enabled") + self.change_mode("FBWA") + + self.progress("Fly above floor and check fence is enabled") self.set_rc(3, 2000) self.change_altitude(75, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Got (%s)" % str(m)) - if (m.onboard_control_sensors_enabled & fence_bit): - raise NotAchievedException("Fence Ceiling re-enabled") + if (not (m.onboard_control_sensors_enabled & fence_bit)): + raise NotAchievedException("Fence Floor not enabled") self.progress("Return to cruise alt") self.set_rc(3, 1500) From e96a5aa547047fb49102cd0e35e371c2a15c9670 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2024 07:58:25 +1100 Subject: [PATCH 04/10] AC_Fence: removed _curr_alt from class this is only ever set/checked within a function --- libraries/AC_Fence/AC_Fence.cpp | 6 +++--- libraries/AC_Fence/AC_Fence.h | 2 -- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index cdbf55e51fe69c..f078bd5eb0a428 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -524,7 +524,7 @@ bool AC_Fence::check_fence_alt_max() float alt; AP::ahrs().get_relative_position_D_home(alt); - _curr_alt = -alt; // translate Down to Up + const float _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence if (_curr_alt >= _alt_max) { @@ -573,7 +573,7 @@ bool AC_Fence::check_fence_alt_min() float alt; AP::ahrs().get_relative_position_D_home(alt); - _curr_alt = -alt; // translate Down to Up + const float _curr_alt = -alt; // translate Down to Up // check if we are under the altitude fence if (_curr_alt <= _alt_min) { @@ -625,7 +625,7 @@ bool AC_Fence::auto_enable_fence_floor() float alt; AP::ahrs().get_relative_position_D_home(alt); - _curr_alt = -alt; // translate Down to Up + const float _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence if (!floor_enabled() && _curr_alt >= _alt_min + _margin) { diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 3165d7b73d4d33..8ed6817d69262d 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -251,8 +251,6 @@ class AC_Fence // other internal variables 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; - // breach information uint8_t _breached_fences; // bitmask holding the fence type that was breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE) From 75655a787cdad71c846a33bace613a3a144a825f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2024 08:00:33 +1100 Subject: [PATCH 05/10] AC_Fence: removed _auto_enable_mask the _auto_enable_mask was try to make AUX function overrides disable the FENCE_AUTOENABLE functionality. This isn't the right bevaviour, both the aux function and the auto-enable should be edge triggered, with last function taking effect --- libraries/AC_Fence/AC_Fence.cpp | 37 ++++++++++++++++++++++----------- libraries/AC_Fence/AC_Fence.h | 1 - 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index f078bd5eb0a428..c9676844768075 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -219,7 +219,6 @@ void AC_Fence::update() // if someone changes the parameter we want to enable or disable everything if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) { // reset the auto mask since we just reconfigured all of fencing - _auto_enable_mask = AC_FENCE_ALL_FENCES; _last_enabled = _enabled; _last_auto_enabled = _auto_enabled; if (_enabled) { @@ -256,11 +255,6 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) return 0; } - // fences that were manually changed are no longer eligible for auto-enablement or disablement - if (update_auto_mask) { - _auto_enable_mask &= ~fences_to_change; - } - #if HAL_LOGGING_ENABLED AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE); if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) { @@ -306,7 +300,7 @@ void AC_Fence::auto_enable_fence_on_arming(void) return; } - const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false); + const uint8_t fences = enable(true, AC_FENCE_ARMING_FENCES, false); print_fence_message("auto-enabled", fences); } @@ -319,7 +313,7 @@ void AC_Fence::auto_disable_fence_on_disarming(void) return; } - const uint8_t fences = enable(false, _auto_enable_mask, false); + const uint8_t fences = enable(false, AC_FENCE_ALL_FENCES, false); print_fence_message("auto-disabled", fences); } @@ -333,7 +327,7 @@ void AC_Fence::auto_enable_fence_after_takeoff(void) return; } - const uint8_t fences = enable(true, _auto_enable_mask, false); + const uint8_t fences = enable(true, AC_FENCE_ALL_FENCES, false); print_fence_message("auto-enabled", fences); } @@ -343,12 +337,12 @@ 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: - auto_disable = _auto_enable_mask; + auto_disable = AC_FENCE_ALL_FENCES; 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; + auto_disable = AC_FENCE_TYPE_ALT_MIN; break; } return auto_disable; @@ -616,7 +610,6 @@ 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 - || !(_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 @@ -727,6 +720,26 @@ uint8_t AC_Fence::check(bool disable_auto_fences) print_fence_message("auto-disabled", fences_to_disable); } +#if 0 + /* + this debug log message is very useful both when developing tests + and doing manual SITL fence testing + */ + { + float alt; + AP::ahrs().get_relative_position_D_home(alt); + + AP::logger().WriteStreaming("FENC", "TimeUS,EN,AE,CF,EF,DF,Alt", "QIIIIIf", + AP_HAL::micros64(), + enabled(), + _auto_enabled, + _configured_fences, + get_enabled_fences(), + disabled_fences, + alt*-1); + } +#endif + // return immediately if disabled if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) { return 0; diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 8ed6817d69262d..84c1ddef5a1441 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -249,7 +249,6 @@ class AC_Fence float _circle_breach_distance; // distance beyond the circular fence // other internal variables - 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) // breach information From 076782f6f18bc200385f6d4ee362c121aa7d6b79 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2024 08:41:05 +1100 Subject: [PATCH 06/10] 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 --- libraries/AC_Fence/AC_Fence.cpp | 26 ++++++++++++++++++++++++-- libraries/AC_Fence/AC_Fence.h | 7 +++++++ 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index c9676844768075..d6255bd44912b2 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -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); diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 84c1ddef5a1441..c4a821f6e2aa40 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -259,6 +259,13 @@ class AC_Fence 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 }; From 9dc6e1cbea2a0c12aae541d1a988374df5cb4a23 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2024 08:41:51 +1100 Subject: [PATCH 07/10] autotest: update plane switch fence test the FENCE_AUTOENABLE option should be honoured even with a fence switch in the disable position --- Tools/autotest/arduplane.py | 59 +++++++++++++++++++++++++++++++------ 1 file changed, 50 insertions(+), 9 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 9e1ea7ede1dbbe..cdfa88c8b19f03 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3661,7 +3661,7 @@ def FenceMinAltAutoEnableAbort(self): def FenceAutoEnableDisableSwitch(self): '''Tests autoenablement of regular fences and manual disablement''' self.set_parameters({ - "FENCE_TYPE": 11, # Set fence type to min alt + "FENCE_TYPE": 9, # Set fence type to min alt, max alt "FENCE_ACTION": 1, # Set action to RTL "FENCE_ALT_MIN": 50, "FENCE_ALT_MAX": 100, @@ -3672,35 +3672,60 @@ def FenceAutoEnableDisableSwitch(self): "FENCE_RET_ALT" : 0, "FENCE_RET_RALLY" : 0, "FENCE_TOTAL" : 0, + "RTL_ALTITUDE" : 75, "TKOFF_ALT" : 75, "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality }) + self.reboot_sitl() + self.context_collect("STATUSTEXT") + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE # Grab Home Position self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.set_rc_from_map({7: 1000}) # Turn fence off with aux function + self.set_rc(7, 1000) # Turn fence off with aux function, does not impact later auto-enable self.wait_ready_to_arm() + + self.progress("Check fence disabled at boot") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence is enabled at boot") + cruise_alt = 75 self.takeoff(cruise_alt, mode='TAKEOFF') - self.progress("Fly above ceiling and check there is no breach") + self.progress("Fly above ceiling and check there is a breach") + self.change_mode('FBWA') self.set_rc(3, 2000) - self.change_altitude(cruise_alt + 80, relative=True) + self.set_rc(2, 1000) + + self.wait_statustext("Max Alt fence breached", timeout=10, check_context=True) + self.wait_mode('RTL') + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) - if (not (m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence Ceiling breached") + if (m.onboard_control_sensors_health & fence_bit): + raise NotAchievedException("Fence ceiling not breached") + + self.set_rc(3, 1500) + self.set_rc(2, 1500) + + self.progress("Wait for RTL alt reached") + self.wait_altitude(cruise_alt-5, cruise_alt+5, relative=True, timeout=30) self.progress("Return to cruise alt") self.set_rc(3, 1500) self.change_altitude(cruise_alt, relative=True) + self.progress("Check fence breach cleared") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence breach not cleared") + self.progress("Fly below floor and check for breach") self.set_rc(2, 2000) + self.wait_statustext("Min Alt fence breached", timeout=10, check_context=True) self.wait_mode("RTL") m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) if (m.onboard_control_sensors_health & fence_bit): raise NotAchievedException("Fence floor not breached") @@ -3710,10 +3735,26 @@ def FenceAutoEnableDisableSwitch(self): self.set_rc(3, 2000) self.change_altitude(75, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) if (not (m.onboard_control_sensors_enabled & fence_bit)): raise NotAchievedException("Fence Floor not enabled") + self.progress("Toggle fence enable/disable") + self.set_rc(7, 2000) + self.delay_sim_time(2) + self.set_rc(7, 1000) + self.delay_sim_time(2) + + self.progress("Check fence is disabled") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence disable with switch failed") + + self.progress("Fly below floor and check for no breach") + self.change_altitude(40, relative=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence floor breached") + self.progress("Return to cruise alt") self.set_rc(3, 1500) self.change_altitude(cruise_alt, relative=True) From c9e9b13404b19933aeb6038cd5e801288227fb47 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2024 09:58:58 +1100 Subject: [PATCH 08/10] autotest: fixed race in MinAltFenceAvoid copter test --- Tools/autotest/arducopter.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 2d968b900d9faf..d6325334cc44da 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1719,8 +1719,6 @@ def MinAltFence(self): # Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance def MinAltFenceAvoid(self): '''Test Min Alt Fence Avoidance''' - self.takeoff(30, mode="LOITER") - """Hold loiter position.""" # enable fence, only min altitude # No action, rely on avoidance to prevent the breach @@ -1731,6 +1729,9 @@ def MinAltFenceAvoid(self): "FENCE_ACTION": 0, }) + self.takeoff(30, mode="LOITER") + """Hold loiter position.""" + # Try and fly past the fence self.set_rc(3, 1120) From 6173356b132ba756f6303913874e3376ec40ae0d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2024 13:55:21 +1100 Subject: [PATCH 09/10] autotest: fixed race condition in Copter.MinAltFenceAvoid --- Tools/autotest/arducopter.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d6325334cc44da..1de6f855c4243f 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1728,6 +1728,7 @@ def MinAltFenceAvoid(self): "FENCE_ALT_MIN": 20, "FENCE_ACTION": 0, }) + self.reboot_sitl() self.takeoff(30, mode="LOITER") """Hold loiter position.""" From 6e263532c8aa5943171a446fd8f53f0f6200d51a Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Mon, 9 Dec 2024 09:56:46 -0600 Subject: [PATCH 10/10] RC_Channel:add QuickTune metadat --- libraries/RC_Channel/RC_Channel.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 1d4c7cc77fd0ca..d6f0d90b4a539c 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -242,6 +242,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Copter}: 178:FlightMode Pause/Resume // @Values{Plane}: 179:ICEngine start / stop // @Values{Copter, Plane}: 180:Test autotuned gains after tune is complete + // @Values{Plane}: 181: QuickTune // @Values{Rover}: 201:Roll // @Values{Rover}: 202:Pitch // @Values{Rover}: 207:MainSail