diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index fcff9c5b00bc6d..cc98e4b09f40f0 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1417,7 +1417,7 @@ def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_beha 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 @@ -1762,8 +1762,6 @@ def FenceFloorAutoDisableLanding(self): 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 @@ -11679,7 +11677,6 @@ def tests(self): 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 629bdb210e497b..d9d79775fad1c6 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3739,6 +3739,49 @@ def FenceMinAltAutoEnable(self): 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({ @@ -5712,6 +5755,7 @@ def tests(self): self.FenceRetRally, self.FenceAltCeilFloor, self.FenceMinAltAutoEnable, + self.FenceMinAltEnableAutoland, self.FenceMinAltAutoEnableAbort, self.FenceAutoEnableDisableSwitch, self.FenceEnableDisableSwitch,