Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-09 22:08:56 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

test(ekf2): add SIH SITL test for EKF2 selector regression

Browse Source 
Adds a two-phase test that reproduces the EKF2 selector whipsaw from
#27013:

Phase 1: Inject accel clipping on IMU 0 so EKF0 declares cs_baro_fault
and its Z state diverges from baro (switches to GPS-only height).

Phase 2: Move clipping to IMU 1. EKF0 now has no filter_fault_flags
but retains diverged Z. EKF1 gets bad_acc_clipping. The selector falls
back to the "healthy" but diverged EKF0, causing altitude spikes.

Verified locally: test fails with 9.3m altitude deviation (5m tolerance),
confirming the selector bug is reproducible in SIH SITL.

Also adds dual magnetometer to sensor_mag_sim (matching v6c sensor
topology), SIH test runner, CI workflow, and set_param_float helper.

Signed-off-by: Ramon Roche <mrpollo@gmail.com>
This commit is contained in:
Ramon Roche
2026-04-13 08:12:39 -07:00
parent 20802d509a
commit 150094a592
15 changed files with 310 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/actions/build-sitl-sih/action.yml
+21
View File
@@ -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
.github/actions/setup-mavsdk/action.yml
+13
View File
@@ -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"
.github/workflows/sitl_tests_sih.yml
+66
View File
@@ -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
cmake/mavsdk_tests.cmake
+30
View File
@@ -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 <sitl_target> 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
)
platforms/posix/CMakeLists.txt
+2
View File
@@ -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 $<TARGET_FILE:px4>
src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp
+11 -7
View File
@@ -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());
}
}
}
src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp
+5 -1
View File
@@ -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};
src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake
+1 -11
View File
@@ -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()
test/mavsdk_tests/CMakeLists.txt
+1
View File
@@ -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
)
test/mavsdk_tests/autopilot_tester.h
+5
View File
@@ -164,6 +164,11 @@ public:
CHECK(_param->set_param_int(param, value) == Param::Result::Success);
}
void set_param_float(const std::string &param, float value)
{
CHECK(_param->set_param_float(param, value) == Param::Result::Success);
}
template<typename Rep, typename Period>
void sleep_for(std::chrono::duration<Rep, Period> duration)
{
test/mavsdk_tests/configs/sih.json
+14
View File
@@ -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
}
]
}
test/mavsdk_tests/integration_test_runner/process_helper.py
+9 -2
View File
@@ -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()
test/mavsdk_tests/integration_test_runner/test_runner.py
+19 -1
View File
@@ -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:
test/mavsdk_tests/mavsdk_test_runner.py
+3 -4
View File
@@ -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")
test/mavsdk_tests/test_ekf2_selector.cpp
+110
View File
@@ -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 <iostream>
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));
}