mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
autotest: fixed waypoint wait code
this allows fly.ArduCopter to pass again
This commit is contained in:
Andrew Tridgell
parent
e6860ca90c
commit
be886ef9bc
@ -143,20 +143,16 @@ def land(mavproxy, mav, timeout=60):
|
||||
mavproxy.send('rc 3 1400\n')
|
||||
tstart = time.time()
|
||||
|
||||
if wait_altitude(mav, -5, 0):
|
||||
print("LANDING OK")
|
||||
return True
|
||||
else:
|
||||
print("LANDING FAILED")
|
||||
return False
|
||||
ret = wait_altitude(mav, -5, 0)
|
||||
print("LANDING: ok=%s" % ret)
|
||||
return ret
|
||||
|
||||
|
||||
def circle(mavproxy, mav, maxaltchange=10, holdtime=90, timeout=35):
|
||||
'''fly circle'''
|
||||
print("FLY CIRCLE")
|
||||
mavproxy.send('switch 1\n') # CIRCLE mode
|
||||
mavproxy.expect('CIRCLE>')
|
||||
mavproxy.send('status\n')
|
||||
mavproxy.expect('>')
|
||||
wait_mode(mav, 'CIRCLE')
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
start_altitude = m.alt
|
||||
tstart = time.time()
|
||||
@ -175,21 +171,27 @@ def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
|
||||
print("Fly a mission")
|
||||
global homeloc
|
||||
global num_wp
|
||||
mavproxy.send('wp set 1\n')
|
||||
mavproxy.send('switch 4\n') # auto mode
|
||||
mavproxy.expect('AUTO>')
|
||||
wait_mode(mav, 'AUTO')
|
||||
|
||||
wait_altitude(mav, 30, 40)
|
||||
if wait_waypoint(mav, 1, num_wp):
|
||||
print("MISSION COMPLETE")
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
ret = wait_waypoint(mav, 1, num_wp, timeout=90)
|
||||
|
||||
print("MISSION COMPLETE: passed=%s" % ret)
|
||||
|
||||
# wait here until ready
|
||||
mavproxy.send('switch 5\n')
|
||||
wait_mode(mav, 'LOITER')
|
||||
|
||||
return ret
|
||||
|
||||
#if not wait_distance(mav, 30, timeout=120):
|
||||
# return False
|
||||
#if not wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy):
|
||||
# return False
|
||||
|
||||
|
||||
def load_mission(mavproxy, mav, filename):
|
||||
'''load a mission from a file'''
|
||||
global num_wp
|
||||
@ -203,7 +205,8 @@ def load_mission(mavproxy, mav, filename):
|
||||
num_wp = wploader.count()
|
||||
print("loaded mission")
|
||||
for i in range(num_wp):
|
||||
print (dir(wploader.wp(i)))
|
||||
print wploader.wp(i)
|
||||
return True
|
||||
|
||||
def setup_rc(mavproxy):
|
||||
'''setup RC override control'''
|
||||
@ -301,17 +304,19 @@ def fly_ArduCopter(viewerip=None):
|
||||
if not circle(mavproxy, mav):
|
||||
failed = True
|
||||
|
||||
# fly the stores mission
|
||||
# fly the stored mission
|
||||
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
||||
failed = True
|
||||
|
||||
#fly_mission(mavproxy, mav, os.path.join(testdir, "mission_ttt.txt"), height_accuracy=0.2)
|
||||
|
||||
if not load_mission(mavproxy, mav, os.path.join(testdir, "mission_ttt.txt")):
|
||||
if not load_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt")):
|
||||
failed = True
|
||||
|
||||
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
||||
failed = True
|
||||
else:
|
||||
print("Flew mission_ttt OK")
|
||||
|
||||
if not land(mavproxy, mav):
|
||||
failed = True
|
||||
|
||||
@ -7,8 +7,8 @@ expect_list = []
|
||||
def message_hook(mav, msg):
|
||||
'''called as each mavlink msg is received'''
|
||||
global expect_list
|
||||
if msg.get_type() in [ 'NAV_CONTROLLER_OUTPUT', 'GPS_RAW' ]:
|
||||
print(msg)
|
||||
# if msg.get_type() in [ 'NAV_CONTROLLER_OUTPUT', 'GPS_RAW' ]:
|
||||
# print(msg)
|
||||
for p in expect_list:
|
||||
try:
|
||||
p.read_nonblocking(100, timeout=0)
|
||||
@ -143,27 +143,30 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=60):
|
||||
start_wp = m.seq
|
||||
current_wp = start_wp
|
||||
|
||||
print("\n***wait for waypoint ranges***\n\n\n")
|
||||
print("\n***wait for waypoint ranges start=%u end=%u ***\n\n\n" % (wpnum_start, wpnum_end))
|
||||
if start_wp != wpnum_start:
|
||||
print("Expected start waypoint %u but got %u" % (wpnum_start, start_wp))
|
||||
return False
|
||||
|
||||
while time.time() < tstart + timeout:
|
||||
m = mav.recv_match(type='WAYPOINT_CURRENT', blocking=True)
|
||||
print("WP %u" % m.seq)
|
||||
if m.seq == current_wp:
|
||||
continue
|
||||
if m.seq == current_wp+1 or (m.seq > current_wp+1 and allow_skip):
|
||||
print("Starting new waypoint %u" % m.seq)
|
||||
seq = m.seq
|
||||
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
|
||||
wp_dist = m.wp_dist
|
||||
print("WP %u (wp_dist=%u)" % (seq, wp_dist))
|
||||
if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
|
||||
print("Starting new waypoint %u" % seq)
|
||||
tstart = time.time()
|
||||
current_wp = m.seq
|
||||
if current_wp == wpnum_end:
|
||||
print("Reached final waypoint %u" % m.seq)
|
||||
return True
|
||||
if m.seq > current_wp+1:
|
||||
print("Skipped waypoint! Got wp %u expected %u" % (m.seq, current_wp+1))
|
||||
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):
|
||||
print("Reached final waypoint %u" % seq)
|
||||
return True
|
||||
if seq > current_wp+1:
|
||||
print("Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1))
|
||||
return False
|
||||
print("Timed out waiting for waypoint %u" % wpnum_end)
|
||||
print("Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end))
|
||||
return False
|
||||
|
||||
def save_wp(mavproxy, mav):
|
||||
|
||||
Loading…
Reference in New Issue
Block a user