diff --git a/Tools/autotest/ap1.txt b/Tools/autotest/ap1.txt
index f12a9c08fb..3f053bf20f 100644
--- a/Tools/autotest/ap1.txt
+++ b/Tools/autotest/ap1.txt
@@ -1,7 +1,8 @@
 QGC WPL 110
-0	1	0	16	0.000000	0.000000	0.000000	0.000000	-35.362881	149.165222	582.000000	1
-1	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361553	149.163956	120.000000	1
-2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364540	149.162857	120.000000	1
-3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361721	149.161835	120.000000	1
-4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364170	149.164627	120.000000	1
-5	0	3	20	0.000000	0.000000	0.000000	0.000000	-35.363289	149.165253	50.000000	1
+0	1	0	16	0	0	0	0	-35.362881	149.165222	582.000000	1
+1	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361553	149.163956	100.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364540	149.162857	100.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361721	149.161835	40.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.367968	149.164124	25.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366813	149.165883	20.000000	1
+6	0	3	21	0.000000	0.000000	0.000000	0.000000	-35.362911	149.165222	0.000000	1
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 230d8e67e1..cbf0157f8e 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -19,15 +19,15 @@ def takeoff(mavproxy, mav):
     wait_mode(mav, 'FBWA')
 
     # some rudder to counteract the prop torque
-    mavproxy.send('rc 4 1600\n')
+    mavproxy.send('rc 4 1650\n')
 
-    # get it moving a bit first to avoid bad JSBSim ground physics
-    mavproxy.send('rc 3 1040\n')
-    mav.recv_match(condition='VFR_HUD.groundspeed>3', blocking=True)
+    # get it moving a bit first
+    mavproxy.send('rc 3 1100\n')
+    mav.recv_match(condition='VFR_HUD.groundspeed>2', blocking=True)
 
     # a bit faster
     mavproxy.send('rc 3 1600\n')
-    mav.recv_match(condition='VFR_HUD.groundspeed>10', blocking=True)
+    mav.recv_match(condition='VFR_HUD.groundspeed>12', blocking=True)
 
     # hit the gas harder now, and give it some elevator
     mavproxy.send('rc 4 1500\n')
@@ -182,9 +182,9 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=Non
     mavproxy.expect('Requesting [0-9]+ waypoints')
     mavproxy.send('switch 1\n') # auto mode
     wait_mode(mav, 'AUTO')
-    if not wait_distance(mav, 30, timeout=120):
+    if not wait_waypoint(mav, 1, 6, max_dist=60):
         return False
-    if not wait_location(mav, homeloc, accuracy=50, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy):
+    if not wait_groundspeed(mav, 0, 0.5):
         return False
     print("Mission OK")
     return True
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 777c0ddba1..1fc0384f70 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -1,4 +1,4 @@
-import util, pexpect, time, math
+import util, pexpect, time, math, mavwp
 
 # a list of pexpect objects to read while waiting for
 # messages. This keeps the output to stdout flowing
@@ -88,6 +88,18 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
     print("Failed to attain altitude range")
     return False
 
+def wait_groundspeed(mav, gs_min, gs_max, timeout=30):
+    '''wait for a given ground speed range'''
+    tstart = time.time()
+    print("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max))
+    while time.time() < tstart + timeout:
+        m = mav.recv_match(type='VFR_HUD', blocking=True)
+        print("Wait groundspeed %.1f, target:%.1f" % (m.groundspeed, gs_min))
+        if m.groundspeed >= gs_min and m.groundspeed <= gs_max:
+            return True
+    print("Failed to attain groundspeed range")
+    return False
+
 
 def wait_roll(mav, roll, accuracy, timeout=30):
     '''wait for a given roll in degrees'''
@@ -164,7 +176,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, timeout=400):
+def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400):
     '''wait for waypoint ranges'''
     tstart = time.time()
     # this message arrives after we set the current WP
@@ -191,7 +203,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=400):
             # 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):
-        if (current_wp == wpnum_end and wp_dist < 2):
+        if (current_wp == wpnum_end and wp_dist < max_dist):
             print("Reached final waypoint %u" % seq)
             return True
         if (current_wp == 255):
@@ -212,3 +224,10 @@ def save_wp(mavproxy, mav):
 def wait_mode(mav, mode):
     '''wait for a flight mode to be engaged'''
     mav.recv_match(condition='MAV.flightmode=="%s"' % mode, blocking=True)
+
+def mission_count(filename):
+    '''load a mission from a file and return number of waypoints'''
+    wploader = mavwp.MAVWPLoader()
+    wploader.load(filename)
+    num_wp = wploader.count()
+    return num_wp