mavsdk_tests: adjust timeouts by lockstep speed up

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

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;
+	}
+}

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,23 +109,15 @@ 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);
 
-	mavsdk::Mavsdk _mavsdk{};
+	std::chrono::milliseconds adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms);
-	std::unique_ptr<mavsdk::Action> _action{};
-	std::unique_ptr<mavsdk::Failure> _failure{};
-	std::unique_ptr<mavsdk::Mission> _mission{};
-	std::unique_ptr<mavsdk::Offboard> _offboard{};
-	std::unique_ptr<mavsdk::Param> _param{};
-	std::unique_ptr<mavsdk::Telemetry> _telemetry{};
-
-	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);
+		std::chrono::milliseconds duration_ms(duration);
+		adjust_to_lockstep_speed(duration_ms);
 
 		unsigned iteration = 0;
 
@@ -139,4 +132,16 @@ bool poll_condition_with_timeout(
 		return true;
 	}
 
-inline float sq(float x) { return x * x; };
+	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{};
+	std::unique_ptr<mavsdk::Telemetry> _telemetry{};
+
+	Telemetry::GroundTruth _home{NAN, NAN, NAN};
+};