From 3001e566ca6a27f989572dff79edf962527258c3 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 13 Nov 2011 22:58:27 -0800 Subject: [PATCH] Had to disable check of current WP. Going into auto mode executes commands right away and the reported index will differ per mission. Multiple commands may be executed. --- Tools/autotest/common.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 3d929a4452..b2567f2baa 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -138,24 +138,25 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=60): '''wait for waypoint ranges''' tstart = time.time() + # this message arrives after we set the current WP m = mav.recv_match(type='WAYPOINT_CURRENT', blocking=True) start_wp = m.seq current_wp = start_wp - 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 + print("\ntest: wait for waypoint ranges start=%u end=%u\n\n" % (wpnum_start, wpnum_end)) + # if start_wp != wpnum_start: + # print("test: 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) 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)) + print("test: 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) + 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 @@ -174,7 +175,8 @@ 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') + #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'''