feat(sensor_gps_sim): add GPS failure injection via VEHICLE_CMD_INJEC…

[gguidone]

Solved Problem

PX4's SITL has no GPS failure injection.

Solution

Add a SensorGpsFailureInjector in sensor_gps_sim that handles VEHICLE_CMD_INJECT_FAILURE with failure_unit == FAILURE_UNIT_SENSOR_GPS and exposes per-instance failure flags. Every publish is routed through a publishWithFailures() helper that applies one of:

• FAILURE_TYPE_OFF, suppress publish (dead receiver)
• FAILURE_TYPE_STUCK, republish last sample with refreshed timestamp (frozen fix)
• FAILURE_TYPE_WRONG, +1° lat/lon offset (~111 km) to trip gpsRedundancyCheck's divergence gate
• FAILURE_TYPE_OK, clear active failures for the targeted instance(s)

param3 targets instances (0 = all, 1 = GPS0, 2 = GPS1), matching the motor injector. Gated by SYS_FAILURE_EN, re-read every cycle so toggling at runtime takes effect immediately.

The implementation mirrors src/modules/commander/failure_detector/FailureInjector.{cpp,hpp}.

Alternatives

• Implement for GPS real hardware directly deferred. A follow-up PR will extend the same injector to the real driver.
• Configurable WRONG offset in meters, not in this PR, coming in the one on real hardware.

Test coverage

Manual SITL testing.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 728 byte (0.04 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +728  +0.0%    +728    .text
    [NEW]    +344  [NEW]    +344    SensorGpsFailureInjector::update()
    [NEW]    +120  [NEW]    +120    SensorGpsSim::publishWithFailures()
    [NEW]     +64  [NEW]     +64    SensorGpsFailureInjector::SensorGpsFailureInjector()
    +7.2%     +64  +7.2%     +64    SensorGpsSim::Run()
    +3.2%     +44  +3.2%     +44    uORB::Publication<>::publish()
     +12%     +36   +12%     +36    SensorGpsSim::SensorGpsSim()
    +0.0%     +36  +0.0%     +36    [section .text]
     +11%     +16   +11%     +16    SensorGpsSim::~SensorGpsSim()
    +1.2%      +4  +1.2%      +4    Sih::~Sih()
     +44%      +4   +44%      +4    g_nullstring
    -1.8%      -4  -1.8%      -4    SensorGpsSim::generate_wgn()
  +0.1% +1.74Ki  [ = ]       0    .debug_abbrev
  +0.0%     +56  [ = ]       0    .debug_aranges
  +0.0%    +192  [ = ]       0    .debug_frame
  +0.1% +18.5Ki  [ = ]       0    .debug_info
  +0.0% +2.29Ki  [ = ]       0    .debug_line
   -33.3%      -1  [ = ]       0    [Unmapped]
    +0.0% +2.29Ki  [ = ]       0    [section .debug_line]
  +0.0% +1.11Ki  [ = ]       0    .debug_loclists
  +0.0%    +122  [ = ]       0    .debug_rnglists
   -50.0%      -1  [ = ]       0    [Unmapped]
    +0.0%    +123  [ = ]       0    [section .debug_rnglists]
  +0.0%    +732  [ = ]       0    .debug_str
  -0.8%      -2  [ = ]       0    .shstrtab
  +0.0%    +206  [ = ]       0    .strtab
    [NEW]     +68  [ = ]       0    SensorGpsFailureInjector::SensorGpsFailureInjector()
    [NEW]     +39  [ = ]       0    SensorGpsFailureInjector::update()
    [NEW]     +90  [ = ]       0    SensorGpsSim::publishWithFailures()
    -0.0%      -2  [ = ]       0    [section .strtab]
   -26.7%     -16  [ = ]       0    ___ZL19param_get_cplusplustPf.isra.0_veneer
     +64%     +16  [ = ]       0    __nxsig_timedwait_veneer
    +0.3%     +11  [ = ]       0    do_not_explicitly_use_this_namespace::Param<>::update()
  +0.0%    +240  [ = ]       0    .symtab
    [NEW]     +64  [ = ]       0    SensorGpsFailureInjector::SensorGpsFailureInjector()
    [NEW]    +112  [ = ]       0    SensorGpsFailureInjector::update()
    +100%     +32  [ = ]       0    SensorGpsSim::generate_wgn()
   -33.3%     -16  [ = ]       0    SensorGpsSim::generate_wgn()::S
    [NEW]     +16  [ = ]       0    SensorGpsSim::publishWithFailures()
   -14.3%     -32  [ = ]       0    ___ZL19param_get_cplusplustPf.isra.0_veneer
     +67%     +32  [ = ]       0    __nxsig_timedwait_veneer
    +2.9%     +32  [ = ]       0    uORB::Publication<>::publish()
  -6.1%    -728  [ = ]       0    [Unmapped]
  +0.1% +25.1Ki  +0.0%    +728    TOTAL

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

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +55  [ = ]       0    .debug_abbrev
  -0.0%      -2  [ = ]       0    .debug_info
  -0.0%      -5  [ = ]       0    .debug_line
    [NEW]      +2  [ = ]       0    [Unmapped]
    -0.0%      -7  [ = ]       0    [section .debug_line]
  +0.0%     +48  [ = ]       0    TOTAL

Updated: 2026-05-19T12:42:29

[hamishwillee]

Just remember to update https://docs.px4.io/main/en/debug/failure_injection#system-failure-injection with information about what is supported both on SIM and real hardware please.