diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index 580fc2b66a..b2df00626c 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -81,7 +81,7 @@ def dive_mission(mavproxy, mav, filename):
     mavproxy.send('mode auto\n')
     wait_mode(mav, 'AUTO')
     
-    if not wait_waypoint(mav, 1, 6, max_dist=5):
+    if not wait_waypoint(mav, 1, 5, max_dist=5):
         return False
     
     mavproxy.send('disarm\n');
diff --git a/Tools/autotest/sub_mission.txt b/Tools/autotest/sub_mission.txt
index 6550cf42af..298b6bdc89 100644
--- a/Tools/autotest/sub_mission.txt
+++ b/Tools/autotest/sub_mission.txt
@@ -3,6 +3,5 @@ QGC WPL 110
 1	0	3	16	0.000000	0.000000	0.000000	0.000000	33.809780	-118.393982	-50.000000	1
 2	0	3	16	0.000000	0.000000	0.000000	0.000000	33.809780	-118.394432	-50.000000	1
 3	0	3	16	0.000000	0.000000	0.000000	0.000000	33.810310	-118.394432	-50.000000	1
-4	0	3	18	2.000000	0.000000	50.000000	1.000000	33.810310	-118.395020	-25.000000	1
-5	0	3	18	2.000000	0.000000	10.000000	1.000000	33.810314	-118.395020	-50.000000	1
-6	0	3	21	25.000000	0.000000	0.000000	0.000000	33.811207	-118.393036	-25.000000	1
+4	0	3	18	1.000000	0.000000	50.000000	1.000000	33.810310	-118.395020	-25.000000	1
+5	0	3	21	25.000000	0.000000	0.000000	0.000000	33.811207	-118.393036	-25.000000	1