aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

autotest: use simulation time for all time delays

Browse Source
This commit is contained in:
Andrew Tridgell 2015-04-13 09:28:24 +10:00
parent d2a188c55d
commit 7f89f73ad1
4 changed files with 35 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/autotest/ArduPlane.parm
View File

@ -1,4 +1,3 @@
ARMING_REQUIRE 0
AHRS_EKF_USE 1
ALT_CTRL_ALG 2
BATT_MONITOR 4

4
Tools/autotest/arducopter.py
View File

@ -978,7 +978,7 @@ def fly_ArduCopter(viewerip=None, map=False):
homeloc = mav.location()
# wait 10sec to allow EKF to settle
wait_sim_seconds(mav, 10)
wait_seconds(mav, 10)
# Arm
print("# Arm motors")
@ -1343,7 +1343,7 @@ def fly_CopterAVC(viewerip=None, map=False):
homeloc = mav.location()
# wait 10sec to allow EKF to settle
wait_sim_seconds(mav, 10)
wait_seconds(mav, 10)
# Arm
print("# Arm motors")

11
Tools/autotest/arduplane.py
View File

@ -16,6 +16,13 @@ homeloc = None
def takeoff(mavproxy, mav):
'''takeoff get to 30m altitude'''
# wait for EKF to settle
wait_seconds(mav, 15)
mavproxy.send('arm throttle\n')
mavproxy.expect('ARMED')
mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA')
@ -150,12 +157,12 @@ def fly_CIRCLE(mavproxy, mav, num_circles=1):
def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
'''wait for level flight'''
tstart = time.time()
tstart = get_sim_time(mav)
print("Waiting for level flight")
mavproxy.send('rc 1 1500\n')
mavproxy.send('rc 2 1500\n')
mavproxy.send('rc 4 1500\n')
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
m = mav.recv_match(type='ATTITUDE', blocking=True)
roll = math.degrees(m.roll)
pitch = math.degrees(m.pitch)

56
Tools/autotest/common.py
View File

@ -49,31 +49,23 @@ def get_bearing(loc1, loc2):
bearing += 360.00
return bearing;
def wait_seconds(seconds_to_wait):
tstart = time.time();
def wait_seconds(mav, seconds_to_wait):
tstart = get_sim_time(mav)
tnow = tstart
while tstart + seconds_to_wait > tnow:
tnow = time.time()
def wait_sim_seconds(mav, seconds_to_wait):
m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
tstart = m.time_boot_ms / 1000;
tnow = tstart
while tstart + seconds_to_wait > tnow:
m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
tnow = m.time_boot_ms / 1000;
tnow = get_sim_time(mav)
def get_sim_time(mav):
m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
return m.time_boot_ms / 1000;
return m.time_boot_ms * 1.0e-3
def wait_altitude(mav, alt_min, alt_max, timeout=30):
climb_rate = 0
previous_alt = 0
'''wait for a given altitude range'''
tstart = time.time()
tstart = get_sim_time(mav)
print("Waiting for altitude between %u and %u" % (alt_min, alt_max))
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True)
climb_rate = m.alt - previous_alt
previous_alt = m.alt
@ -86,9 +78,9 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
def wait_groundspeed(mav, gs_min, gs_max, timeout=30):
'''wait for a given ground speed range'''
tstart = time.time()
tstart = get_sim_time(mav)
print("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max))
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True)
print("Wait groundspeed %.1f, target:%.1f" % (m.groundspeed, gs_min))
if m.groundspeed >= gs_min and m.groundspeed <= gs_max:
@ -99,9 +91,9 @@ def wait_groundspeed(mav, gs_min, gs_max, timeout=30):
def wait_roll(mav, roll, accuracy, timeout=30):
'''wait for a given roll in degrees'''
tstart = time.time()
tstart = get_sim_time(mav)
print("Waiting for roll of %d at %s" % (roll, time.ctime()))
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
m = mav.recv_match(type='ATTITUDE', blocking=True)
p = math.degrees(m.pitch)
r = math.degrees(m.roll)
@ -114,9 +106,9 @@ def wait_roll(mav, roll, accuracy, timeout=30):
def wait_pitch(mav, pitch, accuracy, timeout=30):
'''wait for a given pitch in degrees'''
tstart = time.time()
tstart = get_sim_time(mav)
print("Waiting for pitch of %u at %s" % (pitch, time.ctime()))
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
m = mav.recv_match(type='ATTITUDE', blocking=True)
p = math.degrees(m.pitch)
r = math.degrees(m.roll)
@ -131,9 +123,9 @@ def wait_pitch(mav, pitch, accuracy, timeout=30):
def wait_heading(mav, heading, accuracy=5, timeout=30):
'''wait for a given heading'''
tstart = time.time()
tstart = get_sim_time(mav)
print("Waiting for heading %u with accuracy %u" % (heading, accuracy))
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True)
print("Heading %u" % m.heading)
if math.fabs(m.heading - heading) <= accuracy:
@ -145,9 +137,9 @@ def wait_heading(mav, heading, accuracy=5, timeout=30):
def wait_distance(mav, distance, accuracy=5, timeout=30):
'''wait for flight of a given distance'''
tstart = time.time()
tstart = get_sim_time(mav)
start = mav.location()
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
pos = mav.location()
delta = get_distance(start, pos)
print("Distance %.2f meters" % delta)
@ -163,12 +155,12 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1):
'''wait for arrival at a location'''
tstart = time.time()
tstart = get_sim_time(mav)
if target_altitude is None:
target_altitude = loc.alt
print("Waiting for location %.4f,%.4f at altitude %.1f height_accuracy=%.1f" % (
loc.lat, loc.lng, target_altitude, height_accuracy))
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
pos = mav.location()
delta = get_distance(loc, pos)
print("Distance %.2f meters alt %.1f" % (delta, pos.alt))
@ -182,7 +174,7 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height
def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400, mode=None):
'''wait for waypoint ranges'''
tstart = time.time()
tstart = get_sim_time(mav)
# this message arrives after we set the current WP
start_wp = mav.waypoint_current()
current_wp = start_wp
@ -192,7 +184,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
# print("test: Expected start waypoint %u but got %u" % (wpnum_start, start_wp))
# return False
while time.time() < tstart + timeout:
while get_sim_time(mav) < tstart + timeout:
seq = mav.waypoint_current()
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
wp_dist = m.wp_dist
@ -206,7 +198,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
print("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end))
if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
print("test: Starting new waypoint %u" % seq)
tstart = time.time()
tstart = get_sim_time(mav)
current_wp = seq
# the wp_dist check is a hack until we can sort out the right seqnum
# for end of mission
@ -226,13 +218,13 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
def save_wp(mavproxy, mav):
mavproxy.send('rc 7 1000\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
wait_seconds(1)
wait_seconds(mav, 1)
mavproxy.send('rc 7 2000\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True)
wait_seconds(1)
wait_seconds(mav, 1)
mavproxy.send('rc 7 1000\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
wait_seconds(1)
wait_seconds(mav, 1)
def wait_mode(mav, mode, timeout=None):
print("Waiting for mode %s" % mode)