diff --git a/Tools/autotest/ArduPlane_Tests/FenceNoFenceReturnPointInclusion/flaps.txt b/Tools/autotest/ArduPlane_Tests/FenceNoFenceReturnPointInclusion/flaps.txt new file mode 100644 index 0000000000..1c38e7f211 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/FenceNoFenceReturnPointInclusion/flaps.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.358878 149.163760 80.000000 1 +6 0 0 177 2.000000 3.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.358867 149.164409 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359862 149.164697 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.360670 149.164857 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 37c4445149..e6487105d6 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2706,8 +2706,8 @@ class AutoTestPlane(AutoTest): self.progress("Enable fence and initiate fence action") self.do_fence_enable() self.assert_fence_enabled() - self.wait_mode("GUIDED") # We should RTL because of fence breach - self.delay_sim_time(30) + self.wait_mode("GUIDED", timeout=120) # We should RTL because of fence breach + self.delay_sim_time(60) items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) if len(items) != 4: @@ -2749,6 +2749,62 @@ class AutoTestPlane(AutoTest): self.do_fence_disable() self.fly_home_land_and_disarm() + def test_fence_breach_no_return_point_no_inclusion(self): + """ Test result when a breach occurs and No fence return point is present and + no inclusion fence is present and exclusion fence is present """ + want_radius = 100 # Fence Return Radius + + self.set_parameter("FENCE_ACTION", 6) + self.set_parameter("FENCE_TYPE", 4) + self.set_parameter("RTL_RADIUS", want_radius) + self.set_parameter("NAVL1_LIM_BANK", 60) + + # Generate an exclusion fence just north of takeoff point + home_loc = self.mav.location() + locs = [ + mavutil.location(home_loc.lat + 0.010, home_loc.lng - 0.003, 0, 0), + mavutil.location(home_loc.lat + 0.010, home_loc.lng + 0.003, 0, 0), + mavutil.location(home_loc.lat + 0.008, home_loc.lng + 0.003, 0, 0), + mavutil.location(home_loc.lat + 0.008, home_loc.lng - 0.003, 0, 0), + ] + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + [ + locs + ] + ) + self.delay_sim_time(1) + self.wait_ready_to_arm() + self.takeoff(alt=50) + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + self.progress("Enable fence and initiate fence action") + self.do_fence_enable() + self.assert_fence_enabled() + self.wait_mode("GUIDED") # We should RTL because of fence breach + self.delay_sim_time(30) + + items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + if len(items) != 4: + raise NotAchievedException("Unexpected fencepoint count (want=%u got=%u)" % (4, len(items))) + + # Check there are no fence return points specified still + for fence_loc in items: + if fence_loc.command == mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT: + raise NotAchievedException( + "Unexpected fence return point found (%u) got %u" % + (fence_loc.command, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT)) + + # Wait for guided return to vehicle calculated fence return location + self.wait_distance_to_location(home_loc, 90, 110) + self.wait_circling_point_with_radius(home_loc, 92) + + self.progress("Test complete, disable fence and come home") + self.do_fence_disable() + self.fly_home_land_and_disarm() + def test_fence_disable_under_breach_action(self): """ Fence breach will cause the vehicle to enter guided mode. Upon breach clear, check the vehicle is in the expected mode""" @@ -2895,6 +2951,10 @@ class AutoTestPlane(AutoTest): "Tests calculated return point during fence breach when no fence return point present", self.test_fence_breach_no_return_point), + ("FenceNoFenceReturnPointInclusion", + "Tests using home as fence return point when none is present, and no inclusion fence is uploadedg", + self.test_fence_breach_no_return_point_no_inclusion), + ("FenceDisableUnderAction", "Tests Disabling fence while undergoing action caused by breach", self.test_fence_disable_under_breach_action),