diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index fcc622ddfba76..d416d8efcaf75 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -457,6 +457,7 @@ def MAV_CMD_MISSION_START(self):
     def MAV_CMD_DO_CHANGE_SPEED(self):
         '''ensure vehicle changes speeds when DO_CHANGE_SPEED received'''
         items = [
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, -3),  # Dive so we have constrat drag
             (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, -1),
             (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
         ]
@@ -465,6 +466,8 @@ def MAV_CMD_DO_CHANGE_SPEED(self):
         self.arm_vehicle()
         self.run_cmd(mavutil.mavlink.MAV_CMD_MISSION_START)
         self.progress("SENT MISSION START")
+        self.wait_mode('AUTO')
+        self.wait_current_waypoint(2)  # wait after we finish diving to 3m
         for run_cmd in self.run_cmd, self.run_cmd_int:
             for speed in [1, 1.5, 0.5]:
                 run_cmd(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed)