diff --git a/.github/actions/build-sitl-sih/action.yml b/.github/actions/build-sitl-sih/action.yml new file mode 100644 index 0000000000..d220a24d07 --- /dev/null +++ b/.github/actions/build-sitl-sih/action.yml @@ -0,0 +1,21 @@ +name: Build SIH SITL +description: Build PX4 SIH firmware and MAVSDK integration tests + +runs: + using: composite + steps: + - name: Build - PX4 Firmware (SIH) + shell: bash + run: make px4_sitl_sih + + - name: Cache - Stats after PX4 Firmware + shell: bash + run: ccache -s + + - name: Build - MAVSDK Tests + shell: bash + run: make px4_sitl_sih mavsdk_tests + + - name: Cache - Stats after MAVSDK Tests + shell: bash + run: ccache -s diff --git a/.github/actions/setup-mavsdk/action.yml b/.github/actions/setup-mavsdk/action.yml new file mode 100644 index 0000000000..f14fd69b95 --- /dev/null +++ b/.github/actions/setup-mavsdk/action.yml @@ -0,0 +1,13 @@ +name: Setup MAVSDK +description: Download and install the MAVSDK C++ library from GitHub releases + +runs: + using: composite + steps: + - name: Download MAVSDK + shell: bash + run: wget -q "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb" + + - name: Install MAVSDK + shell: bash + run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb" diff --git a/.github/workflows/sitl_tests_sih.yml b/.github/workflows/sitl_tests_sih.yml new file mode 100644 index 0000000000..257bcab477 --- /dev/null +++ b/.github/workflows/sitl_tests_sih.yml @@ -0,0 +1,66 @@ +name: SITL Tests (SIH) + +on: + push: + branches: + - 'main' + paths-ignore: + - 'docs/**' + pull_request: + branches: + - '**' + paths-ignore: + - 'docs/**' + +permissions: + contents: read + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build: + name: Testing PX4 SIH + runs-on: [runs-on,runner=2cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",extras=s3-cache] + container: + image: px4io/px4-dev:v1.17.0-rc2 + env: + PX4_CMAKE_BUILD_TYPE: RelWithDebInfo + PX4_SBOM_DISABLE: 1 + + steps: + - uses: runs-on/action@v2 + - uses: actions/checkout@v6 + with: + fetch-depth: 1 + - name: Configure Git Safe Directory + run: git config --system --add safe.directory '*' + + - uses: ./.github/actions/setup-ccache + id: ccache + with: + cache-key-prefix: ccache-sitl-sih-mavsdk + max-size: 250M + + - uses: ./.github/actions/setup-mavsdk + - uses: ./.github/actions/build-sitl-sih + + - uses: ./.github/actions/save-ccache + if: always() + with: + cache-primary-key: ${{ steps.ccache.outputs.cache-primary-key }} + + - name: Run SITL / MAVSDK Tests + run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 10 --abort-early --model quadx test/mavsdk_tests/configs/sih.json --verbose --force-color --build-dir build/px4_sitl_sih/ + timeout-minutes: 10 + + - name: Upload failed logs + if: failure() + uses: actions/upload-artifact@v7 + with: + name: failed-sih-logs + path: | + logs/**/**/**/*.log + logs/**/**/**/*.ulg + build/px4_sitl_sih/tmp_mavsdk_tests/rootfs/log/**/*.ulg diff --git a/cmake/mavsdk_tests.cmake b/cmake/mavsdk_tests.cmake new file mode 100644 index 0000000000..b9feab8804 --- /dev/null +++ b/cmake/mavsdk_tests.cmake @@ -0,0 +1,30 @@ +############################################################################ +# +# mavsdk_tests ExternalProject +# +# Builds the MAVSDK C++ integration tests as an external project. +# Available to any SITL build (SIH, Gazebo, etc.) when the MAVSDK +# library is installed. Use: make mavsdk_tests +# +# The ExternalProject is always defined (EXCLUDE_FROM_ALL) so the +# target exists even if MAVSDK is not yet installed. The actual build +# will fail at compile time if MAVSDK is missing, which is the +# expected workflow (install MAVSDK, then build the target). +# +############################################################################ + +if(TARGET mavsdk_tests) + return() +endif() + +include(ExternalProject) +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/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 594fa95c99..b8c4ed12b5 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -163,6 +163,8 @@ elseif("${PX4_BOARD}" MATCHES "sitl") include(sitl_tests) endif() + include(mavsdk_tests) + # "none" legacy SITL helper target add_custom_target(none COMMAND $ diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp index 61ecb6963b..6b3cdcd8ed 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp @@ -44,7 +44,9 @@ SensorMagSim::SensorMagSim() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { - _px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM); + for (uint8_t i = 0; i < MAG_COUNT; i++) { + _px4_mag[i].set_device_type(DRV_MAG_DEVTYPE_MAGSIM); + } } SensorMagSim::~SensorMagSim() @@ -130,14 +132,16 @@ void SensorMagSim::Run() vehicle_attitude_s attitude; if (_vehicle_attitude_sub.update(&attitude)) { - Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred; + const Vector3f base_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred; - expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f); + for (uint8_t i = 0; i < MAG_COUNT; i++) { + const Vector3f field = base_field + noiseGauss3f(0.02f, 0.02f, 0.03f); - _px4_mag.update(attitude.timestamp, - expected_field(0) + _sim_mag_offset_x.get(), - expected_field(1) + _sim_mag_offset_y.get(), - expected_field(2) + _sim_mag_offset_z.get()); + _px4_mag[i].update(attitude.timestamp, + field(0) + _sim_mag_offset_x.get(), + field(1) + _sim_mag_offset_y.get(), + field(2) + _sim_mag_offset_z.get()); + } } } diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index f4613f6f0e..bf735f3a2e 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp @@ -80,7 +80,11 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION + static constexpr uint8_t MAG_COUNT = 2; + PX4Magnetometer _px4_mag[MAG_COUNT] { + {197388, ROTATION_NONE}, // DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3 + {197644, ROTATION_NONE}, // DRV_MAG_DEVTYPE_MAGSIM, BUS: 2, ADDR: 3 + }; bool _mag_earth_available{false}; 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..97f785c7ae 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -247,15 +247,5 @@ 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 - ) + include(mavsdk_tests) endif() diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 73da84c07e..123a09bd84 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -34,6 +34,7 @@ if(MAVSDK_FOUND) test_vtol_rtl.cpp test_vtol_mission_wind.cpp test_vtol_loiter_airspeed_failure_blockage.cpp + test_ekf2_selector.cpp # test_multicopter_follow_me.cpp ) diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index b0ac078d5d..9c1d50bbcd 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -164,6 +164,11 @@ public: CHECK(_param->set_param_int(param, value) == Param::Result::Success); } + void set_param_float(const std::string ¶m, float value) + { + CHECK(_param->set_param_float(param, value) == Param::Result::Success); + } + template void sleep_for(std::chrono::duration duration) { diff --git a/test/mavsdk_tests/configs/sih.json b/test/mavsdk_tests/configs/sih.json new file mode 100644 index 0000000000..829938dd16 --- /dev/null +++ b/test/mavsdk_tests/configs/sih.json @@ -0,0 +1,14 @@ +{ + "mode": "sitl", + "simulator": "sih", + "mavlink_connection": "udp://0.0.0.0:14540", + "tests": + [ + { + "model": "quadx", + "vehicle": "quadx", + "test_filter": "[ekf2_selector]", + "timeout_min": 5 + } + ] +} diff --git a/test/mavsdk_tests/integration_test_runner/process_helper.py b/test/mavsdk_tests/integration_test_runner/process_helper.py index b399a4756d..9c038b5881 100644 --- a/test/mavsdk_tests/integration_test_runner/process_helper.py +++ b/test/mavsdk_tests/integration_test_runner/process_helper.py @@ -152,7 +152,8 @@ class Px4Runner(Runner): def __init__(self, workspace_dir: str, log_dir: str, model: str, case: str, speed_factor: float, debugger: str, verbose: bool, build_dir: str, - rootfs_base_dirname: str): + rootfs_base_dirname: str, + simulator: str = "gazebo"): super().__init__(log_dir, model, case, verbose) self.name = "px4" self.cwd = os.path.join(workspace_dir, build_dir, @@ -168,7 +169,13 @@ class Px4Runner(Runner): os.path.join(workspace_dir, "test_data"), "-d" ] - self.env["PX4_SIM_MODEL"] = "gazebo-classic_" + self.model + + if simulator == "sih": + self.env["PX4_SIM_MODEL"] = "sihsim_" + self.model + self.env["PX4_SYS_AUTOSTART"] = "10040" + else: + self.env["PX4_SIM_MODEL"] = "gazebo-classic_" + self.model + 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..5391907008 100644 --- a/test/mavsdk_tests/integration_test_runner/test_runner.py +++ b/test/mavsdk_tests/integration_test_runner/test_runner.py @@ -302,7 +302,25 @@ class Tester: self.active_runners = [] if self.config['mode'] == 'sitl': - if self.config['simulator'] == 'gazebo': + simulator = self.config.get('simulator', 'gazebo') + + if simulator == 'sih': + px4_runner = ph.Px4Runner( + os.getcwd(), + log_dir, + test['model'], + case, + self.get_max_speed_factor(test), + self.debugger, + self.verbose, + self.build_dir, + self.tester_interface.rootfs_base_dirname(), + simulator="sih") + for env_key in test.get('env', []): + px4_runner.env[env_key] = str(test['env'][env_key]) + self.active_runners.append(px4_runner) + + elif simulator == 'gazebo': # Use RegEx to extract worldname.world from case name match = re.search(r'\((.*?\.world)\)', case) if match: diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index b1035b48d7..80eddd83ff 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -136,11 +136,10 @@ def is_everything_ready(config: Dict[str, str], build_dir: str) -> bool: result = False if not os.path.isfile(os.path.join(build_dir, 'bin/px4')): print("PX4 SITL is not built\n" - "run `DONT_RUN=1 " - "make px4_sitl gazebo mavsdk_tests` or " - "`DONT_RUN=1 make px4_sitl_default gazebo mavsdk_tests`") + "run `DONT_RUN=1 make px4_sitl_default mavsdk_tests`" + " or `DONT_RUN=1 make px4_sitl_sih mavsdk_tests`") result = False - if config['simulator'] == 'gazebo': + if config.get('simulator') == 'gazebo': if is_running('gzserver'): print("gzserver process already running\n" "run `killall gzserver` and try again") diff --git a/test/mavsdk_tests/test_ekf2_selector.cpp b/test/mavsdk_tests/test_ekf2_selector.cpp new file mode 100644 index 0000000000..f772ad4b2e --- /dev/null +++ b/test/mavsdk_tests/test_ekf2_selector.cpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_ekf2_selector.cpp + * + * Regression test for EKF2 multi-instance selector bug (#27013). + * + * Reproduces the failure chain from the real flight: + * 1. Inject accel clipping on IMU 0 so EKF0 declares cs_baro_fault + * and its Z state diverges (GPS-only height). + * 2. Move the clipping to IMU 1. Now EKF0 has no filter_fault_flags + * (clipping stopped) but has a diverged Z. EKF1 gets bad_acc_clipping. + * 3. The selector should NOT whipsaw to the diverged EKF0 just because + * EKF1 has a transient clipping fault. + * + * Requires SIH with dual-IMU support (EKF2_MULTI_IMU=2) and per-IMU + * fault injection (SIH_FAULT_IMU / SIH_FAULT_VIBE). + */ + +#include "autopilot_tester.h" + +#include + +TEST_CASE("EKF2 selector - no whipsaw after baro fault and IMU swap", "[ekf2_selector]") +{ + const float takeoff_altitude = 20.f; + const float altitude_tolerance = 5.f; + + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + + tester.set_takeoff_altitude(takeoff_altitude); + tester.store_home(); + tester.sleep_for(std::chrono::seconds(1)); + + // Takeoff and stabilize + tester.arm(); + tester.takeoff(); + tester.wait_until_hovering(); + tester.wait_until_altitude(takeoff_altitude, std::chrono::seconds(30)); + tester.sleep_for(std::chrono::seconds(5)); + + // Phase 1: Clip IMU 0 to force EKF0 into cs_baro_fault. + // The corrupted accel integration causes EKF0's height prediction + // to diverge from baro, triggering baro rejection. Once EKF0 + // switches to GPS-only height, its Z state drifts. + std::cout << time_str() << "Phase 1: clipping IMU 0 to diverge EKF0\n"; + tester.set_param_int("SIH_FAULT_IMU", 1); // 1-indexed: IMU 0 + tester.set_param_float("SIH_FAULT_VIBE", 200.f); + + // Hold long enough for EKF0 to declare baro fault and diverge + tester.sleep_for(std::chrono::seconds(20)); + + // Phase 2: Move clipping from IMU 0 to IMU 1. + // EKF0 stops clipping (filter_fault_flags clears) but retains + // cs_baro_fault with diverged Z. The selector sees EKF0 as + // "healthy" because cs_baro_fault is not in filter_fault_flags. + // EKF1 now has bad_acc_clipping, so the selector marks it unhealthy + // and wants to switch to the "healthy" but diverged EKF0. + std::cout << time_str() << "Phase 2: swapping clipping to IMU 1\n"; + tester.set_param_int("SIH_FAULT_IMU", 2); // 1-indexed: IMU 1 + + // Monitor altitude. If the selector whipsaws to the diverged EKF0, + // altitude will spike well beyond tolerance. + tester.start_checking_altitude(altitude_tolerance); + tester.sleep_for(std::chrono::seconds(30)); + tester.stop_checking_altitude(); + + // Cleanup + std::cout << time_str() << "Removing faults\n"; + tester.set_param_int("SIH_FAULT_IMU", 0); + tester.set_param_float("SIH_FAULT_VIBE", 0.f); + + tester.sleep_for(std::chrono::seconds(5)); + + tester.land(); + tester.wait_until_disarmed(std::chrono::seconds(60)); +}