From d48eab893a3dd38929d9271de59e0145f1ae3a19 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Mar 2019 09:34:09 +1100 Subject: [PATCH] Tools: autotest: stop swallowing critical messages with get_sim_time() --- Tools/autotest/apmrover2.py | 2 +- Tools/autotest/arducopter.py | 58 ++++++++++++++++++++---------------- Tools/autotest/arduplane.py | 4 +-- Tools/autotest/ardusub.py | 2 +- Tools/autotest/common.py | 46 ++++++++++++++-------------- 5 files changed, 58 insertions(+), 54 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index 63148fb1a3..59a2c4e758 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -498,7 +498,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) def wait_distance_home_gt(self, distance, timeout=60): home_distance = None tstart = self.get_sim_time() - while self.get_sim_time() - tstart < timeout: + while self.get_sim_time_cached() - tstart < timeout: # m = self.mav.recv_match(type='VFR_HUD', blocking=True) if self.distance_to_home() > distance: return diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 4fefa93957..be42b803e7 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -213,7 +213,7 @@ class AutoTestCopter(AutoTest): tstart = self.get_sim_time() self.progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) - while self.get_sim_time() < tstart + holdtime: + while self.get_sim_time_cached() < tstart + holdtime: m = self.mav.recv_match(type='VFR_HUD', blocking=True) pos = self.mav.location() delta = self.get_distance(start, pos) @@ -389,7 +389,7 @@ class AutoTestCopter(AutoTest): self.change_mode("RTL") self.set_rc(3, 1500) tstart = self.get_sim_time() - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m home_distance = self.distance_to_home(use_cached_home=True) @@ -498,7 +498,7 @@ class AutoTestCopter(AutoTest): homeloc.longitude/1e7, homeloc.altitude/1e3, 0) - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m pos = self.mav.location() @@ -576,7 +576,7 @@ class AutoTestCopter(AutoTest): self.progress("Cutting motor 1 to 60% efficiency") self.set_parameter("SIM_ENGINE_MUL", 0.60) - while self.get_sim_time() < tstart + holdtime: + while self.get_sim_time_cached() < tstart + holdtime: m = self.mav.recv_match(type='VFR_HUD', blocking=True) pos = self.mav.location() delta = self.get_distance(start, pos) @@ -687,7 +687,7 @@ class AutoTestCopter(AutoTest): # start timer tstart = self.get_sim_time() - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) alt = m.relative_alt / 1000.0 # mm -> m home_distance = self.distance_to_home(use_cached_home=True) @@ -831,7 +831,7 @@ class AutoTestCopter(AutoTest): # record position for 30 seconds while tnow < tstart + timeout: - tnow = self.get_sim_time() + tnow = self.get_sim_time_cached() desired_glitch_num = int((tnow - tstart) * 2.2) if desired_glitch_num > glitch_current and glitch_current != -1: glitch_current = desired_glitch_num @@ -861,6 +861,8 @@ class AutoTestCopter(AutoTest): if moved_distance > max_distance: raise NotAchievedException( "Moved over %u meters, Failed!" % max_distance) + else: + self.drain_mav() # disable gps glitch if glitch_current != -1: @@ -965,7 +967,7 @@ class AutoTestCopter(AutoTest): # wait for arrival back home self.mav.recv_match(type='VFR_HUD', blocking=True) while self.distance_to_home(use_cached_home=True) > 5: - if self.get_sim_time() > (tstart + timeout): + if self.get_sim_time_cached() > (tstart + timeout): raise AutoTestTimeoutException( ("GPS Glitch testing failed" "- exceeded timeout %u seconds" % timeout)) @@ -1006,7 +1008,7 @@ class AutoTestCopter(AutoTest): self.progress("# Flying west for 8 seconds") self.set_rc(2, 1300) tstart = self.get_sim_time() - while self.get_sim_time() < (tstart + 8): + while self.get_sim_time_cached() < (tstart + 8): self.mav.recv_match(type='VFR_HUD', blocking=True) self.set_rc(2, 1500) @@ -1020,7 +1022,7 @@ class AutoTestCopter(AutoTest): self.progress("# Flying east for 8 seconds") self.set_rc(2, 1700) tstart = self.get_sim_time() - while self.get_sim_time() < (tstart + 8): + while self.get_sim_time_cached() < (tstart + 8): self.mav.recv_match(type='VFR_HUD', blocking=True) self.set_rc(2, 1500) @@ -1057,7 +1059,7 @@ class AutoTestCopter(AutoTest): % timeout) self.set_rc(1, 1300) tstart = self.get_sim_time() - while self.get_sim_time() < (tstart + timeout): + while self.get_sim_time_cached() < (tstart + timeout): self.mav.recv_match(type='VFR_HUD', blocking=True) # stop rolling and yawing @@ -1101,7 +1103,7 @@ class AutoTestCopter(AutoTest): tstart = self.get_sim_time() self.progress("Circle at %u meters for %u seconds" % (start_altitude, holdtime)) - while self.get_sim_time() < tstart + holdtime: + while self.get_sim_time_cached() < tstart + holdtime: m = self.mav.recv_match(type='VFR_HUD', blocking=True) self.progress("heading %d" % m.heading) @@ -1372,15 +1374,17 @@ class AutoTestCopter(AutoTest): int_error_yaw = 0 self.progress("Hovering for %u seconds" % hover_time) failed = False - while self.get_sim_time() < tstart + holdtime + hover_time: - ti = self.get_sim_time() + while True: + now = self.get_sim_time_cached() + if now - tstart > holdtime + hover_time: + break servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', blocking=True) hud = self.mav.recv_match(type='VFR_HUD', blocking=True) attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) - if not failed and self.get_sim_time() - tstart > hover_time: + if not failed and now - tstart > hover_time: self.progress("Killing motor %u (%u%%)" % (fail_servo+1, fail_mul)) self.set_parameter("SIM_ENGINE_FAIL", fail_servo) @@ -1388,8 +1392,7 @@ class AutoTestCopter(AutoTest): failed = True if failed: - self.progress("Hold Time: %f/%f" % - (self.get_sim_time()-tstart, holdtime)) + self.progress("Hold Time: %f/%f" % (now-tstart, holdtime)) servo_pwm = [servo.servo1_raw, servo.servo2_raw, @@ -1424,7 +1427,7 @@ class AutoTestCopter(AutoTest): self.progress("Yaw=%f (delta=%f) (deg)" % (attitude.yaw, yaw_delta)) - dt = self.get_sim_time() - ti + dt = self.get_sim_time() - now int_error_alt += abs(alt_delta/dt) int_error_yaw_rate += abs(yawrate_delta/dt) int_error_yaw += abs(yaw_delta/dt) @@ -1483,7 +1486,7 @@ class AutoTestCopter(AutoTest): if gpi.lat != 0: break - if self.get_sim_time() - tstart > 10: + if self.get_sim_time_cached() - tstart > 10: raise AutoTestTimeoutException("Did not get non-zero lat") self.takeoff() @@ -1499,7 +1502,7 @@ class AutoTestCopter(AutoTest): if vicon_pos.x > 40: break - if self.get_sim_time() - tstart > 100: + if self.get_sim_time_cached() - tstart > 100: raise AutoTestTimeoutException("Vicon showed no movement") # recenter controls: @@ -1509,7 +1512,7 @@ class AutoTestCopter(AutoTest): self.set_rc(3, 1500) tstart = self.get_sim_time() while True: - if self.get_sim_time() - tstart > 200: + if self.get_sim_time_cached() - tstart > 200: raise NotAchievedException("Did not disarm") self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) @@ -1970,7 +1973,7 @@ class AutoTestCopter(AutoTest): count_stop = -1 tstart = self.get_sim_time() while self.armed(): # we RTL at end of mission - now = self.get_sim_time() + now = self.get_sim_time_cached() if now - tstart > 120: raise AutoTestTimeoutException("Did not disarm as expected") m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) @@ -2145,7 +2148,7 @@ class AutoTestCopter(AutoTest): tstart = self.get_sim_time() while True: - if self.get_sim_time() - tstart > 200: + if self.get_sim_time_cached() - tstart > 200: raise NotAchievedException("Did not move far enough") # send a position-control command self.mav.mav.set_position_target_global_int_send( @@ -2180,7 +2183,7 @@ class AutoTestCopter(AutoTest): tstart = self.get_sim_time() while True: - if self.get_sim_time() - tstart > timeout: + if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Did not start to move") # send a position-control command self.mav.mav.set_position_target_local_ned_send( @@ -2230,9 +2233,12 @@ class AutoTestCopter(AutoTest): # print('r=%f p=%f y=%f' % (m.roll, m.pitch, m.yaw)) return vector - x - def loiter_to_ne(self, x, y, z): + def loiter_to_ne(self, x, y, z, timeout=30): dest = rotmat.Vector3(x, y, z) + tstart = self.get_sim_time() while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Did not loiter to ne!") m_pos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) pos = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z) @@ -2259,7 +2265,7 @@ class AutoTestCopter(AutoTest): ) tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 10: + while self.get_sim_time_cached() - tstart < 10: m_pos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) pos = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z) @@ -2406,7 +2412,7 @@ class AutoTestCopter(AutoTest): def test_mount_pitch(self, despitch, despitch_tolerance, timeout=5): tstart = self.get_sim_time() while True: - if self.get_sim_time() - tstart > timeout: + if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Mount pitch not achieved") m = self.mav.recv_match(type='MOUNT_STATUS', diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index c305592a62..8170a15a90 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -239,7 +239,7 @@ class AutoTestPlane(AutoTest): self.set_rc(1, 1500) self.set_rc(2, 1500) self.set_rc(4, 1500) - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: m = self.mav.recv_match(type='ATTITUDE', blocking=True) roll = math.degrees(m.roll) pitch = math.degrees(m.pitch) @@ -674,7 +674,7 @@ class AutoTestPlane(AutoTest): raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!") self.set_rc(12, 2000) tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 10: + while self.get_sim_time_cached() - tstart < 10: x = self.mav.messages.get("CAMERA_FEEDBACK", None) if x is not None: break diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index c048fab1f8..133f5c1b6a 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -180,7 +180,7 @@ class AutoTestSub(AutoTest): tstart = self.get_sim_time() while True: - if self.get_sim_time() - tstart > 200: + if self.get_sim_time_cached() - tstart > 200: raise NotAchievedException("Did not move far enough") # send a position-control command self.mav.mav.set_position_target_global_int_send( diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 7c241fd15a..4c657dda95 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -588,7 +588,7 @@ class AutoTest(ABC): path = os.path.join(self.mission_directory(), filename) tstart = self.get_sim_time_cached() while True: - t2 = self.get_sim_time() + t2 = self.get_sim_time_cached() if t2 - tstart > 10: raise AutoTestTimeoutException("Failed to do waypoint thing") self.mavproxy.send('wp load %s\n' % path) @@ -798,7 +798,7 @@ class AutoTest(ABC): 0, timeout=timeout) tstart = self.get_sim_time() - while self.get_sim_time() - tstart < timeout: + while self.get_sim_time_cached() - tstart < timeout: self.wait_heartbeat() if self.mav.motors_armed(): self.progress("Motors ARMED") @@ -818,7 +818,7 @@ class AutoTest(ABC): 0, timeout=timeout) tstart = self.get_sim_time() - while self.get_sim_time() - tstart < timeout: + while self.get_sim_time_cached() - tstart < timeout: self.wait_heartbeat() if not self.mav.motors_armed(): self.progress("Motors DISARMED") @@ -848,13 +848,12 @@ class AutoTest(ABC): tstart = self.get_sim_time() while True: self.wait_heartbeat() + tdelta = self.get_sim_time_cached() - tstart if self.mav.motors_armed(): - arm_delay = self.get_sim_time() - tstart self.progress("MOTORS ARMED OK WITH RADIO") self.set_output_to_trim(self.get_rudder_channel()) - self.progress("Arm in %ss" % arm_delay) # TODO check arming time + self.progress("Arm in %ss" % tdelta) # TODO check arming time return True - tdelta = self.get_sim_time() - tstart print("Not armed after %f seconds" % (tdelta)) if tdelta > timeout: break @@ -867,10 +866,10 @@ class AutoTest(ABC): self.progress("Disarm motors with radio") self.set_output_to_min(self.get_rudder_channel()) tstart = self.get_sim_time() - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: self.wait_heartbeat() if not self.mav.motors_armed(): - disarm_delay = self.get_sim_time() - tstart + disarm_delay = self.get_sim_time_cached() - tstart self.progress("MOTORS DISARMED OK WITH RADIO") self.set_output_to_trim(self.get_rudder_channel()) self.progress("Disarm in %ss" % disarm_delay) # TODO check disarming time @@ -884,7 +883,7 @@ class AutoTest(ABC): self.progress("Arm motors with switch %d" % switch_chan) self.set_rc(switch_chan, 2000) tstart = self.get_sim_time() - while self.get_sim_time() - tstart < timeout: + while self.get_sim_time_cached() - tstart < timeout: self.wait_heartbeat() if self.mav.motors_armed(): self.progress("MOTORS ARMED OK WITH SWITCH") @@ -897,7 +896,7 @@ class AutoTest(ABC): self.progress("Disarm motors with switch %d" % switch_chan) self.set_rc(switch_chan, 1000) tstart = self.get_sim_time() - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: self.wait_heartbeat() if not self.mav.motors_armed(): self.progress("MOTORS DISARMED OK WITH SWITCH") @@ -912,10 +911,10 @@ class AutoTest(ABC): disarm_delay = self.get_disarm_delay() tstart = self.get_sim_time() timeout = disarm_delay * 2 - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: self.wait_heartbeat() if not self.mav.motors_armed(): - disarm_time = self.get_sim_time() - tstart + disarm_time = self.get_sim_time_cached() - tstart self.progress("MOTORS AUTODISARMED") self.progress("Autodisarm in %ss, expect less than %ss" % (disarm_time, disarm_delay)) return disarm_time <= disarm_delay @@ -1128,7 +1127,7 @@ class AutoTest(ABC): self.progress("mav.flightmode=%s Want=%s custom=%u" % ( self.mav.flightmode, mode, custom_num)) if (timeout is not None and - self.get_sim_time() > tstart + timeout): + self.get_sim_time_cached() > tstart + timeout): raise WaitModeTimeout("Did not change mode") self.mavproxy.send('mode %s\n' % mode) self.wait_heartbeat() @@ -1138,7 +1137,7 @@ class AutoTest(ABC): def do_get_autopilot_capabilities(self): tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 10: + 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, @@ -1271,7 +1270,7 @@ class AutoTest(ABC): 0, # p7 ) while True: - if self.get_sim_time() - tstart > 200: + if self.get_sim_time_cached() - tstart > 200: raise NotAchievedException("Did not achieve heading") m = self.mav.recv_match(type='VFR_HUD', blocking=True) self.progress("heading=%d want=%f" % (m.heading, heading)) @@ -1336,7 +1335,7 @@ class AutoTest(ABC): """Wait for a given roll in degrees.""" tstart = self.get_sim_time() self.progress("Waiting for roll of %d at %s" % (roll, time.ctime())) - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: m = self.mav.recv_match(type='ATTITUDE', blocking=True) p = math.degrees(m.pitch) r = math.degrees(m.roll) @@ -1350,7 +1349,7 @@ class AutoTest(ABC): """Wait for a given pitch in degrees.""" tstart = self.get_sim_time() self.progress("Waiting for pitch of %.02f at %s" % (pitch, time.ctime())) - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: m = self.mav.recv_match(type='ATTITUDE', blocking=True) p = math.degrees(m.pitch) r = math.degrees(m.roll) @@ -1385,7 +1384,7 @@ class AutoTest(ABC): tstart = self.get_sim_time() start = self.mav.location() last_distance_message = 0 - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: pos = self.mav.location() delta = self.get_distance(start, pos) if self.get_sim_time_cached() - last_distance_message >= 1: @@ -1551,9 +1550,8 @@ class AutoTest(ABC): self.progress("mav.flightmode=%s Want=%s custom=%u" % ( self.mav.flightmode, mode, custom_num)) if (timeout is not None and - self.get_sim_time() > tstart + timeout): + self.get_sim_time_cached() > tstart + timeout): raise WaitModeTimeout("Did not change mode") - self.wait_heartbeat() # self.progress("heartbeat mode %s Want: %s" % ( # self.mav.flightmode, mode)) self.progress("Got mode %s" % mode) @@ -1616,7 +1614,7 @@ class AutoTest(ABC): """Wait a specific STATUS_TEXT.""" self.progress("Waiting for text : %s" % text.lower()) tstart = self.get_sim_time() - while self.get_sim_time() < tstart + timeout: + while self.get_sim_time_cached() < tstart + timeout: if the_function is not None: the_function() m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=0.1) @@ -1767,7 +1765,7 @@ class AutoTest(ABC): def monitor_groundspeed(self, want, tolerance=0.5, timeout=5): tstart = self.get_sim_time() while True: - if self.get_sim_time() - tstart > timeout: + if self.get_sim_time_cached() - tstart > timeout: break m = self.mav.recv_match(type='VFR_HUD', blocking=True) if m.groundspeed > want+tolerance: @@ -1818,8 +1816,8 @@ class AutoTest(ABC): self.progress("monitoring home to ensure it doesn't drift at all") tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 10: - home = self.poll_home_position() + while self.get_sim_time_cached() - tstart < 10: + home = self.poll_home_position(quiet=True) self.progress("home: %s" % str(home)) if (home.latitude != got_home_latitude or home.longitude != got_home_longitude or