diff --git a/mavsdk_tests/autopilot_tester.cpp b/mavsdk_tests/autopilot_tester.cpp
index 12ad2eb7d4..fe4a46d912 100644
--- a/mavsdk_tests/autopilot_tester.cpp
+++ b/mavsdk_tests/autopilot_tester.cpp
@@ -100,6 +100,11 @@ void AutopilotTester::execute_mission()
         prom.set_value();
     });
 
+    // TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.
+
+    REQUIRE(poll_condition_with_timeout(
+        [this]() { return _mission->mission_finished(); }, std::chrono::seconds(30)));
+
     REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready);
 }
 
diff --git a/mavsdk_tests/test_mission_multicopter.cpp b/mavsdk_tests/test_mission_multicopter.cpp
index 8a27935ffc..f5ee5e7584 100644
--- a/mavsdk_tests/test_mission_multicopter.cpp
+++ b/mavsdk_tests/test_mission_multicopter.cpp
@@ -35,6 +35,5 @@ TEST_CASE("We can fly a square mission and do RTL", "[multicopter]")
 
     tester.arm();
     tester.execute_mission();
-    tester.wait_until_hovering();
     tester.wait_until_disarmed();
 }
\ No newline at end of file