diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index 599a1a879f..693f49af26 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -226,13 +226,7 @@ void AutopilotTester::execute_mission()
 
 	// TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.
 
-	REQUIRE(poll_condition_with_timeout(
-	[this]() {
-		auto result = _mission->is_mission_finished();
-		return result.first == Mission::Result::Success && result.second;
-	}, std::chrono::seconds(60)));
-
-	REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready);
+	wait_for_mission_finished(std::chrono::seconds(60));
 }
 
 void AutopilotTester::execute_mission_and_lose_gps()
@@ -611,6 +605,21 @@ void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state,
 	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
 }
 
+void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout)
+{
+	auto prom = std::promise<void> {};
+	auto fut = prom.get_future();
+
+	_mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) {
+		if (progress.current == progress.total) {
+			_mission->subscribe_mission_progress(nullptr);
+			prom.set_value();
+		}
+	});
+
+	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
+}
+
 std::chrono::milliseconds AutopilotTester::adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms)
 {
 	if (_info == nullptr) {
diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
index a798479dc5..55003f33f7 100644
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@@ -129,6 +129,7 @@ private:
 	void start_and_wait_for_first_mission_item();
 	void wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout);
 	void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout);
+	void wait_for_mission_finished(std::chrono::seconds timeout);
 
 	std::chrono::milliseconds adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms);