SIM: refactor sensor-simulation and add faliure injection in SIH

[haumarco]

Solved Problem

When developing and testing autonomous flight systems, failure simulation is crucial for robust system validation. The existing sensor simulation in PX4 was fragmented across multiple individual modules (sensor_gps_sim, sensor_baro_sim, sensor_mag_sim, etc.), making it difficult to implement comprehensive failure injection scenarios. It was also difficult to maintain and extend the simulation capabilities.
Additionally, there was no unified failure injection system for simulated sensors, limiting the ability to test failure modes like GPS altitude faults, sensor drift, and coordinated multi-sensor failures.

Solution

• Unified Sensor Simulation Manager: Refactor multiple individual sensor simulation modules into a single SensorSimManager that coordinates all simulated sensors (GPS, barometer, magnetometer, airspeed, AGP, distance sensor, IMU). SIH is now only responsible for
  the simulation of the equations of motion, all sensor data is produced in SensorSimManager. SimulatorMavlink can still use selected
  sensors which are not covered by gazebo.

• Comprehensive Failure Injection Framework: Add new FailureInjection class that supports:

  • GPS failures: blocked, stuck, wrong data, and drift

  • GPS altitude-specific failures: blocked, stuck, wrong data, and drift

  • IMU failures: accelerometer and gyroscope blocked/stuck per instance

  • Magnetometer failures: blocked/stuck per instance

  • Barometer failures: blocked/stuck

  • Airspeed sensor failures: disconnected, blocked (pitot tube simulation)

  • Visual-Inertial Odometry (VIO) failures: blocked

  • Auxiliary Global Position (AGP) failures: blocked, stuck, drift

  • Distance sensor failures: blocked, stuck, wrong readings

  • Extend VEHICLE_CMD_INJECT_FAILURE support with new failure units (GPS_ALT, AGP) and failure type (DRIFT)

  • Consolidate simulation configuration in startup scripts

  • Keep individual sensor enable/disable parameters for flexibility

  • Better integration with Gazebo and SIH simulation environments

All existing sensor simulation parameters (SENS_EN_GPSSIM, SENS_EN_BAROSIM, etc.) continue to work as before.

Changelog Entry

For release notes:

Feature: Unified sensor simulation manager with comprehensive failure injection
- Refactored individual sensor simulation modules into unified SensorSimManager
- Added extensive failure injection capabilities for all simulated sensors
- Enhanced VEHICLE_CMD_INJECT_FAILURE with GPS altitude and drift failure types
New failure units: FAILURE_UNIT_SENSOR_GPS_ALT, FAILURE_UNIT_SENSOR_AGP
New failure type: FAILURE_TYPE_DRIFT

Testing

Tested in SIH / classic gazebo.
Further testing for each sensor and failure is not completed!

Draft:

Testing and dependent on #25552

[hamishwillee]

When/if this goes in, please update the docs to reflect this - suggest doing it in both the SIH and failure injection docs. Also add a release note in the /releases/main doc.

[dagar]

1. I'm not sure I agree with merging all the super simple little simulation modules. Most of these are pretty simple/crappy sensor models that were intended to fill in as a starting point with the intention that the actual simulator would takeover if you needed something better.
2. Pushing the failure injection downstream would ultimately provide a lot more flexibility in testing across any simulator, real vehicles, and possibly unit tests. It could be build time conditional to fully strip it out of products that don't want it.

[haumarco]

1. Whats the benefit of having this many small modules? We have duplicated code for the SIH, gazebo simulation. And for the SIH case, we wont have an actual simulator

2. Can you explain a bit further what you mean by "pushing downstream"?

[hamishwillee]

Can you explain a bit further what you mean by "pushing downstream"?

I infer @dagar means make the changes to simulate failure independent of the simulation code itself.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: -60 byte (-0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
-0.2% -4.79Ki  [ = ]       0    .debug_abbrev
-0.3%    -440  [ = ]       0    .debug_aranges
-0.2% -1.08Ki  [ = ]       0    .debug_frame
-0.1% -17.7Ki  [ = ]       0    .debug_info
-0.1% -4.65Ki  [ = ]       0    .debug_line
  +400%      +4  [ = ]       0    [Unmapped]
  -0.1% -4.66Ki  [ = ]       0    [section .debug_line]
-0.0%    -313  [ = ]       0    .debug_loclists
-0.2%    -959  [ = ]       0    .debug_rnglists
  [NEW]      +3  [ = ]       0    [Unmapped]
  -0.2%    -962  [ = ]       0    [section .debug_rnglists]
+0.1% +3.28Ki  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
-0.3% -2.09Ki  [ = ]       0    .strtab
 -28.2%     -11  [ = ]       0    ConstLayer::contains()
  [NEW]     +50  [ = ]       0    FailureInjection::check_failure_injections()
  [NEW]     +62  [ = ]       0    FailureInjection::handle_gps_alt_failure()
  [NEW]     +58  [ = ]       0    FailureInjection::handle_gps_failure()
  -3.6%     -63  [ = ]       0    ModuleBase<>
  -3.4%    -162  [ = ]       0    ModuleBase<>::_object
  -3.4%     -81  [ = ]       0    ModuleBase<>::_task_id
  -4.2%     -91  [ = ]       0    ModuleBase<>::lock_module()
  -3.4%     -81  [ = ]       0    ModuleBase<>::main()
  -3.8%     -93  [ = ]       0    ModuleBase<>::print_status()
  -3.4%     -93  [ = ]       0    ModuleBase<>::request_stop()
  -3.7%     -73  [ = ]       0    ModuleBase<>::run()
  -3.4%     -93  [ = ]       0    ModuleBase<>::stop_command()
  -4.2%     -95  [ = ]       0    ModuleBase<>::unlock_module()
  -3.7%    -207  [ = ]       0    ModuleBase<>::~ModuleBase()
  [DEL]     -20  [ = ]       0    SensorBaroSim
  [DEL]     -56  [ = ]       0    SensorBaroSim::Run()
  [DEL]     -23  [ = ]       0    SensorBaroSim::SensorBaroSim()
  [DEL]     -35  [ = ]       0    SensorBaroSim::generate_wgn()
  [DEL]     -39  [ = ]       0    SensorBaroSim::generate_wgn()::S
 -103.6%    -996  [ = ]       0    [70 Others]
-0.4% -2.36Ki  [ = ]       0    .symtab
 -25.0%     -16  [ = ]       0    ActuatorTest::reset()
 -50.0%     -16  [ = ]       0    ConstLayer::contains()
  [NEW]    +304  [ = ]       0    FailureInjection::check_failure_injections()
  [NEW]     +48  [ = ]       0    FailureInjection::handle_gps_alt_failure()
  [NEW]     +48  [ = ]       0    FailureInjection::handle_gps_failure()
  -4.3%     -64  [ = ]       0    ModuleBase<>
  -3.8%    -128  [ = ]       0    ModuleBase<>::_object
  -3.8%     -64  [ = ]       0    ModuleBase<>::_task_id
  -4.5%     -96  [ = ]       0    ModuleBase<>::lock_module()
  -3.8%     -96  [ = ]       0    ModuleBase<>::main()
  -4.3%     -96  [ = ]       0    ModuleBase<>::print_status()
  -3.0%     -32  [ = ]       0    ModuleBase<>::request_stop()
  -5.2%     -96  [ = ]       0    ModuleBase<>::run()
  -3.6%     -96  [ = ]       0    ModuleBase<>::stop_command()
  -4.5%     -96  [ = ]       0    ModuleBase<>::unlock_module()
  -5.2%    -192  [ = ]       0    ModuleBase<>::~ModuleBase()
   +50%     +16  [ = ]       0    PX4Magnetometer::set_device_type()
  [DEL]     -32  [ = ]       0    SensorBaroSim
  [DEL]     -80  [ = ]       0    SensorBaroSim::Run()
  [DEL]     -80  [ = ]       0    SensorBaroSim::SensorBaroSim()
 -108.3% -1.52Ki  [ = ]       0    [89 Others]
+0.4%     +40  [ = ]       0    [Unmapped]
-2.0%      -8  -2.0%      -8    .init_section
-0.4%     -20  -0.4%     -20    .data
  [NEW]      +4  [NEW]      +4    SensorSimManager::generate_wgn()::phase
  [DEL]      -4  [DEL]      -4    SensorBaroSim::generate_wgn()::phase
  [DEL]      -4  [DEL]      -4    SensorGpsSim::generate_wgn()::phase
  [DEL]      -4  [DEL]      -4    SensorMagSim::generate_wgn()::phase
  [DEL]      -4  [DEL]      -4    Sih::generate_wgn()::phase
  -3.8%      -8  -3.8%      -8    ModuleBase<>::_task_id
-0.0%     -32  -0.0%     -32    .text
  [NEW] +1.69Ki  [NEW] +1.69Ki    SensorSimManager::SensorSimManager()
  [NEW] +1.68Ki  [NEW] +1.68Ki    FailureInjection::check_failure_injections()
  +0.8% +1.23Ki  +0.8% +1.23Ki    [section .text]
  [NEW]    +832  [NEW]    +832    SensorSimManager::updateIMU()
  [NEW]    +612  [NEW]    +612    SensorSimManager::updateAirspeed()
  [NEW]    +604  [NEW]    +604    SensorSimManager::updateMagnetometer()
  [NEW]    +576  [NEW]    +576    SensorSimManager::updateGPS()
  [NEW]    +560  [NEW]    +560    FailureInjection::handle_gps_failure()
  [NEW]    +552  [NEW]    +552    SensorSimManager::Run()
  [NEW]    +488  [NEW]    +488    SensorSimManager::updateAGP()
  [NEW]    +404  [NEW]    +404    SensorSimManager::updateBarometer()
  [NEW]    +356  [NEW]    +356    SensorSimManager::updateDistanceSensor()
  -3.7%    -336  -3.7%    -336    ModuleBase<>::stop_command()
  [DEL]    -496  [DEL]    -496    log
  -3.8%    -576  -3.8%    -576    ModuleBase<>::main()
  [DEL]    -648  [DEL]    -648    SensorBaroSim::Run()
  [DEL]    -648  [DEL]    -648    SensorGpsSim::Run()
  [DEL]    -692  [DEL]    -692    Sih::reconstruct_sensors_signals()
  [DEL]    -932  [DEL]    -932    SensorMagSim::Run()
  [DEL] -2.14Ki  [DEL] -2.14Ki    __log_data
 -101.5% -3.14Ki -101.5% -3.14Ki    [81 Others]
-0.1% -31.1Ki  -0.0%     -60    TOTAL

px4_fmu-v6x [Total VM Diff: 40 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +40  +0.0%     +40    .text
  +0.0%     +40  +0.0%     +40    g_cromfs_image
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0% +2.45Ki  [ = ]       0    .debug_info
-0.0%      -4  [ = ]       0    .debug_line
  +300%      +3  [ = ]       0    [Unmapped]
  -0.0%      -7  [ = ]       0    [section .debug_line]
-0.0%      -4  [ = ]       0    .debug_loclists
-0.0%      -1  [ = ]       0    .debug_rnglists
+0.0%    +217  [ = ]       0    .debug_str
-0.8%     -40  [ = ]       0    [Unmapped]
+0.0% +2.71Ki  +0.0%     +40    TOTAL

Updated: 2025-10-03T10:33:09

[github-actions]

This pull request has been automatically closed due to 120 days of inactivity. If you would like to continue, please feel free to reopen it or submit a new PR.

[hamishwillee]

Not sure this one should go stale - even if the implementation isn't quite optimal, the use case is valid.