From bba019abc2220fd0d0c46bde13019760b8733f1c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 14 Sep 2019 14:40:06 +1000 Subject: [PATCH] Tools: autotest: add basic tests for AP_AdvancedFailsafe --- Tools/autotest/arduplane.py | 4 + Tools/autotest/common.py | 173 ++++++++++++++++++++++++++++-------- 2 files changed, 138 insertions(+), 39 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index f7fd85989b..7637183387 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1418,6 +1418,10 @@ class AutoTestPlane(AutoTest): "Test ADSB", self.test_adsb), + ("AdvancedFailsafe", + "Test Advanced Failsafe", + self.test_advanced_failsafe), + ("LogDownLoad", "Log download", lambda: self.log_download( diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 8904b464c1..b731497edb 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -689,6 +689,9 @@ class AutoTest(ABC): self.mavproxy.send('rally load %s\n' % path) self.mavproxy.expect("Loaded") + def load_sample_mission(self): + self.load_mission(self.sample_mission_filename()) + def load_mission(self, filename): """Load a mission from a file to flight controller.""" self.progress("Loading mission (%s)" % filename) @@ -1178,20 +1181,18 @@ class AutoTest(ABC): z) self.run_cmd_get_ack(command, want_result, timeout) - def run_cmd(self, - command, - p1, - p2, - p3, - p4, - p5, - p6, - p7, - want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, - target_sysid=None, - target_compid=None, - timeout=10, - quiet=False): + def send_cmd(self, + command, + p1, + p2, + p3, + p4, + p5, + p6, + p7, + target_sysid=None, + target_compid=None, + ): """Send a MAVLink command long.""" if target_sysid is None: target_sysid = self.sysid_thismav() @@ -1208,6 +1209,32 @@ class AutoTest(ABC): p5, p6, p7) + + def run_cmd(self, + command, + p1, + p2, + p3, + p4, + p5, + p6, + p7, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED, + target_sysid=None, + target_compid=None, + timeout=10, + quiet=False): + self.send_cmd(command, + p1, + p2, + p3, + p4, + p5, + p6, + p7, + target_sysid=target_sysid, + target_compid=target_compid, + ) self.run_cmd_get_ack(command, want_result, timeout, quiet=quiet) def run_cmd_get_ack(self, command, want_result, timeout, quiet=False): @@ -1327,32 +1354,41 @@ class AutoTest(ABC): self.mavproxy.send('mode %s\n' % mode) self.progress("Got mode %s" % mode) + def capable(self, capability): + return self.get_autopilot_capabilities() & capability + + def assert_capability(self, capability): + if not self.capable(capability): + name = mavutil.mavlink.enums["MAV_PROTOCOL_CAPABILITY"][capability].name + raise NotAchievedException("AutoPilot does not have capbility %s" % (name,)) + + def assert_no_capability(self, capability): + if self.capable(capability): + name = mavutil.mavlink.enums["MAV_PROTOCOL_CAPABILITY"][capability].name + raise NotAchievedException("AutoPilot has feature %s (when it shouln't)" % (name,)) + + def get_autopilot_capabilities(self): + # Cannot use run_cmd otherwise the respond is lost during the wait for ACK + self.mav.mav.command_long_send(1, + 1, + mavutil.mavlink.MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, + 0, # confirmation + 1, # 1: Request autopilot version + 0, + 0, + 0, + 0, + 0, + 0) + m = self.mav.recv_match(type='AUTOPILOT_VERSION', + blocking=True, + timeout=10) + if m is None: + raise NotAchievedException("Did not get AUTOPILOT_VERSION") + return m.capabilities + def do_get_autopilot_capabilities(self): - tstart = self.get_sim_time() - while self.get_sim_time_cached() - tstart < 10: - # Cannot use run_cmd otherwise the respond is lost during the wait for ACK - self.mav.mav.command_long_send(1, - 1, - mavutil.mavlink.MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, - 0, # confirmation - 1, # 1: Request autopilot version - 0, - 0, - 0, - 0, - 0, - 0) - m = self.mav.recv_match(type='AUTOPILOT_VERSION', - blocking=True, - timeout=10) - if m is None: - continue - if not (m.capabilities & mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT): - # all AP vehicles support PARAM_FLOAT - raise ValueError("Vehicle did not report itself as supporting PARAM_FLOAT") - self.progress("AUTOPILOT_VERSION received") - return - raise AutoTestTimeoutException("No AUTOPILOT_VERSION received") + self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT) def get_mode_from_mode_mapping(self, mode): """Validate and return the mode number from a string or int.""" @@ -3142,6 +3178,65 @@ switch value''' if m is None: raise PreconditionFailedException("Did not start to get PID_TUNING message") + def sample_mission_filename(self): + return "flaps.txt" + + def test_advanced_failsafe(self): + self.context_push() + ex = None + try: + self.drain_mav() + self.assert_no_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION) + self.set_parameter("AFS_ENABLE", 1) + self.drain_mav() + self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION) + self.set_parameter("AFS_TERM_ACTION", 42) + self.load_sample_mission() + self.change_mode("AUTO") # must go to auto for AFS to latch on + self.change_mode("MANUAL") + self.start_subtest("RC Failure") + self.set_parameter("AFS_RC_FAIL_TIME", 1) + self.set_parameter("SIM_RC_FAIL", 1) + self.wait_statustext("Terminating due to RC failure") + self.set_parameter("AFS_RC_FAIL_TIME", 0) + self.set_parameter("SIM_RC_FAIL", 0) + self.set_parameter("AFS_TERMINATE", 0) + + if not self.is_plane(): + # plane requires a polygon fence... + self.start_subtest("Altitude Limit breach") + self.set_parameter("AFS_AMSL_LIMIT", 100) + self.mavproxy.send("fence enable\n") + self.wait_statustext("Terminating due to fence breach") + self.set_parameter("AFS_AMSL_LIMIT", 0) + self.set_parameter("AFS_TERMINATE", 0) + self.mavproxy.send("fence disable\n") + + self.start_subtest("GPS Failure") + self.set_parameter("AFS_MAX_GPS_LOSS", 1) + self.set_parameter("SIM_GPS_DISABLE", 1) + self.wait_statustext("AFS State: GPS_LOSS") + self.set_parameter("SIM_GPS_DISABLE", 0) + self.set_parameter("AFS_MAX_GPS_LOSS", 0) + self.set_parameter("AFS_TERMINATE", 0) + + self.send_cmd(mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, + 1, # terminate + 0, + 0, + 0, + 0, + 0, + 0, + ) + self.wait_statustext("Terminating due to GCS request") + + except Exception as e: + ex = e + self.mavproxy.send("fence disable\n") + if ex is not None: + raise ex + def tests(self): return [ ("PIDTuning",