Initial stab at running MAVSDK tests with SIH and fix the broken parts O.O

test/mavsdk_tests/autopilot_tester.cpp

@@ -225,13 +225,13 @@ void AutopilotTester::wait_until_hovering()
 
 void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout, float delta)
 {
-	auto prom = std::promise<void> {};
+	auto prom = std::promise<void>{};
 	auto fut = prom.get_future();
 
-	Telemetry::PositionHandle handle = _telemetry->subscribe_position([&prom, rel_altitude_m, delta, &handle,
-	       this](Telemetry::Position new_position) {
-		if (fabs(rel_altitude_m - new_position.relative_altitude_m) <= delta) {
-			_telemetry->unsubscribe_position(handle);
+	Telemetry::PositionVelocityNedHandle handle = _telemetry->subscribe_position_velocity_ned([&prom, rel_altitude_m, delta, &handle,
+	       this](Telemetry::PositionVelocityNed new_position) {
+		if (fabs(rel_altitude_m + new_position.position.down_m) <= delta) {
+			_telemetry->unsubscribe_position_velocity_ned(handle);
 			prom.set_value();
 		}
 	});
@@ -640,16 +640,19 @@ void AutopilotTester::start_checking_altitude(const float max_deviation_m)
 	std::array<float, 3> initial_position = get_current_position_ned();
 	float target_altitude = initial_position[2];
 
-	_check_altitude_handle = _telemetry->subscribe_position([target_altitude, max_deviation_m,
-			 this](Telemetry::Position new_position) {
-		const float current_deviation = fabs((-target_altitude) - new_position.relative_altitude_m);
+	_check_altitude_handle = _telemetry->subscribe_position_velocity_ned([target_altitude, max_deviation_m,
+			 this](Telemetry::PositionVelocityNed new_position) {
+		const float current_deviation = fabs(target_altitude - new_position.position.down_m);
+		printf("target_altitude: %.3f\n", (double)target_altitude);
+		// printf("position_velocity_ned.position.down_m: %.3f\n", (double)position_velocity_ned.position.down_m);
+		printf("new_position.position.down_m: %.3f\n", (double)new_position.position.down_m);
 		CHECK(current_deviation <= max_deviation_m);
 	});
 }
 
 void AutopilotTester::stop_checking_altitude()
 {
-	_telemetry->unsubscribe_position(_check_altitude_handle);
+	_telemetry->unsubscribe_position_velocity_ned(_check_altitude_handle);
 }
 
 void AutopilotTester::check_tracks_mission_raw(float corridor_radius_m, bool reverse)

test/mavsdk_tests/autopilot_tester.h

@@ -298,7 +298,7 @@ private:
 
 	Telemetry::GroundTruth _home{NAN, NAN, NAN};
 
-	mavsdk::Telemetry::PositionHandle _check_altitude_handle{};
+	mavsdk::Telemetry::PositionVelocityNedHandle _check_altitude_handle{};
 
 	std::atomic<bool> _should_exit {false};
 	std::thread _real_time_report_thread {};

test/mavsdk_tests/integration_test_runner/process_helper.py

@@ -168,7 +168,7 @@ class Px4Runner(Runner):
                 os.path.join(workspace_dir, "test_data"),
                 "-d"
             ]
-        self.env["PX4_SIM_MODEL"] = "gazebo-classic_" + self.model
+        self.env["PX4_SIM_MODEL"] = "sihsim_quadx"
         self.env["PX4_SIM_SPEED_FACTOR"] = str(speed_factor)
         self.debugger = debugger
         self.clear_rootfs()

test/mavsdk_tests/integration_test_runner/test_runner.py

@@ -310,35 +310,6 @@ class Tester:
                 else:
                     world_name = 'empty.world'
 
-                gzserver_runner = ph.GzserverRunner(
-                    os.getcwd(),
-                    log_dir,
-                    test['vehicle'],
-                    case,
-                    self.get_max_speed_factor(test),
-                    self.verbose,
-                    self.build_dir,
-                    world_name)
-                self.active_runners.append(gzserver_runner)
-
-                gzmodelspawn_runner = ph.GzmodelspawnRunner(
-                    os.getcwd(),
-                    log_dir,
-                    test['vehicle'],
-                    case,
-                    self.verbose,
-                    self.build_dir)
-                self.active_runners.append(gzmodelspawn_runner)
-
-                if self.gui:
-                    gzclient_runner = ph.GzclientRunner(
-                        os.getcwd(),
-                        log_dir,
-                        test['model'],
-                        case,
-                        self.verbose)
-                    self.active_runners.append(gzclient_runner)
-
                 # We must start the PX4 instance at the end, as starting
                 # it in the beginning, then connecting Gazebo server freaks
                 # out the PX4 (it needs to have data coming in when started),

test/mavsdk_tests/test_multicopter_basics.cpp

@@ -40,8 +40,6 @@
 TEST_CASE("Takeoff and hold position", "[multicopter][vtol]")
 {
 	const float takeoff_altitude = 10.f;
-	const float altitude_tolerance = 0.2f;
-	const int delay_seconds = 60.f;
 
 	AutopilotTester tester;
 	tester.connect(connection_url);
@@ -52,14 +50,18 @@ TEST_CASE("Takeoff and hold position", "[multicopter][vtol]")
 	// The sleep here is necessary for the takeoff altitude to be applied properly
 	std::this_thread::sleep_for(std::chrono::seconds(1));
 
+	// Capture altitude before takeoff
+	std::array<float, 3> initial_position = tester.get_current_position_ned();
+	float ground_altitude = -initial_position[2];
+
 	// Takeoff
 	tester.arm();
 	tester.takeoff();
 	tester.wait_until_hovering();
-	tester.wait_until_altitude(takeoff_altitude, std::chrono::seconds(30), altitude_tolerance);
+	tester.wait_until_altitude(ground_altitude + takeoff_altitude, std::chrono::seconds(15), 0.01f);
 
 	// Monitor altitude and fail if it exceeds the tolerance
-	tester.start_checking_altitude(altitude_tolerance + 0.1);
+	tester.start_checking_altitude(0.15);
 
-	std::this_thread::sleep_for(std::chrono::seconds(delay_seconds));
+	std::this_thread::sleep_for(std::chrono::seconds(15));
 }