From debca7f517f7640753acad04c16f0cf5b683951a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 3 Dec 2025 11:33:44 +0100 Subject: [PATCH] Initial stab at running MAVSDK tests with SIH and fix the broken parts O.O --- .../sensor_gps_sim/SensorGpsSim.cpp | 2 +- .../sitl_targets_gazebo-classic.cmake | 24 +++++++-------- src/modules/simulation/simulator_sih/sih.cpp | 2 +- test/mavsdk_tests/autopilot_tester.cpp | 21 ++++++++------ test/mavsdk_tests/autopilot_tester.h | 2 +- .../integration_test_runner/process_helper.py | 2 +- .../integration_test_runner/test_runner.py | 29 ------------------- test/mavsdk_tests/test_multicopter_basics.cpp | 12 ++++---- 8 files changed, 35 insertions(+), 59 deletions(-) diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index b7fb16a227..aa1fe2b944 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -118,7 +118,7 @@ void SensorGpsSim::Run() double longitude = vehicle_global_position.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); double altitude = (double)(vehicle_global_position.alt + (generate_wgn() * 0.5f)); - Vector3f gps_vel = Vector3f{vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f); + Vector3f gps_vel = Vector3f{vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz};// + noiseGauss3f(0.06f, 0.077f, 0.158f); // device id device::Device::DeviceId device_id; diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 693c0fa6e5..39a6c57b11 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -246,16 +246,16 @@ if(gazebo_FOUND) add_custom_target(gazebo-classic DEPENDS gazebo-classic_iris) # alias add_custom_target(gazebo DEPENDS gazebo-classic_iris) # alias - - # mavsdk tests currently depend on sitl_gazebo - ExternalProject_Add(mavsdk_tests - SOURCE_DIR ${PX4_SOURCE_DIR}/test/mavsdk_tests - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} - BINARY_DIR ${PX4_BINARY_DIR}/mavsdk_tests - INSTALL_COMMAND "" - USES_TERMINAL_CONFIGURE true - USES_TERMINAL_BUILD true - EXCLUDE_FROM_ALL true - BUILD_ALWAYS 1 - ) endif() + +# mavsdk tests DO NOT depend on sitl_gazebo +ExternalProject_Add(mavsdk_tests + SOURCE_DIR ${PX4_SOURCE_DIR}/test/mavsdk_tests + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + BINARY_DIR ${PX4_BINARY_DIR}/mavsdk_tests + INSTALL_COMMAND "" + USES_TERMINAL_CONFIGURE true + USES_TERMINAL_BUILD true + EXCLUDE_FROM_ALL true + BUILD_ALWAYS 1 +) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index d6227ed4fa..e5c989b2f7 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -619,7 +619,7 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) Vector3f accel_noise; Vector3f gyro_noise; - if (_T_B.longerThan(FLT_EPSILON)) { + if (false && _T_B.longerThan(FLT_EPSILON)) { // too much noise at least for the low update rate O.O accel_noise = noiseGauss3f(0.5f, 1.7f, 1.4f); gyro_noise = noiseGauss3f(0.14f, 0.07f, 0.03f); diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 253e9aebb1..3131a889ae 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/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 {}; + auto prom = std::promise{}; 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 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) diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index b0ac078d5d..46ac3ea3b4 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/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 _should_exit {false}; std::thread _real_time_report_thread {}; diff --git a/test/mavsdk_tests/integration_test_runner/process_helper.py b/test/mavsdk_tests/integration_test_runner/process_helper.py index b399a4756d..dfee81a2b9 100644 --- a/test/mavsdk_tests/integration_test_runner/process_helper.py +++ b/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() diff --git a/test/mavsdk_tests/integration_test_runner/test_runner.py b/test/mavsdk_tests/integration_test_runner/test_runner.py index cf05d8d742..bb577bf947 100644 --- a/test/mavsdk_tests/integration_test_runner/test_runner.py +++ b/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), diff --git a/test/mavsdk_tests/test_multicopter_basics.cpp b/test/mavsdk_tests/test_multicopter_basics.cpp index 427e69c03f..eb00b3bcb1 100644 --- a/test/mavsdk_tests/test_multicopter_basics.cpp +++ b/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 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)); }