diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index d9c5fb5151..ffba005424 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -476,6 +476,9 @@ void AutopilotTester::fly_forward_in_altctl()
 	}
 
 	CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success);
+	store_home();
+	wait_until_ready();
+	arm();
 
 	// Climb up for 20 seconds
 	for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) {
diff --git a/test/mavsdk_tests/test_multicopter_manual.cpp b/test/mavsdk_tests/test_multicopter_manual.cpp
index b4ad122670..34fa8b4351 100644
--- a/test/mavsdk_tests/test_multicopter_manual.cpp
+++ b/test/mavsdk_tests/test_multicopter_manual.cpp
@@ -48,9 +48,6 @@ TEST_CASE("Fly forward in altitude control", "[multicopter][vtol]")
 {
 	AutopilotTester tester;
 	tester.connect(connection_url);
-	tester.wait_until_ready();
-	tester.store_home();
-	tester.arm();
 	tester.fly_forward_in_altctl();
 	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
 	tester.wait_until_disarmed(until_disarmed_timeout);