diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt
index 483f17afb6..356cf641c1 100644
--- a/test/mavsdk_tests/CMakeLists.txt
+++ b/test/mavsdk_tests/CMakeLists.txt
@@ -22,6 +22,7 @@ if(MAVSDK_FOUND)
         MAVSDK::mavsdk
         MAVSDK::mavsdk_action
         MAVSDK::mavsdk_failure
+        MAVSDK::mavsdk_info
         MAVSDK::mavsdk_mission
         MAVSDK::mavsdk_offboard
         MAVSDK::mavsdk_param
diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index 9b4295a2f8..337a9f5932 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -97,12 +97,13 @@ void AutopilotTester::connect(const std::string uri)
 
 	std::cout << "Waiting for system connect" << std::endl;
 	REQUIRE(poll_condition_with_timeout(
-	[this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25)));
+	[this]() { return _mavsdk.is_connected(); }, adjust_to_lockstep_speed(std::chrono::seconds(25))));
 
 	auto &system = _mavsdk.system();
 
 	_action.reset(new Action(system));
 	_failure.reset(new Failure(system));
+	_info.reset(new Info(system));
 	_mission.reset(new Mission(system));
 	_offboard.reset(new Offboard(system));
 	_param.reset(new Param(system));
@@ -569,3 +570,22 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry:
 
 	return pass;
 }
+
+std::chrono::milliseconds AutopilotTester::adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms)
+{
+	if (_info == nullptr) {
+		return duration_ms;
+	}
+
+	auto speed_factor = _info->get_speed_factor();
+
+	if (speed_factor.first == Info::Result::Success) {
+		return static_cast<std::chrono::milliseconds>(
+			       static_cast<unsigned long>(
+				       std::round(
+					       static_cast<double>(duration_ms.count()) / speed_factor.second)));
+
+	} else {
+		return duration_ms;
+	}
+}
diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
index 7b781ca93f..10f3b6c58e 100644
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@@ -37,6 +37,7 @@
 #include <mavsdk/geometry.h>
 #include <mavsdk/plugins/action/action.h>
 #include <mavsdk/plugins/failure/failure.h>
+#include <mavsdk/plugins/info/info.h>
 #include <mavsdk/plugins/mission/mission.h>
 #include <mavsdk/plugins/offboard/offboard.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
@@ -108,9 +109,35 @@ private:
 	bool estimated_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m);
 	bool estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m);
 
+	std::chrono::milliseconds adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms);
+
+	template<typename Rep, typename Period>
+	bool poll_condition_with_timeout(
+		std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)
+	{
+		// We need millisecond resolution for sleeping.
+		std::chrono::milliseconds duration_ms(duration);
+		adjust_to_lockstep_speed(duration_ms);
+
+		unsigned iteration = 0;
+
+		while (!fun()) {
+			std::this_thread::sleep_for(duration_ms / 100);
+
+			if (iteration++ >= 100) {
+				return false;
+			}
+		}
+
+		return true;
+	}
+
+	static float sq(float x) { return x * x; };
+
 	mavsdk::Mavsdk _mavsdk{};
 	std::unique_ptr<mavsdk::Action> _action{};
 	std::unique_ptr<mavsdk::Failure> _failure{};
+	std::unique_ptr<mavsdk::Info> _info{};
 	std::unique_ptr<mavsdk::Mission> _mission{};
 	std::unique_ptr<mavsdk::Offboard> _offboard{};
 	std::unique_ptr<mavsdk::Param> _param{};
@@ -118,25 +145,3 @@ private:
 
 	Telemetry::GroundTruth _home{NAN, NAN, NAN};
 };
-
-template<typename Rep, typename Period>
-bool poll_condition_with_timeout(
-	std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)
-{
-	// We need millisecond resolution for sleeping.
-	const std::chrono::milliseconds duration_ms(duration);
-
-	unsigned iteration = 0;
-
-	while (!fun()) {
-		std::this_thread::sleep_for(duration_ms / 100);
-
-		if (iteration++ >= 100) {
-			return false;
-		}
-	}
-
-	return true;
-}
-
-inline float sq(float x) { return x * x; };