From aad02a1a89505a9546ee245f368e4d4a4fb8a9f5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 16:54:19 +0100 Subject: [PATCH] autotest: test that vehicle can be landed manually after descending below fence floor --- Tools/autotest/arducopter.py | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 8cbe0e35b9da16..54364aae29c35b 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1476,7 +1476,7 @@ def HorizontalFence(self, timeout=180): self.load_fence("fence-in-middle-of-nowhere.txt") self.delay_sim_time(5) # let fence check run so it loads-from-eeprom - self.assert_prearm_failure("vehicle outside fence") + self.assert_prearm_failure("vehicle outside Polygon fence") self.progress("Failed to arm outside fence (good!)") self.clear_fence() self.delay_sim_time(5) # let fence breach clear @@ -1486,7 +1486,7 @@ def HorizontalFence(self, timeout=180): self.start_subtest("ensure we can't arm with bad radius") self.context_push() self.set_parameter("FENCE_RADIUS", -1) - self.assert_prearm_failure("Invalid FENCE_RADIUS value") + self.assert_prearm_failure("Invalid Circle FENCE_RADIUS value") self.context_pop() self.progress("Failed to arm with bad radius") self.drain_mav() @@ -1704,14 +1704,14 @@ def FenceFloorEnabledLanding(self): "AVOID_ENABLE": 0, "FENCE_ENABLE" : 1, "FENCE_TYPE": 15, - "FENCE_ALT_MIN": 10, - "FENCE_ALT_MAX": 20, + "FENCE_ALT_MIN": 20, + "FENCE_ALT_MAX": 30, }) self.change_mode("GUIDED") self.wait_ready_to_arm() self.arm_vehicle() - self.user_takeoff(alt_min=15) + self.user_takeoff(alt_min=25) # Check fence is enabled self.do_fence_enable() @@ -1723,8 +1723,19 @@ def FenceFloorEnabledLanding(self): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) + # center throttle + self.set_rc(3, 1500) - self.wait_landed_and_disarmed() + # wait until we are below the fence floor and re-enter loiter + self.wait_altitude(5, 15, relative=True) + self.change_mode('LOITER') + # wait for manual recovery to expire + self.delay_sim_time(15) + + # lower throttle and try and land + self.set_rc(3, 1300) + self.wait_altitude(0, 2, relative=True) + self.wait_disarmed() self.assert_fence_enabled() # Assert fence is not healthy since it was enabled manually