Skip to content
Snippets Groups Projects
Commit ddcfa90b authored by Jason Short's avatar Jason Short
Browse files

testing updates

parent c5859515
No related branches found
No related tags found
No related merge requests found
...@@ -80,7 +80,7 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30): ...@@ -80,7 +80,7 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
climb_rate = m.alt - previous_alt climb_rate = m.alt - previous_alt
previous_alt = m.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: if abs(climb_rate) > 0:
tstart = time.time(); tstart = time.time();
if m.alt >= alt_min and m.alt <= alt_max: if m.alt >= alt_min and m.alt <= alt_max:
...@@ -140,6 +140,8 @@ def wait_distance(mav, distance, accuracy=5, timeout=30): ...@@ -140,6 +140,8 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
print("Distance %.2f meters" % delta) print("Distance %.2f meters" % delta)
if math.fabs(delta - distance) <= accuracy: if math.fabs(delta - distance) <= accuracy:
return True return True
if(delta > (distance + accuracy)):
return False
print("Failed to attain distance %u" % distance) print("Failed to attain distance %u" % distance)
return False return False
...@@ -181,14 +183,18 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=400): ...@@ -181,14 +183,18 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=400):
seq = m.seq seq = m.seq
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True) m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
wp_dist = m.wp_dist 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): if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
print("test: Starting new waypoint %u" % seq) print("test: Starting new waypoint %u" % seq)
tstart = time.time() tstart = time.time()
current_wp = seq current_wp = seq
# the wp_dist check is a hack until we can sort out the right seqnum # the wp_dist check is a hack until we can sort out the right seqnum
# for end of mission # 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) print("Reached final waypoint %u" % seq)
return True return True
if seq > current_wp+1: if seq > current_wp+1:
...@@ -202,8 +208,6 @@ def save_wp(mavproxy, mav): ...@@ -202,8 +208,6 @@ def save_wp(mavproxy, mav):
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True) mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True)
mavproxy.send('rc 7 1000\n') mavproxy.send('rc 7 1000\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True) 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): def wait_mode(mav, mode):
'''wait for a flight mode to be engaged''' '''wait for a flight mode to be engaged'''
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment