From bcc0da9c47d94b68605f9faf6589cec27a63c4c5 Mon Sep 17 00:00:00 2001 From: James O'Shannessy <12959316+joshanne@users.noreply.github.com> Date: Thu, 4 Mar 2021 13:22:49 +1100 Subject: [PATCH] AutoTest: Adds additional autotests to capture mode change while breached AutoTest: Adds test for fence breach switching to guided mode when no fence return point is present. In upstream, this results in a vehicle fly-away. --- .../FenceBreachedChangeMode/flaps.txt | 13 ++ .../FenceNoFenceReturnPoint/flaps.txt | 13 ++ Tools/autotest/arduplane.py | 118 ++++++++++++++++++ 3 files changed, 144 insertions(+) create mode 100644 Tools/autotest/ArduPlane_Tests/FenceBreachedChangeMode/flaps.txt create mode 100644 Tools/autotest/ArduPlane_Tests/FenceNoFenceReturnPoint/flaps.txt diff --git a/Tools/autotest/ArduPlane_Tests/FenceBreachedChangeMode/flaps.txt b/Tools/autotest/ArduPlane_Tests/FenceBreachedChangeMode/flaps.txt new file mode 100644 index 0000000000..93d0726c86 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/FenceBreachedChangeMode/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.390015 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.361027 149.164093 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.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 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_Tests/FenceNoFenceReturnPoint/flaps.txt b/Tools/autotest/ArduPlane_Tests/FenceNoFenceReturnPoint/flaps.txt new file mode 100644 index 0000000000..1c38e7f211 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/FenceNoFenceReturnPoint/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 7df025a2a0..13e10b3439 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2504,6 +2504,116 @@ class AutoTestPlane(AutoTest): self.fly_home_land_and_disarm(timeout=150) + def test_fence_breached_change_mode(self): + """ Attempts to change mode while a fence is breached. + This should revert to the mode specified by the fence action. """ + self.set_parameter("FENCE_ACTION", 1) + self.set_parameter("FENCE_TYPE", 4) + home_loc = self.mav.location() + locs = [ + mavutil.location(home_loc.lat - 0.001, home_loc.lng - 0.001, 0, 0), + mavutil.location(home_loc.lat - 0.001, home_loc.lng + 0.001, 0, 0), + mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0), + mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), + ] + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + [ + 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("RTL") # We should RTL because of fence breach + + self.progress("User mode change to cruise should retrigger fence action") + self.change_mode("CRUISE") + self.wait_mode("RTL", timeout=5) + + self.progress("Test complete, disable fence and come home") + self.do_fence_disable() + self.fly_home_land_and_disarm() + + def test_fence_breach_no_return_point(self): + """ Attempts to change mode while a fence is breached. + This should revert to the mode specified by the fence action. """ + 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) + home_loc = self.mav.location() + locs = [ + mavutil.location(home_loc.lat - 0.003, home_loc.lng - 0.001, 0, 0), + mavutil.location(home_loc.lat - 0.003, home_loc.lng + 0.003, 0, 0), + mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.003, 0, 0), + mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), + ] + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + [ + 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)) + + # Work out the approximate return point when no fence return point present + # Logic taken from AC_PolyFence_loader.cpp + min_loc = self.mav.location() + max_loc = self.mav.location() + for new_loc in locs: + if new_loc.lat < min_loc.lat: + min_loc.lat = new_loc.lat + if new_loc.lng < min_loc.lng: + min_loc.lng = new_loc.lng + if new_loc.lat > max_loc.lat: + max_loc.lat = new_loc.lat + if new_loc.lng > max_loc.lng: + max_loc.lng = new_loc.lng + + # Generate the return location based on min and max locs + ret_lat = (min_loc.lat + max_loc.lat) / 2 + ret_lng = (min_loc.lng + max_loc.lng) / 2 + ret_loc = mavutil.location(ret_lat, ret_lng, 0, 0) + self.progress("Return loc: (%s)" % str(ret_loc)) + + # Wait for guided return to vehicle calculated fence return location + self.wait_distance_to_location(ret_loc, 105, 115) + self.wait_circling_point_with_radius(ret_loc, want_radius) + + 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""" @@ -2604,6 +2714,14 @@ class AutoTestPlane(AutoTest): "Tests the fence ceiling and floor", self.test_fence_alt_ceil_floor), + ("FenceBreachedChangeMode", + "Tests retrigger of fence action when changing of mode while fence is breached", + self.test_fence_breached_change_mode), + + ("FenceNoFenceReturnPoint", + "Tests calculated return point during fence breach when no fence return point present", + self.test_fence_breach_no_return_point), + ("FenceDisableUnderAction", "Tests Disabling fence while undergoing action caused by breach", self.test_fence_disable_under_breach_action),