Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tools: autotest: stop swallowing critical messages with get_sim_time()

This commit is contained in:
Peter Barker 2019-03-08 09:34:09 +11:00 committed by Peter Barker
parent 38e26757fd
commit d48eab893a
5 changed files with 58 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/autotest/apmrover2.py
View File

@ -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): def wait_distance_home_gt(self, distance, timeout=60):
home_distance = None home_distance = None
tstart = self.get_sim_time() 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) # m = self.mav.recv_match(type='VFR_HUD', blocking=True)
if self.distance_to_home() > distance: if self.distance_to_home() > distance:
return return

58
Tools/autotest/arducopter.py
View File

@ -213,7 +213,7 @@ class AutoTestCopter(AutoTest):
tstart = self.get_sim_time() tstart = self.get_sim_time()
self.progress("Holding loiter at %u meters for %u seconds" % self.progress("Holding loiter at %u meters for %u seconds" %
(start_altitude, holdtime)) (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) m = self.mav.recv_match(type='VFR_HUD', blocking=True)
pos = self.mav.location() pos = self.mav.location()
delta = self.get_distance(start, pos) delta = self.get_distance(start, pos)
@ -389,7 +389,7 @@ class AutoTestCopter(AutoTest):
self.change_mode("RTL") self.change_mode("RTL")
self.set_rc(3, 1500) self.set_rc(3, 1500)
tstart = self.get_sim_time() 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) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m alt = m.relative_alt / 1000.0 # mm -> m
home_distance = self.distance_to_home(use_cached_home=True) home_distance = self.distance_to_home(use_cached_home=True)
@ -498,7 +498,7 @@ class AutoTestCopter(AutoTest):
homeloc.longitude/1e7, homeloc.longitude/1e7,
homeloc.altitude/1e3, homeloc.altitude/1e3,
0) 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) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m alt = m.relative_alt / 1000.0 # mm -> m
pos = self.mav.location() pos = self.mav.location()
@ -576,7 +576,7 @@ class AutoTestCopter(AutoTest):
self.progress("Cutting motor 1 to 60% efficiency") self.progress("Cutting motor 1 to 60% efficiency")
self.set_parameter("SIM_ENGINE_MUL", 0.60) 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) m = self.mav.recv_match(type='VFR_HUD', blocking=True)
pos = self.mav.location() pos = self.mav.location()
delta = self.get_distance(start, pos) delta = self.get_distance(start, pos)
@ -687,7 +687,7 @@ class AutoTestCopter(AutoTest):
# start timer # start timer
tstart = self.get_sim_time() 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) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m alt = m.relative_alt / 1000.0 # mm -> m
home_distance = self.distance_to_home(use_cached_home=True) home_distance = self.distance_to_home(use_cached_home=True)
@ -831,7 +831,7 @@ class AutoTestCopter(AutoTest):
# record position for 30 seconds # record position for 30 seconds
while tnow < tstart + timeout: while tnow < tstart + timeout:
tnow = self.get_sim_time() tnow = self.get_sim_time_cached()
desired_glitch_num = int((tnow - tstart) * 2.2) desired_glitch_num = int((tnow - tstart) * 2.2)
if desired_glitch_num > glitch_current and glitch_current != -1: if desired_glitch_num > glitch_current and glitch_current != -1:
glitch_current = desired_glitch_num glitch_current = desired_glitch_num
@ -861,6 +861,8 @@ class AutoTestCopter(AutoTest):
if moved_distance > max_distance: if moved_distance > max_distance:
raise NotAchievedException( raise NotAchievedException(
"Moved over %u meters, Failed!" % max_distance) "Moved over %u meters, Failed!" % max_distance)
else:
self.drain_mav()
# disable gps glitch # disable gps glitch
if glitch_current != -1: if glitch_current != -1:
@ -965,7 +967,7 @@ class AutoTestCopter(AutoTest):
# wait for arrival back home # wait for arrival back home
self.mav.recv_match(type='VFR_HUD', blocking=True) self.mav.recv_match(type='VFR_HUD', blocking=True)
while self.distance_to_home(use_cached_home=True) > 5: 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( raise AutoTestTimeoutException(
("GPS Glitch testing failed" ("GPS Glitch testing failed"
"- exceeded timeout %u seconds" % timeout)) "- exceeded timeout %u seconds" % timeout))
@ -1006,7 +1008,7 @@ class AutoTestCopter(AutoTest):
self.progress("# Flying west for 8 seconds") self.progress("# Flying west for 8 seconds")
self.set_rc(2, 1300) self.set_rc(2, 1300)
tstart = self.get_sim_time() 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.mav.recv_match(type='VFR_HUD', blocking=True)
self.set_rc(2, 1500) self.set_rc(2, 1500)
@ -1020,7 +1022,7 @@ class AutoTestCopter(AutoTest):
self.progress("# Flying east for 8 seconds") self.progress("# Flying east for 8 seconds")
self.set_rc(2, 1700) self.set_rc(2, 1700)
tstart = self.get_sim_time() 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.mav.recv_match(type='VFR_HUD', blocking=True)
self.set_rc(2, 1500) self.set_rc(2, 1500)
@ -1057,7 +1059,7 @@ class AutoTestCopter(AutoTest):
% timeout) % timeout)
self.set_rc(1, 1300) self.set_rc(1, 1300)
tstart = self.get_sim_time() 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) self.mav.recv_match(type='VFR_HUD', blocking=True)
# stop rolling and yawing # stop rolling and yawing
@ -1101,7 +1103,7 @@ class AutoTestCopter(AutoTest):
tstart = self.get_sim_time() tstart = self.get_sim_time()
self.progress("Circle at %u meters for %u seconds" % self.progress("Circle at %u meters for %u seconds" %
(start_altitude, holdtime)) (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) m = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.progress("heading %d" % m.heading) self.progress("heading %d" % m.heading)
@ -1372,15 +1374,17 @@ class AutoTestCopter(AutoTest):
int_error_yaw = 0 int_error_yaw = 0
self.progress("Hovering for %u seconds" % hover_time) self.progress("Hovering for %u seconds" % hover_time)
failed = False failed = False
while self.get_sim_time() < tstart + holdtime + hover_time: while True:
ti = self.get_sim_time() now = self.get_sim_time_cached()
if now - tstart > holdtime + hover_time:
break
servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW',
blocking=True) blocking=True)
hud = self.mav.recv_match(type='VFR_HUD', blocking=True) hud = self.mav.recv_match(type='VFR_HUD', blocking=True)
attitude = self.mav.recv_match(type='ATTITUDE', 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%%)" % self.progress("Killing motor %u (%u%%)" %
(fail_servo+1, fail_mul)) (fail_servo+1, fail_mul))
self.set_parameter("SIM_ENGINE_FAIL", fail_servo) self.set_parameter("SIM_ENGINE_FAIL", fail_servo)
@ -1388,8 +1392,7 @@ class AutoTestCopter(AutoTest):
failed = True failed = True
if failed: if failed:
self.progress("Hold Time: %f/%f" % self.progress("Hold Time: %f/%f" % (now-tstart, holdtime))
(self.get_sim_time()-tstart, holdtime))
servo_pwm = [servo.servo1_raw, servo_pwm = [servo.servo1_raw,
servo.servo2_raw, servo.servo2_raw,
@ -1424,7 +1427,7 @@ class AutoTestCopter(AutoTest):
self.progress("Yaw=%f (delta=%f) (deg)" % self.progress("Yaw=%f (delta=%f) (deg)" %
(attitude.yaw, yaw_delta)) (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_alt += abs(alt_delta/dt)
int_error_yaw_rate += abs(yawrate_delta/dt) int_error_yaw_rate += abs(yawrate_delta/dt)
int_error_yaw += abs(yaw_delta/dt) int_error_yaw += abs(yaw_delta/dt)
@ -1483,7 +1486,7 @@ class AutoTestCopter(AutoTest):
if gpi.lat != 0: if gpi.lat != 0:
break break
if self.get_sim_time() - tstart > 10: if self.get_sim_time_cached() - tstart > 10:
raise AutoTestTimeoutException("Did not get non-zero lat") raise AutoTestTimeoutException("Did not get non-zero lat")
self.takeoff() self.takeoff()
@ -1499,7 +1502,7 @@ class AutoTestCopter(AutoTest):
if vicon_pos.x > 40: if vicon_pos.x > 40:
break break
if self.get_sim_time() - tstart > 100: if self.get_sim_time_cached() - tstart > 100:
raise AutoTestTimeoutException("Vicon showed no movement") raise AutoTestTimeoutException("Vicon showed no movement")
# recenter controls: # recenter controls:
@ -1509,7 +1512,7 @@ class AutoTestCopter(AutoTest):
self.set_rc(3, 1500) self.set_rc(3, 1500)
tstart = self.get_sim_time() tstart = self.get_sim_time()
while True: while True:
if self.get_sim_time() - tstart > 200: if self.get_sim_time_cached() - tstart > 200:
raise NotAchievedException("Did not disarm") raise NotAchievedException("Did not disarm")
self.mav.recv_match(type='GLOBAL_POSITION_INT', self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True) blocking=True)
@ -1970,7 +1973,7 @@ class AutoTestCopter(AutoTest):
count_stop = -1 count_stop = -1
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.armed(): # we RTL at end of mission while self.armed(): # we RTL at end of mission
now = self.get_sim_time() now = self.get_sim_time_cached()
if now - tstart > 120: if now - tstart > 120:
raise AutoTestTimeoutException("Did not disarm as expected") raise AutoTestTimeoutException("Did not disarm as expected")
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
@ -2145,7 +2148,7 @@ class AutoTestCopter(AutoTest):
tstart = self.get_sim_time() tstart = self.get_sim_time()
while True: while True:
if self.get_sim_time() - tstart > 200: if self.get_sim_time_cached() - tstart > 200:
raise NotAchievedException("Did not move far enough") raise NotAchievedException("Did not move far enough")
# send a position-control command # send a position-control command
self.mav.mav.set_position_target_global_int_send( self.mav.mav.set_position_target_global_int_send(
@ -2180,7 +2183,7 @@ class AutoTestCopter(AutoTest):
tstart = self.get_sim_time() tstart = self.get_sim_time()
while True: while True:
if self.get_sim_time() - tstart > timeout: if self.get_sim_time_cached() - tstart > timeout:
raise NotAchievedException("Did not start to move") raise NotAchievedException("Did not start to move")
# send a position-control command # send a position-control command
self.mav.mav.set_position_target_local_ned_send( 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)) # print('r=%f p=%f y=%f' % (m.roll, m.pitch, m.yaw))
return vector - x 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) dest = rotmat.Vector3(x, y, z)
tstart = self.get_sim_time()
while True: 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', m_pos = self.mav.recv_match(type='LOCAL_POSITION_NED',
blocking=True) blocking=True)
pos = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z) pos = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z)
@ -2259,7 +2265,7 @@ class AutoTestCopter(AutoTest):
) )
tstart = self.get_sim_time() 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', m_pos = self.mav.recv_match(type='LOCAL_POSITION_NED',
blocking=True) blocking=True)
pos = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z) 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): def test_mount_pitch(self, despitch, despitch_tolerance, timeout=5):
tstart = self.get_sim_time() tstart = self.get_sim_time()
while True: while True:
if self.get_sim_time() - tstart > timeout: if self.get_sim_time_cached() - tstart > timeout:
raise NotAchievedException("Mount pitch not achieved") raise NotAchievedException("Mount pitch not achieved")
m = self.mav.recv_match(type='MOUNT_STATUS', m = self.mav.recv_match(type='MOUNT_STATUS',

4
Tools/autotest/arduplane.py
View File

@ -239,7 +239,7 @@ class AutoTestPlane(AutoTest):
self.set_rc(1, 1500) self.set_rc(1, 1500)
self.set_rc(2, 1500) self.set_rc(2, 1500)
self.set_rc(4, 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) m = self.mav.recv_match(type='ATTITUDE', blocking=True)
roll = math.degrees(m.roll) roll = math.degrees(m.roll)
pitch = math.degrees(m.pitch) pitch = math.degrees(m.pitch)
@ -674,7 +674,7 @@ class AutoTestPlane(AutoTest):
raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!") raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!")
self.set_rc(12, 2000) self.set_rc(12, 2000)
tstart = self.get_sim_time() 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) x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None: if x is not None:
break break

2
Tools/autotest/ardusub.py
View File

@ -180,7 +180,7 @@ class AutoTestSub(AutoTest):
tstart = self.get_sim_time() tstart = self.get_sim_time()
while True: while True:
if self.get_sim_time() - tstart > 200: if self.get_sim_time_cached() - tstart > 200:
raise NotAchievedException("Did not move far enough") raise NotAchievedException("Did not move far enough")
# send a position-control command # send a position-control command
self.mav.mav.set_position_target_global_int_send( self.mav.mav.set_position_target_global_int_send(

46
Tools/autotest/common.py
View File

@ -588,7 +588,7 @@ class AutoTest(ABC):
path = os.path.join(self.mission_directory(), filename) path = os.path.join(self.mission_directory(), filename)
tstart = self.get_sim_time_cached() tstart = self.get_sim_time_cached()
while True: while True:
t2 = self.get_sim_time() t2 = self.get_sim_time_cached()
if t2 - tstart > 10: if t2 - tstart > 10:
raise AutoTestTimeoutException("Failed to do waypoint thing") raise AutoTestTimeoutException("Failed to do waypoint thing")
self.mavproxy.send('wp load %s\n' % path) self.mavproxy.send('wp load %s\n' % path)
@ -798,7 +798,7 @@ class AutoTest(ABC):
0, 0,
timeout=timeout) timeout=timeout)
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() - tstart < timeout: while self.get_sim_time_cached() - tstart < timeout:
self.wait_heartbeat() self.wait_heartbeat()
if self.mav.motors_armed(): if self.mav.motors_armed():
self.progress("Motors ARMED") self.progress("Motors ARMED")
@ -818,7 +818,7 @@ class AutoTest(ABC):
0, 0,
timeout=timeout) timeout=timeout)
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() - tstart < timeout: while self.get_sim_time_cached() - tstart < timeout:
self.wait_heartbeat() self.wait_heartbeat()
if not self.mav.motors_armed(): if not self.mav.motors_armed():
self.progress("Motors DISARMED") self.progress("Motors DISARMED")
@ -848,13 +848,12 @@ class AutoTest(ABC):
tstart = self.get_sim_time() tstart = self.get_sim_time()
while True: while True:
self.wait_heartbeat() self.wait_heartbeat()
tdelta = self.get_sim_time_cached() - tstart
if self.mav.motors_armed(): if self.mav.motors_armed():
arm_delay = self.get_sim_time() - tstart
self.progress("MOTORS ARMED OK WITH RADIO") self.progress("MOTORS ARMED OK WITH RADIO")
self.set_output_to_trim(self.get_rudder_channel()) 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 return True
tdelta = self.get_sim_time() - tstart
print("Not armed after %f seconds" % (tdelta)) print("Not armed after %f seconds" % (tdelta))
if tdelta > timeout: if tdelta > timeout:
break break
@ -867,10 +866,10 @@ class AutoTest(ABC):
self.progress("Disarm motors with radio") self.progress("Disarm motors with radio")
self.set_output_to_min(self.get_rudder_channel()) self.set_output_to_min(self.get_rudder_channel())
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() < tstart + timeout: while self.get_sim_time_cached() < tstart + timeout:
self.wait_heartbeat() self.wait_heartbeat()
if not self.mav.motors_armed(): 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.progress("MOTORS DISARMED OK WITH RADIO")
self.set_output_to_trim(self.get_rudder_channel()) self.set_output_to_trim(self.get_rudder_channel())
self.progress("Disarm in %ss" % disarm_delay) # TODO check disarming time 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.progress("Arm motors with switch %d" % switch_chan)
self.set_rc(switch_chan, 2000) self.set_rc(switch_chan, 2000)
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() - tstart < timeout: while self.get_sim_time_cached() - tstart < timeout:
self.wait_heartbeat() self.wait_heartbeat()
if self.mav.motors_armed(): if self.mav.motors_armed():
self.progress("MOTORS ARMED OK WITH SWITCH") self.progress("MOTORS ARMED OK WITH SWITCH")
@ -897,7 +896,7 @@ class AutoTest(ABC):
self.progress("Disarm motors with switch %d" % switch_chan) self.progress("Disarm motors with switch %d" % switch_chan)
self.set_rc(switch_chan, 1000) self.set_rc(switch_chan, 1000)
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() < tstart + timeout: while self.get_sim_time_cached() < tstart + timeout:
self.wait_heartbeat() self.wait_heartbeat()
if not self.mav.motors_armed(): if not self.mav.motors_armed():
self.progress("MOTORS DISARMED OK WITH SWITCH") self.progress("MOTORS DISARMED OK WITH SWITCH")
@ -912,10 +911,10 @@ class AutoTest(ABC):
disarm_delay = self.get_disarm_delay() disarm_delay = self.get_disarm_delay()
tstart = self.get_sim_time() tstart = self.get_sim_time()
timeout = disarm_delay * 2 timeout = disarm_delay * 2
while self.get_sim_time() < tstart + timeout: while self.get_sim_time_cached() < tstart + timeout:
self.wait_heartbeat() self.wait_heartbeat()
if not self.mav.motors_armed(): 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("MOTORS AUTODISARMED")
self.progress("Autodisarm in %ss, expect less than %ss" % (disarm_time, disarm_delay)) self.progress("Autodisarm in %ss, expect less than %ss" % (disarm_time, disarm_delay))
return 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.progress("mav.flightmode=%s Want=%s custom=%u" % (
self.mav.flightmode, mode, custom_num)) self.mav.flightmode, mode, custom_num))
if (timeout is not None and 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") raise WaitModeTimeout("Did not change mode")
self.mavproxy.send('mode %s\n' % mode) self.mavproxy.send('mode %s\n' % mode)
self.wait_heartbeat() self.wait_heartbeat()
@ -1138,7 +1137,7 @@ class AutoTest(ABC):
def do_get_autopilot_capabilities(self): def do_get_autopilot_capabilities(self):
tstart = self.get_sim_time() 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 # Cannot use run_cmd otherwise the respond is lost during the wait for ACK
self.mav.mav.command_long_send(1, self.mav.mav.command_long_send(1,
1, 1,
@ -1271,7 +1270,7 @@ class AutoTest(ABC):
0, # p7 0, # p7
) )
while True: while True:
if self.get_sim_time() - tstart > 200: if self.get_sim_time_cached() - tstart > 200:
raise NotAchievedException("Did not achieve heading") raise NotAchievedException("Did not achieve heading")
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='VFR_HUD', blocking=True)
self.progress("heading=%d want=%f" % (m.heading, heading)) self.progress("heading=%d want=%f" % (m.heading, heading))
@ -1336,7 +1335,7 @@ class AutoTest(ABC):
"""Wait for a given roll in degrees.""" """Wait for a given roll in degrees."""
tstart = self.get_sim_time() tstart = self.get_sim_time()
self.progress("Waiting for roll of %d at %s" % (roll, time.ctime())) 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) m = self.mav.recv_match(type='ATTITUDE', blocking=True)
p = math.degrees(m.pitch) p = math.degrees(m.pitch)
r = math.degrees(m.roll) r = math.degrees(m.roll)
@ -1350,7 +1349,7 @@ class AutoTest(ABC):
"""Wait for a given pitch in degrees.""" """Wait for a given pitch in degrees."""
tstart = self.get_sim_time() tstart = self.get_sim_time()
self.progress("Waiting for pitch of %.02f at %s" % (pitch, time.ctime())) 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) m = self.mav.recv_match(type='ATTITUDE', blocking=True)
p = math.degrees(m.pitch) p = math.degrees(m.pitch)
r = math.degrees(m.roll) r = math.degrees(m.roll)
@ -1385,7 +1384,7 @@ class AutoTest(ABC):
tstart = self.get_sim_time() tstart = self.get_sim_time()
start = self.mav.location() start = self.mav.location()
last_distance_message = 0 last_distance_message = 0
while self.get_sim_time() < tstart + timeout: while self.get_sim_time_cached() < tstart + timeout:
pos = self.mav.location() pos = self.mav.location()
delta = self.get_distance(start, pos) delta = self.get_distance(start, pos)
if self.get_sim_time_cached() - last_distance_message >= 1: 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.progress("mav.flightmode=%s Want=%s custom=%u" % (
self.mav.flightmode, mode, custom_num)) self.mav.flightmode, mode, custom_num))
if (timeout is not None and 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") raise WaitModeTimeout("Did not change mode")
self.wait_heartbeat()
# self.progress("heartbeat mode %s Want: %s" % ( # self.progress("heartbeat mode %s Want: %s" % (
# self.mav.flightmode, mode)) # self.mav.flightmode, mode))
self.progress("Got mode %s" % mode) self.progress("Got mode %s" % mode)
@ -1616,7 +1614,7 @@ class AutoTest(ABC):
"""Wait a specific STATUS_TEXT.""" """Wait a specific STATUS_TEXT."""
self.progress("Waiting for text : %s" % text.lower()) self.progress("Waiting for text : %s" % text.lower())
tstart = self.get_sim_time() 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: if the_function is not None:
the_function() the_function()
m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=0.1) 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): def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
tstart = self.get_sim_time() tstart = self.get_sim_time()
while True: while True:
if self.get_sim_time() - tstart > timeout: if self.get_sim_time_cached() - tstart > timeout:
break break
m = self.mav.recv_match(type='VFR_HUD', blocking=True) m = self.mav.recv_match(type='VFR_HUD', blocking=True)
if m.groundspeed > want+tolerance: if m.groundspeed > want+tolerance:
@ -1818,8 +1816,8 @@ class AutoTest(ABC):
self.progress("monitoring home to ensure it doesn't drift at all") self.progress("monitoring home to ensure it doesn't drift at all")
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.get_sim_time() - tstart < 10: while self.get_sim_time_cached() - tstart < 10:
home = self.poll_home_position() home = self.poll_home_position(quiet=True)
self.progress("home: %s" % str(home)) self.progress("home: %s" % str(home))
if (home.latitude != got_home_latitude or if (home.latitude != got_home_latitude or
home.longitude != got_home_longitude or home.longitude != got_home_longitude or