diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index 00c9024eed..48d7c4dd4e 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -144,6 +144,7 @@ class AutoTestTracker(AutoTest): return { "ArmFeatures": "See https://github.com/ArduPilot/ardupilot/issues/10652", "Parameters": "reboot does not work", + "CPUFailsafe": " tracker doesn't have a CPU failsafe", } def tests(self): diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2c31d68e54..6e8bd15dfd 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4,6 +4,7 @@ from __future__ import print_function import math import os +import time from pymavlink import quaternion from pymavlink import mavutil @@ -1632,6 +1633,10 @@ class AutoTestPlane(AutoTest): self.wait_altitude(initial_alt-1, initial_alt+1) self.fly_home_land_and_disarm() + def CPUFailsafe(self): + '''In lockup Plane should copy RC inputs to RC outputs''' + self.plane_CPUFailsafe() + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index c728db3c3c..64e8fbfdef 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -893,6 +893,20 @@ class AutoTest(ABC): 0) self.detect_and_handle_reboot(old_bootcount, required_bootcount=required_bootcount) + def send_cmd_enter_cpu_lockup(self): + """Poke ArduPilot to stop the main loop from running""" + self.mav.mav.command_long_send(self.sysid_thismav(), + 1, + mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, + 1, # confirmation + 42, # lockup autopilot + 24, # no, really, we mean it + 71, # seriously, we're not kidding + 93, # we know exactly what we're + 0, + 0, + 0) + def reboot_sitl(self, required_bootcount=None): """Reboot SITL instance and wait for it to reconnect.""" self.progress("Rebooting SITL") @@ -1163,9 +1177,11 @@ class AutoTest(ABC): ################################################# # SIM UTILITIES ################################################# - def get_sim_time(self): + def get_sim_time(self, timeout=60): """Get SITL time in seconds.""" - m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True) + m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True, timeout=timeout) + if m is None: + raise AutoTestTimeoutException("Did not get SYSTEM_TIME message") return m.time_boot_ms * 1.0e-3 def get_sim_time_cached(self): @@ -1737,21 +1753,84 @@ class AutoTest(ABC): def wait_disarmed_default_wait_time(self): return 30 - def wait_disarmed(self, timeout=None): + def wait_disarmed(self, timeout=None, tstart=None): if timeout is None: timeout = self.wait_disarmed_default_wait_time() self.progress("Waiting for DISARM") - tstart = self.get_sim_time() + if tstart is None: + tstart = self.get_sim_time() while True: delta = self.get_sim_time_cached() - tstart if delta > timeout: raise AutoTestTimeoutException("Failed to DISARM") self.wait_heartbeat() + self.progress("Got heartbeat") if not self.mav.motors_armed(): self.progress("DISARMED after %.2f seconds (allowed=%.2f)" % (delta, timeout)) return True + def CPUFailsafe(self): + '''Most vehicles just disarm on failsafe''' + # customising the SITL commandline ensures the process will + # get stopped/started at the end of the test + self.customise_SITL_commandline([]) + self.wait_ready_to_arm() + self.arm_vehicle() + self.progress("Sending enter-cpu-lockup") + # when we're in CPU lockup we don't get SYSTEM_TIME messages, + # so get_sim_time breaks: + tstart = self.get_sim_time() + self.send_cmd_enter_cpu_lockup() + self.wait_disarmed(timeout=5, tstart=tstart) + + + def cpufailsafe_wait_servo_channel_value(self, channel, value, timeout=30): + '''we get restricted messages while doing cpufailsafe, this working then''' + start = time.time() + while True: + if time.time() - start > timeout: + raise NotAchievedException("Did not achieve value") + m = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True, timeout=1) + + if m is None: + raise NotAchievedException("Did not get SERVO_OUTPUT_RAW") + channel_field = "servo%u_raw" % channel + m_value = getattr(m, channel_field, None) + self.progress("Servo%u=%u want=%u" % (channel, m_value, value)) + if m_value == value: + break + + def plane_CPUFailsafe(self): + '''In lockup Plane should copy RC inputs to RC outputs''' + # customising the SITL commandline ensures the process will + # get stopped/started at the end of the test + self.customise_SITL_commandline([]) + self.wait_ready_to_arm() + self.arm_vehicle() + self.progress("Sending enter-cpu-lockup") + # when we're in CPU lockup we don't get SYSTEM_TIME messages, + # so get_sim_time breaks: + self.send_cmd_enter_cpu_lockup() + start_time = time.time() # not sim time! + while True: + want = "Initialising ArduPilot" + if time.time() - start_time > 30: + raise NotAchievedException("Did not get %s" % want) + # we still need to parse the incoming messages: + m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=0.1) + x = self.mav.messages.get("STATUSTEXT", None) + if x is not None and want in x.text: + break + # Different scaling for RC input and servo output means the + # servo output value isn't the rc input value: + self.progress("Setting RC to 1200") + self.send_set_rc(2, 1200) + self.progress("Waiting for servo of 1260") + self.cpufailsafe_wait_servo_channel_value(2, 1260) + self.send_set_rc(2, 1700) + self.cpufailsafe_wait_servo_channel_value(2, 1660) + def mavproxy_arm_vehicle(self): """Arm vehicle with mavlink arm message send from MAVProxy.""" self.progress("Arm motors with MavProxy") @@ -4954,6 +5033,10 @@ switch value''' "Test Config Error Loop", self.test_config_error_loop), + ("CPUFailsafe", + "Ensure we do something appropriate when the main loop stops", + self.CPUFailsafe), + ("Parameters", "Test Parameter Set/Get", self.test_parameters), diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index bf93fe8c29..84ea69a2bb 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -404,8 +404,13 @@ class AutoTestQuadPlane(AutoTest): return { "QAutoTune": "See https://github.com/ArduPilot/ardupilot/issues/10411", "FRSkyPassThrough": "Currently failing", + "CPUFailsafe": "servo channel values not scaled like ArduPlane", } + def CPUFailsafe(self): + '''In lockup Plane should copy RC inputs to RC outputs''' + self.plane_CPUFailsafe() + def tests(self): '''return list of all tests''' m = os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt")