From 8545715837e1b48cde3e8ffe3e40cd85b03b5603 Mon Sep 17 00:00:00 2001 From: James O'Shannessy <12959316+joshanne@users.noreply.github.com> Date: Tue, 16 Feb 2021 14:20:56 +1100 Subject: [PATCH] AutoTest: Add Static Fence Tests for unallowed arming when breaching geofence --- Tools/autotest/arduplane.py | 84 ++++++++++++++++++++++++++++++++++++- Tools/autotest/common.py | 16 ------- 2 files changed, 82 insertions(+), 18 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 9b90cdbc6b..5843b0b078 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1122,7 +1122,7 @@ class AutoTestPlane(AutoTest): # test a rather unfortunate behaviour: self.progress("Killing a live fence with fence-clear") self.load_fence("CMAC-fence.txt") - self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) + self.set_parameter("FENCE_ACTION", 1) # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 self.do_fence_enable() self.assert_fence_sys_status(True, True, True) self.clear_fence() @@ -1141,6 +1141,61 @@ class AutoTestPlane(AutoTest): self.set_parameter("FENCE_TYPE", 2) # tin can self.assert_fence_sys_status(True, False, True) + # Test cannot arm if outside of fence and fence is enabled + self.progress("Test Arming while vehicle below FENCE_ALT_MIN") + default_fence_alt_min = self.get_parameter("FENCE_ALT_MIN") + self.set_parameter("FENCE_ALT_MIN", 50) + self.set_parameter("FENCE_TYPE", 8) # Enables minimum altitude breaches + self.do_fence_enable() + self.assert_fence_enabled() + self.try_arm(False, "vehicle outside fence") + self.do_fence_disable() + self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) + + # Test arming outside inclusion zone + self.progress("Test arming while vehicle outside of inclusion zone") + self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types + locs = [ + mavutil.location(1, 1.0, 0, 0), + mavutil.location(1.0, 1.001, 0, 0), + mavutil.location(1.001, 1.001, 0, 0), + mavutil.location(1.001, 1.0, 0, 0) + ] + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + [ + locs + ]) + self.delay_sim_time(10) # let fence check run so it loads-from-eeprom + self.do_fence_enable() + self.assert_fence_enabled() + self.try_arm(False, "vehicle outside fence") + self.do_fence_disable() + self.clear_fence() + + self.progress("Test arming while vehicle inside exclusion zone") + self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types + 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_EXCLUSION, + [ + locs + ]) + self.delay_sim_time(10) # let fence check run so it loads-from-eeprom + self.do_fence_enable() + self.assert_fence_enabled() + self.try_arm(False, "vehicle outside fence") + self.do_fence_disable() + self.clear_fence() + + + except Exception as e: @@ -2369,8 +2424,29 @@ class AutoTestPlane(AutoTest): self.fly_home_land_and_disarm(timeout=150) + 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""" + self.set_parameter("FENCE_ALT_MIN", 50) # Sets the fence floor + self.set_parameter("FENCE_TYPE", 8) # Only use fence floor for breaches + self.wait_ready_to_arm() + + def attempt_fence_breached_disable(start_mode, end_mode, expected_mode, action): + self.set_parameter("FENCE_ACTION", action) # Set Fence Action to Guided + self.change_mode(start_mode) + self.arm_vehicle() + self.do_fence_enable() + self.assert_fence_enabled() + self.wait_mode(expected_mode) + self.do_fence_disable() + self.assert_fence_disabled() + self.wait_mode(end_mode) + self.disarm_vehicle(force=True) + + attempt_fence_breached_disable(start_mode="FBWA", end_mode="RTL", expected_mode="RTL", action=1) + attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=5) + attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=6) - def tests(self): '''return list of all tests''' @@ -2445,6 +2521,10 @@ class AutoTestPlane(AutoTest): "Tests the fence ceiling and floor", self.test_fence_alt_ceil_floor), + ("FenceDisableUnderAction", + "Tests Disabling fence while undergoing action caused by breach", + self.test_fence_disable_under_breach_action), + ("ADSB", "Test ADSB", self.test_adsb), diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 2b5f18b100..d11d9b13ea 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1514,22 +1514,6 @@ class AutoTest(ABC): if m is None: raise ValueError("Did not match (%s)" % line) locs.append(mavutil.location(float(m.group(1)), float(m.group(2)), 0, 0)) - if self.is_plane(): - # create return point as the centroid: - total_lat = 0 - total_lng = 0 - total_cnt = 0 - for loc in locs: - total_lat += loc.lat - total_lng += loc.lng - total_cnt += 1 - locs2 = [mavutil.location(total_lat/total_cnt, - total_lng/total_cnt, - 0, - 0)] # return point - locs2.extend(locs) - locs2.append(copy.copy(locs2[1])) - return self.roundtrip_fence_using_fencepoint_protocol(locs2) self.upload_fences_from_locations( mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,