diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 59df7171c5..2885bd6f27 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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 diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 8d28325873..3d929a4452 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -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):