diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 5fe1236629..dc13bdc2af 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -292,7 +292,7 @@ def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None): mavproxy.send('switch 4\n') # auto mode wait_mode(mav, 'AUTO') #wait_altitude(mav, 30, 40) - ret = wait_waypoint(mav, 0, num_wp, timeout=500) + ret = wait_waypoint(mav, 0, num_wp, timeout=500, mode='AUTO') print("test: MISSION COMPLETE: passed=%s" % ret) # wait here until ready mavproxy.send('switch 5\n') diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 7668a3cef5..c3bfa3c8d3 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -160,7 +160,7 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height print("Failed to attain location") return False -def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400): +def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400, mode=None): '''wait for waypoint ranges''' tstart = time.time() # this message arrives after we set the current WP @@ -178,6 +178,11 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time wp_dist = m.wp_dist m = mav.recv_match(type='VFR_HUD', blocking=True) + # if we exited the required mode, finish + if mode is not None and mav.flightmode != mode: + print('Exited %s mode' % mode) + return True + print("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end)) if seq == current_wp+1 or (seq > current_wp+1 and allow_skip): print("test: Starting new waypoint %u" % seq)