diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 4ff2a92a91..4d0c23f841 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -6542,6 +6542,7 @@ Also, ignores heartbeats not from our target system'''
         try:
             self.check_rc_defaults()
             self.change_mode(self.default_mode())
+            self.set_current_waypoint(0, check_afterwards=False)
             self.drain_mav()
             self.drain_all_pexpects()
 
diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py
index 923e9b4ed9..62b07de753 100644
--- a/Tools/autotest/quadplane.py
+++ b/Tools/autotest/quadplane.py
@@ -446,6 +446,7 @@ class AutoTestQuadPlane(AutoTest):
 
     def takeoff(self, height, mode):
         """climb to specified height and set throttle to 1500"""
+        self.set_current_waypoint(0, check_afterwards=False)
         self.change_mode(mode)
         self.wait_ready_to_arm()
         self.arm_vehicle()