diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index 61916b1f2b..430ca78890 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -603,28 +603,3 @@ void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout)
 
 	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) {
-		return duration_ms;
-	}
-
-	auto speed_factor = _info->get_speed_factor();
-
-	if (speed_factor.first == Info::Result::Success) {
-		// FIXME: Remove this again:
-		//        Sanitize speed factor to avoid test failures.
-		if (speed_factor.second > 20.0f) {
-			speed_factor.second = 20.0f;
-		}
-
-		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 55003f33f7..76174dff59 100644
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@@ -131,8 +131,6 @@ private:
 	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);
-
 	template<typename Rep, typename Period>
 	bool poll_condition_with_timeout(
 		std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)
@@ -184,6 +182,33 @@ private:
 		return true;
 	}
 
+	template<typename Rep, typename Period>
+	std::chrono::microseconds adjust_to_lockstep_speed(std::chrono::duration<Rep, Period> duration)
+	{
+		const std::chrono::microseconds duration_us(duration);
+
+		if (_info == nullptr) {
+			return duration_us;
+		}
+
+		auto speed_factor = _info->get_speed_factor();
+
+		if (speed_factor.first != Info::Result::Success) {
+			return duration_us;
+		}
+
+		// FIXME: Remove this again:
+		//        Sanitize speed factor to avoid test failures.
+		if (speed_factor.second > 50.0f) {
+			speed_factor.second = 50.0f;
+		}
+
+		const auto new_duration_us  = static_cast<std::chrono::microseconds>(duration_us.count() / static_cast<int>
+					      (speed_factor.second));
+
+		return new_duration_us;
+	}
+
 	mavsdk::Mavsdk _mavsdk{};
 	std::unique_ptr<mavsdk::Action> _action{};
 	std::unique_ptr<mavsdk::Failure> _failure{};