From a320a54f66827163f65ed631aabf2c73e928fdab Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 14 Dec 2018 09:17:02 +1100 Subject: [PATCH] Tools: autotest: new wait_heartbeat raises exception if none received --- Tools/autotest/apmrover2.py | 14 ++++++------ Tools/autotest/arducopter.py | 20 +++++++++--------- Tools/autotest/arduplane.py | 12 +++++------ Tools/autotest/ardusub.py | 2 +- Tools/autotest/common.py | 41 +++++++++++++++++++++--------------- 5 files changed, 48 insertions(+), 41 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index 427e083fe6..9374abb6b8 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -99,13 +99,13 @@ class AutoTestRover(AutoTest): # def reset_and_arm(self): # """Reset RC, set to MANUAL and arm.""" - # self.mav.wait_heartbeat() + # self.wait_heartbeat() # # ensure all sticks in the middle # self.set_rc_default() # self.mavproxy.send('switch 1\n') - # self.mav.wait_heartbeat() + # self.wait_heartbeat() # self.disarm_vehicle() - # self.mav.wait_heartbeat() + # self.wait_heartbeat() # self.arm_vehicle() # @@ -120,14 +120,14 @@ class AutoTestRover(AutoTest): # self.progress("Test mission %s" % filename) # num_wp = self.load_mission(filename) # self.mavproxy.send('wp set 1\n') - # self.mav.wait_heartbeat() + # self.wait_heartbeat() # self.mavproxy.send('switch 4\n') # auto mode # self.wait_mode('AUTO') # ret = self.wait_waypoint(0, num_wp-1, max_dist=5, timeout=500) # # if ret: # self.mavproxy.expect("Mission Complete") - # self.mav.wait_heartbeat() + # self.wait_heartbeat() # self.wait_mode('HOLD') # self.progress("test: MISSION COMPLETE: passed=%s" % ret) # return ret @@ -272,7 +272,7 @@ class AutoTestRover(AutoTest): # self.mavproxy.send('rc 3 1500\n') # self.mavproxy.expect('APM: Failsafe ended') # self.mavproxy.send('switch 2\n') # manual mode - # self.mav.wait_heartbeat() + # self.wait_heartbeat() # self.wait_mode('MANUAL') # # if success: @@ -812,7 +812,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) - self.mav.wait_heartbeat() + self.wait_heartbeat() self.progress("Setting up RC parameters") self.set_rc_default() self.set_rc(8, 1800) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 65af43f7d2..d77609df01 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1295,7 +1295,7 @@ class AutoTestCopter(AutoTest): self.progress("Waiting for location") self.mavproxy.send('switch 6\n') # stabilize mode - self.mav.wait_heartbeat() + self.wait_heartbeat() self.wait_mode('STABILIZE') self.progress("Waiting reading for arm") self.wait_ready_to_arm() @@ -1382,7 +1382,7 @@ class AutoTestCopter(AutoTest): self.load_mission("copter_nav_delay.txt") self.mavproxy.send('mode loiter\n') - self.mav.wait_heartbeat() + self.wait_heartbeat() self.wait_mode('LOITER') self.progress("Waiting reading for arm") self.wait_ready_to_arm() @@ -1635,7 +1635,7 @@ class AutoTestCopter(AutoTest): self.progress("Starting mission") self.mavproxy.send('mode loiter\n') # stabilize mode - self.mav.wait_heartbeat() + self.wait_heartbeat() self.wait_mode('LOITER') self.progress("Waiting reading for arm") self.wait_ready_to_arm() @@ -1693,7 +1693,7 @@ class AutoTestCopter(AutoTest): self.progress("Starting mission") self.mavproxy.send('mode loiter\n') # stabilize mode - self.mav.wait_heartbeat() + self.wait_heartbeat() self.wait_mode('LOITER') self.progress("Waiting reading for arm") self.wait_ready_to_arm() @@ -1925,7 +1925,7 @@ class AutoTestCopter(AutoTest): self.mav.location() self.set_rc(3, 1000) self.mavproxy.send('switch 6\n') # stabilize mode - self.mav.wait_heartbeat() + self.wait_heartbeat() self.wait_mode('STABILIZE') self.progress("Waiting reading for arm") self.wait_ready_to_arm() @@ -1933,7 +1933,7 @@ class AutoTestCopter(AutoTest): self.arm_vehicle() self.mavproxy.send('switch 4\n') # auto mode - self.mav.wait_heartbeat() + self.wait_heartbeat() self.wait_mode('AUTO') self.set_rc(3, 1500) @@ -2361,7 +2361,7 @@ class AutoTestCopter(AutoTest): self.mav.location() self.set_rc(3, 1000) self.mavproxy.send('switch 6\n') # stabilize mode - self.mav.wait_heartbeat() + self.wait_heartbeat() self.wait_mode('STABILIZE') self.progress("Waiting reading for arm") self.wait_ready_to_arm() @@ -2414,7 +2414,7 @@ class AutoTestCopter(AutoTest): try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) - self.mav.wait_heartbeat() + self.wait_heartbeat() self.progress("Setting up RC parameters") self.set_rc_default() self.set_rc(3, 1000) @@ -2443,7 +2443,7 @@ class AutoTestCopter(AutoTest): self.homeloc = self.mav.location() self.progress("Home location: %s" % self.homeloc) self.mavproxy.send('switch 6\n') # stabilize mode - self.mav.wait_heartbeat() + self.wait_heartbeat() self.wait_mode('STABILIZE') self.set_rc(3, 1000) self.progress("Waiting reading for arm") @@ -2661,7 +2661,7 @@ class AutoTestCopter(AutoTest): self.fail_list = [] try: - self.mav.wait_heartbeat() + self.wait_heartbeat() self.set_rc_default() self.set_rc(3, 1000) self.homeloc = self.mav.location() diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 53524e3031..3df46df5d8 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -629,14 +629,14 @@ class AutoTestPlane(AutoTest): if off: raise PreconditionFailedException("SIM_MASK_PIN off") self.set_rc(12, 2000) - self.mav.wait_heartbeat() - self.mav.wait_heartbeat() + self.wait_heartbeat() + self.wait_heartbeat() on = self.get_parameter("SIM_PIN_MASK") if not on: raise NotAchievedException("SIM_PIN_MASK doesn't reflect ON") self.set_rc(12, 1000) - self.mav.wait_heartbeat() - self.mav.wait_heartbeat() + self.wait_heartbeat() + self.wait_heartbeat() off = self.get_parameter("SIM_PIN_MASK") if off: raise NotAchievedException("SIM_PIN_MASK doesn't reflect OFF") @@ -652,7 +652,7 @@ class AutoTestPlane(AutoTest): x = self.mav.messages.get("CAMERA_FEEDBACK", None) if x is not None: break - self.mav.wait_heartbeat() + self.wait_heartbeat() self.set_rc(12, 1000) if x is None: raise NotAchievedException("No CAMERA_FEEDBACK message received") @@ -687,7 +687,7 @@ class AutoTestPlane(AutoTest): try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) - self.mav.wait_heartbeat() + self.wait_heartbeat() self.progress("Setting up RC parameters") self.set_rc_default() self.set_rc(3, 1000) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 0eb862c003..c0f60b0177 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -172,7 +172,7 @@ class AutoTestSub(AutoTest): try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) - self.mav.wait_heartbeat() + self.wait_heartbeat() self.set_parameter("FS_GCS_ENABLE", 0) self.progress("Waiting for GPS fix") self.mav.wait_gps_fix() diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index fa67b214d3..a4fc640804 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -373,16 +373,16 @@ class AutoTest(ABC): def log_download(self, filename, timeout=360, upload_logs=False): """Download latest log.""" - self.mav.wait_heartbeat() + self.wait_heartbeat() self.mavproxy.send("log list\n") self.mavproxy.expect("numLogs") - self.mav.wait_heartbeat() - self.mav.wait_heartbeat() + self.wait_heartbeat() + self.wait_heartbeat() self.mavproxy.send("set shownoise 0\n") self.mavproxy.send("log download latest %s\n" % filename) self.mavproxy.expect("Finished downloading", timeout=timeout) - self.mav.wait_heartbeat() - self.mav.wait_heartbeat() + self.wait_heartbeat() + self.wait_heartbeat() if upload_logs and not os.getenv("AUTOTEST_NO_UPLOAD"): # optionally upload logs to server so we can see travis failure logs import datetime @@ -636,7 +636,7 @@ class AutoTest(ABC): ) tstart = self.get_sim_time() while self.get_sim_time() - tstart < timeout: - self.mav.wait_heartbeat() + self.wait_heartbeat() if self.mav.motors_armed(): self.progress("Motors ARMED") return True @@ -656,7 +656,7 @@ class AutoTest(ABC): ) tstart = self.get_sim_time() while self.get_sim_time() - tstart < timeout: - self.mav.wait_heartbeat() + self.wait_heartbeat() if not self.mav.motors_armed(): self.progress("Motors DISARMED") return True @@ -684,7 +684,7 @@ class AutoTest(ABC): self.set_output_to_max(self.get_rudder_channel()) tstart = self.get_sim_time() while True: - self.mav.wait_heartbeat() + self.wait_heartbeat() if self.mav.motors_armed(): arm_delay = self.get_sim_time() - tstart self.progress("MOTORS ARMED OK WITH RADIO") @@ -705,7 +705,7 @@ class AutoTest(ABC): self.set_output_to_min(self.get_rudder_channel()) tstart = self.get_sim_time() while self.get_sim_time() < tstart + timeout: - self.mav.wait_heartbeat() + self.wait_heartbeat() if not self.mav.motors_armed(): disarm_delay = self.get_sim_time() - tstart self.progress("MOTORS DISARMED OK WITH RADIO") @@ -722,7 +722,7 @@ class AutoTest(ABC): self.set_rc(switch_chan, 2000) tstart = self.get_sim_time() while self.get_sim_time() - tstart < timeout: - self.mav.wait_heartbeat() + self.wait_heartbeat() if self.mav.motors_armed(): self.progress("MOTORS ARMED OK WITH SWITCH") return True @@ -735,7 +735,7 @@ class AutoTest(ABC): self.set_rc(switch_chan, 1000) tstart = self.get_sim_time() while self.get_sim_time() < tstart + timeout: - self.mav.wait_heartbeat() + self.wait_heartbeat() if not self.mav.motors_armed(): self.progress("MOTORS DISARMED OK WITH SWITCH") return True @@ -751,7 +751,7 @@ class AutoTest(ABC): tstart = self.get_sim_time() timeout = 15 while self.get_sim_time() < tstart + timeout: - self.mav.wait_heartbeat() + self.wait_heartbeat() if not self.mav.motors_armed(): disarm_delay = self.get_sim_time() - tstart self.progress("MOTORS AUTODISARMED") @@ -1282,12 +1282,12 @@ class AutoTest(ABC): self.get_mode_from_mode_mapping(mode) self.progress("Waiting for mode %s" % mode) tstart = self.get_sim_time() - self.mav.wait_heartbeat() + self.wait_heartbeat() while self.mav.flightmode != mode: if (timeout is not None and self.get_sim_time() > tstart + timeout): raise WaitModeTimeout("Did not change mode") - self.mav.wait_heartbeat() + self.wait_heartbeat() # self.progress("heartbeat mode %s Want: %s" % ( # self.mav.flightmode, mode)) self.progress("Got mode %s" % mode) @@ -1298,6 +1298,13 @@ class AutoTest(ABC): return self.wait_ekf_happy(timeout=timeout, require_absolute=require_absolute) + def wait_heartbeat(self, *args, **x): + '''as opposed to mav.wait_heartbeat, raises an exception on timeout''' + self.drain_mav() + m = self.mav.wait_heartbeat(*args, **x) + if m is None: + raise AutoTestTimeoutException("Did not receive heartbeat") + def wait_ekf_happy(self, timeout=30, require_absolute=True): """Wait for EKF to be happy""" @@ -1499,14 +1506,14 @@ class AutoTest(ABC): if self.disarm_motors_with_rc_input(): raise NotAchievedException("Failed to NOT DISARM") self.disarm_vehicle() - self.mav.wait_heartbeat() + self.wait_heartbeat() self.start_test("Test disarming failure with ARMING_RUDDER=1") self.set_parameter("ARMING_RUDDER", 1) self.arm_vehicle() if self.disarm_motors_with_rc_input(): raise NotAchievedException("Failed to NOT ARM") self.disarm_vehicle() - self.mav.wait_heartbeat() + self.wait_heartbeat() self.set_parameter("ARMING_RUDDER", 2) if self.mav.mav_type in [mavutil.mavlink.MAV_TYPE_QUADROTOR, @@ -1522,7 +1529,7 @@ class AutoTest(ABC): if self.arm_motors_with_switch(arming_switch): raise NotAchievedException("Failed to NOT ARM") self.disarm_vehicle() - self.mav.wait_heartbeat() + self.wait_heartbeat() self.set_rc(arming_switch, 1000) self.set_rc(interlock_channel, 1000) if self.mav.mav_type is mavutil.mavlink.MAV_TYPE_HELICOPTER: