testing updates

This commit is contained in:
Jason Short 2011-12-15 20:42:10 -08:00
parent 6e81b1a6f8
commit ced4cf1538
1 changed files with 9 additions and 5 deletions

View File

@ -80,7 +80,7 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
m = mav.recv_match(type='VFR_HUD', blocking=True)
climb_rate = m.alt - previous_alt
previous_alt = m.alt
print("Altitude %u, rate: %u" % (m.alt, climb_rate))
print("Wait Altitude %u, alt:%u, rate: %u" % (m.alt, alt_min , climb_rate))
if abs(climb_rate) > 0:
tstart = time.time();
if m.alt >= alt_min and m.alt <= alt_max:
@ -140,6 +140,8 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
print("Distance %.2f meters" % delta)
if math.fabs(delta - distance) <= accuracy:
return True
if(delta > (distance + accuracy)):
return False
print("Failed to attain distance %u" % distance)
return False
@ -181,14 +183,18 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=400):
seq = m.seq
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
wp_dist = m.wp_dist
print("test: WP %u (wp_dist=%u)" % (seq, wp_dist))
print("test: WP %u (wp_dist=%u), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, 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()
current_wp = seq
# the wp_dist check is a hack until we can sort out the right seqnum
# for end of mission
if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2):
#if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2):
if (current_wp == wpnum_end and wp_dist < 2):
print("Reached final waypoint %u" % seq)
return True
if (current_wp == 255):
print("Reached final waypoint %u" % seq)
return True
if seq > current_wp+1:
@ -202,8 +208,6 @@ def save_wp(mavproxy, mav):
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True)
mavproxy.send('rc 7 1000\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
#mavproxy.send('wp list\n')
#mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
def wait_mode(mav, mode):
'''wait for a flight mode to be engaged'''