diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index e0eda4cdae..84303118b7 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1454,7 +1454,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.wait_altitude(10, 100, relative=True) self.set_rc(3, 1500) self.set_rc(2, 1400) - self.wait_distance_to_home(12, 20) + self.wait_distance_to_home(12, 20, timeout=30) tstart = self.get_sim_time() push_time = 70 # push against barrier for 60 seconds failed_max = False @@ -1513,7 +1513,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): 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 @@ -1523,7 +1523,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): 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() @@ -1741,14 +1741,14 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): "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() @@ -1760,8 +1760,19 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): 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 @@ -1799,8 +1810,6 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) - # Assert fence is not healthy now that we are in RTL - self.assert_sensor_state(fence_bit, healthy=False) self.wait_landed_and_disarmed(0) # the breach should have cleared since we auto-disable the @@ -12005,7 +12014,6 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): def disabled_tests(self): return { "Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702", - "HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525", "AltEstimation": "See https://github.com/ArduPilot/ardupilot/issues/15191", "GroundEffectCompensation_takeOffExpected": "Flapping", "GroundEffectCompensation_touchDownExpected": "Flapping", diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 24821c55ea..6e4122eaf3 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1601,7 +1601,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.delay_sim_time(2) # Allow breach to propagate self.assert_fence_enabled() - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Min Alt fence") self.do_fence_disable() self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) @@ -1620,7 +1620,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.do_fence_enable() self.assert_fence_enabled() self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Polygon fence") self.do_fence_disable() self.clear_fence() @@ -1640,7 +1640,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.do_fence_enable() self.assert_fence_enabled() self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Polygon fence") self.do_fence_disable() self.clear_fence() @@ -3742,6 +3742,49 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.set_current_waypoint(0, check_afterwards=False) self.set_rc(3, 1000) # lower throttle + def FenceMinAltEnableAutoland(self): + '''Tests autolanding when alt min fence is enabled''' + self.set_parameters({ + "FENCE_TYPE": 12, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 20, + "FENCE_AUTOENABLE": 0, + "FENCE_ENABLE" : 1, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # switch to FBWA + self.change_mode("FBWA") + self.set_rc(3, 1500) # raise throttle + self.wait_altitude(25, 35, timeout=50, relative=True) + self.set_rc(3, 1000) # lower throttle + # Now check we can land + self.fly_home_land_and_disarm() + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.set_current_waypoint(0, check_afterwards=False) + self.set_rc(3, 1000) # lower throttle + def FenceMinAltAutoEnableAbort(self): '''Tests autoenablement of the alt min fence and fences on arming''' self.set_parameters({ @@ -5776,6 +5819,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.FenceRetRally, self.FenceAltCeilFloor, self.FenceMinAltAutoEnable, + self.FenceMinAltEnableAutoland, self.FenceMinAltAutoEnableAbort, self.FenceAutoEnableDisableSwitch, self.FenceEnableDisableSwitch, diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 22c38c5c50..941ae2f766 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6881,10 +6881,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) ]), ]) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False) self.reboot_sitl() - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.progress("Ensure we can arm when a polyfence fence is cleared when we've previously been in breach") self.clear_fence() @@ -6900,7 +6900,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) ]), ]) self.reboot_sitl() - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.clear_fence() self.wait_ready_to_arm() @@ -6914,11 +6914,11 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.offset_location_ne(here, 50, 20), # tl, ]), ]) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.set_parameter('FENCE_TYPE', 2) self.wait_ready_to_arm() self.set_parameter('FENCE_TYPE', 6) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) def OpticalFlow(self): '''lightly test OpticalFlow'''