From c8a4af76fe542812dff7c6d87f63fa947cb5b391 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 1 Jun 2019 15:07:49 +1000 Subject: [PATCH] Tools: autotest: add tests for Plane fence --- Tools/autotest/CMAC-fence.txt | 6 + Tools/autotest/arduplane.py | 253 ++++++++++++++++++++++++++++++++++ Tools/autotest/common.py | 27 +++- 3 files changed, 285 insertions(+), 1 deletion(-) create mode 100644 Tools/autotest/CMAC-fence.txt diff --git a/Tools/autotest/CMAC-fence.txt b/Tools/autotest/CMAC-fence.txt new file mode 100644 index 0000000000..a6169e4e3c --- /dev/null +++ b/Tools/autotest/CMAC-fence.txt @@ -0,0 +1,6 @@ +-35.363720 149.163651 +-35.358738 149.165070 +-35.359295 149.154434 +-35.372292 149.157135 +-35.368290 149.166809 +-35.358738 149.165070 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ca97b7fbc0..1d33995cea 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -16,6 +16,8 @@ from common import AutoTestTimeoutException from common import NotAchievedException from common import PreconditionFailedException +from MAVProxy.modules.lib import mp_util + # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 585, 354) @@ -846,6 +848,245 @@ class AutoTestPlane(AutoTest): if ex is not None: raise ex + def assert_fence_sys_status(self, present, enabled, health): + self.delay_sim_time(1) + self.drain_mav_unparsed() + m = self.mav.recv_match(type='SYS_STATUS', blocking=True, timeout=1) + if m is None: + raise NotAchievedException("Did not receive SYS_STATUS") + tests = [ ( "present", present, m.onboard_control_sensors_present ), + ( "enabled", enabled, m.onboard_control_sensors_enabled ), + ( "health", health, m.onboard_control_sensors_health ), + ] + bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + for test in tests: + (name, want, field) = test + got = (field & bit) != 0 + if want != got: + raise NotAchievedException("fence status incorrect; %s want=%u got=%u" % + (name, want, got)) + + def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + if value: + p1 = 1 + else: + p1 = 0 + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, + p1, # param1 + 0, # param2 + 0, # param3 + 0, # param4 + 0, # param5 + 0, # param6 + 0, # param7 + want_result=want_result) + + def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.do_fence_en_or_dis_able(True, want_result=want_result) + + def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.do_fence_en_or_dis_able(False, want_result=want_result) + + def wait_circling_point_with_radius(self, loc, want_radius, epsilon=2.0, min_circle_time=5): + on_radius_start_heading = None + average_radius = 0.0 + circle_time_start = 0 + done_time = False + done_angle = False + while True: + here = self.mav.location() + got_radius = self.get_distance(loc, here) + average_radius = 0.95*average_radius + 0.05*got_radius + now = self.get_sim_time() + on_radius = abs(got_radius - want_radius) < epsilon + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + heading = m.heading + on_string = "off" + got_angle = "" + if on_radius_start_heading is not None: + got_angle = "%0.2f" % abs(on_radius_start_heading - heading) # FIXME + on_string = "on" + + want_angle = 180 # we don't actually get this (angle-substraction issue. But we get enough... + self.progress("wait-circling: got-r=%0.2f want-r=%f avg-r=%f %s want-a=%0.1f got-a=%s" % + (got_radius, want_radius, average_radius, on_string, want_angle, got_angle)) + if on_radius: + if on_radius_start_heading is None: + on_radius_start_heading = heading + average_radius = got_radius + circle_time_start = self.get_sim_time() + continue + if abs(on_radius_start_heading - heading) > want_angle: # FIXME + done_angle = True + if self.get_sim_time() - circle_time_start > min_circle_time: + done_time = True + if done_time and done_angle: + return + continue + if on_radius_start_heading is not None: + average_radius = 0.0 + on_radius_start_heading = None + circle_time_start = 0 + + def test_fence_static(self): + ex = None + try: + self.progress("Checking for bizarre healthy-when-not-present-or-enabled") + self.assert_fence_sys_status(False, False, True) + fence_filepath = os.path.join(self.mission_directory(), + "CMAC-fence.txt") + self.mavproxy.send("fence load %s\n" % fence_filepath) + self.mavproxy.expect("Loaded 6 geo-fence") + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + if m is not None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly"); + self.drain_mav_unparsed() + self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE) # report only + self.assert_fence_sys_status(False, False, True) + self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) # report only + self.assert_fence_sys_status(True, False, True) + self.mavproxy.send('fence enable\n') + self.mavproxy.expect("fence enabled") + self.assert_fence_sys_status(True, True, True) + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + if m is None: + raise NotAchievedException("Did not get FENCE_STATUS"); + if m.breach_status: + raise NotAchievedException("Breached fence unexpectedly (%u)" % + (m.breach_status)) + self.mavproxy.send('fence disable\n') + self.mavproxy.expect("fence disabled") + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE) + self.assert_fence_sys_status(False, False, True) + self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) + self.assert_fence_sys_status(True, False, True) + self.mavproxy.send("fence clear\n") + self.mavproxy.expect("fence removed") + if self.get_parameter("FENCE_TOTAL") != 0: + raise NotAchievedException("Expected zero points remaining") + self.assert_fence_sys_status(False, False, True) + self.progress("Trying to enable fence with no points") + self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED) + + # test a rather unfortunate behaviour: + self.progress("Killing a live fence with fence-clear") + self.mavproxy.send("fence load %s\n" % fence_filepath) + self.mavproxy.expect("Loaded 6 geo-fence") + self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) + self.do_fence_enable() + self.assert_fence_sys_status(True, True, True) + self.mavproxy.send("fence clear\n") + self.mavproxy.expect("fence removed") + if self.get_parameter("FENCE_TOTAL") != 0: + raise NotAchievedException("Expected zero points remaining") + self.assert_fence_sys_status(False, False, True) + + except Exception as e: + self.progress("Exception caught:") + self.progress(self.get_exception_stacktrace(e)) + ex = e + self.mavproxy.send('fence clear\n') + if ex is not None: + raise ex + + def test_fence_breach_circle_at(self, loc): + ex = None + try: + fence_filepath = os.path.join(self.mission_directory(), + "CMAC-fence.txt") + self.mavproxy.send("fence load %s\n" % fence_filepath) + self.mavproxy.expect("Loaded 6 geo-fence") + want_radius = 100 + # when ArduPlane is fixed, remove this fudge factor + REALLY_BAD_FUDGE_FACTOR = 1.16 + expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius + self.set_parameter("RTL_RADIUS", want_radius) + self.set_parameter("NAVL1_LIM_BANK", 60) + self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) + + self.do_fence_enable() + self.assert_fence_sys_status(True, True, True) + + self.takeoff(alt=45, alt_max=300) + + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > 30: + raise NotAchievedException("Did not breach fence") + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + if m is None: + raise NotAchievedException("Did not get FENCE_STATUS"); + if m.breach_status == 0: + continue + + # we've breached; check our state; + if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY: + raise NotAchievedException("Unexpected breach type %u" % + (m.breach_type,)) + if m.breach_count == 0: + raise NotAchievedException("Unexpected breach count %u" % + (m.breach_count,)) + self.assert_fence_sys_status(True, True, False) + break + + self.wait_circling_point_with_radius(loc, expected_radius) + + self.disarm_vehicle(force=True) + self.reboot_sitl() + + except Exception as e: + self.progress("Exception caught:") + self.progress(self.get_exception_stacktrace(e)) + ex = e + self.mavproxy.send('fence clear\n') + if ex is not None: + raise ex + + def test_fence_rtl(self): + self.progress("Testing FENCE_ACTION_RTL no rally point") + self.test_fence_breach_circle_at(self.home_position_as_mav_location()) + + def location_offset_ne(self, location, north, east): + print("old: %f %f" % (location.lat, location.lng)) + (lat, lng) = mp_util.gps_offset(location.lat, location.lng, east, north) + location.lat = lat + location.lng = lng + print("new: %f %f" % (location.lat, location.lng)) + + def test_fence_rtl_rally(self): + ex = None + target_system = 1 + target_component = 1 + try: + self.progress("Testing FENCE_ACTION_RTL with rally point") + + self.wait_ready_to_arm() + loc = self.home_position_as_mav_location() + self.location_offset_ne(loc, 50, -50) + + self.set_parameter("RALLY_TOTAL", 1) + self.mav.mav.rally_point_send(target_system, + target_component, + 0, # sequence number + 1, # total count + int(loc.lat * 1e7), + int(loc.lng * 1e7), + 15, + 0, # "break" alt?! + 0, # "land dir" + 0) # flags + self.delay_sim_time(1) + self.mavproxy.send("rally list\n") + self.test_fence_breach_circle_at(loc) + except Exception as e: + self.progress("Exception caught:") + self.progress(self.get_exception_stacktrace(e)) + ex = e + self.mavproxy.send('rally clear\n') + if ex is not None: + raise ex + def test_parachute(self): self.set_rc(9, 1000) self.set_parameter("CHUTE_ENABLED", 1) @@ -1045,6 +1286,18 @@ class AutoTestPlane(AutoTest): "Test RangeFinder Basic Functionality", self.test_rangefinder), + ("FenceStatic", + "Test Basic Fence Functionality", + self.test_fence_static), + + ("FenceRTL", + "Test Fence RTL", + self.test_fence_rtl), + + ("FenceRTLRally", + "Test Fence RTL Rally", + self.test_fence_rtl_rally), + ("LogDownLoad", "Log download", lambda: self.log_download( diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index c31067b9f4..19201045b6 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -13,6 +13,8 @@ import pexpect import fnmatch import operator +from MAVProxy.modules.lib import mp_util + from pymavlink import mavwp, mavutil, DFReader from pysim import util, vehicleinfo @@ -1170,16 +1172,35 @@ class AutoTest(ABC): ################################################# # UTILITIES ################################################# + @staticmethod + def longitude_scale(lat): + ret = math.cos(lat * (math.radians(1))); + print("scale=%f" % ret) + return ret + @staticmethod def get_distance(loc1, loc2): """Get ground distance between two locations.""" + return AutoTest.get_distance_accurate(loc1, loc2) dlat = loc2.lat - loc1.lat try: dlong = loc2.lng - loc1.lng except AttributeError: dlong = loc2.lon - loc1.lon - return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5 + return math.sqrt((dlat*dlat) + (dlong*dlong)*AutoTest.longitude_scale(loc2.lat)) * 1.113195e5 + + @staticmethod + def get_distance_accurate(loc1, loc2): + """Get ground distance between two locations.""" + try: + lon1 = loc1.lng + lon2 = loc2.lng + except AttributeError: + lon1 = loc1.lon + lon2 = loc2.lon + + return mp_util.gps_distance(loc1.lat, lon1, loc2.lat, lon2) @staticmethod def get_latlon_attr(loc, attrs): @@ -1984,6 +2005,10 @@ class AutoTest(ABC): blocking=True) return self.get_distance_int(m, here) + def home_position_as_mav_location(self): + m = self.poll_home_position() + return mavutil.location(m.latitude*1.0e-7, m.longitude*1.0e-7, m.altitude*1.0e-3, 0) + def monitor_groundspeed(self, want, tolerance=0.5, timeout=5): tstart = self.get_sim_time() while True: