From 4701d1bee8ee606237de8a21759fbcfb999d7860 Mon Sep 17 00:00:00 2001 From: gguidone Date: Fri, 10 Apr 2026 13:58:42 +0200 Subject: [PATCH] feat(sensor_gps_sim): add GPS failure injection via VEHICLE_CMD_INJECT_FAILURE Implements FAILURE_UNIT_SENSOR_GPS support in the SITL sensor_gps_sim module, mirroring the existing motor failure injection pattern in FailureInjector. Supported failure types per GPS instance (param3=0 all, 1=GPS0, 2=GPS1): - FAILURE_TYPE_OFF, stop publishing (simulates dead receiver) - FAILURE_TYPE_STUCK, freeze last known position (simulates frozen fix) - FAILURE_TYPE_WRONG, corrupt position by +1 deg (~111 km offset, triggers gpsRedundancyCheck divergence gate) - FAILURE_TYPE_OK, recover all active failures for that instance Requires SYS_FAILURE_EN=1. The enable gate is re-read every cycle so runtime parameter changes take effect without restarting the module. --- .../sensor_gps_sim/SensorGpsSim.cpp | 141 ++++++++++++++++-- .../sensor_gps_sim/SensorGpsSim.hpp | 26 ++++ 2 files changed, 158 insertions(+), 9 deletions(-) diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index d5d9925eb42..7a1f9b691f8 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -45,6 +45,7 @@ SensorGpsSim::SensorGpsSim() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { + _param_sys_failure_en = param_find("SYS_FAILURE_EN"); } SensorGpsSim::~SensorGpsSim() @@ -108,6 +109,8 @@ void SensorGpsSim::Run() updateParams(); } + check_failure_injection(); + if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()) { vehicle_local_position_s lpos{}; @@ -196,27 +199,147 @@ void SensorGpsSim::Run() sensor_gps.vel_ned_valid = true; sensor_gps.satellites_used = _sim_gps_used.get(); - sensor_gps.timestamp = hrt_absolute_time(); - _sensor_gps_pub.publish(sensor_gps); + publishWithFailures(0, sensor_gps, _last_gps0, _sensor_gps_pub); const float gps1_offx = _param_gps1_offx.get(); const float gps1_offy = _param_gps1_offy.get(); if (fabsf(gps1_offx) > 0.f || fabsf(gps1_offy) > 0.f) { - // Make instance 1 look like a physically distinct receiver - device_id.devid_s.address = 1; - sensor_gps.device_id = device_id.devid; + sensor_gps_s gps1 = sensor_gps; - sensor_gps.latitude_deg = latitude + (double)gps1_offx / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI); - sensor_gps.longitude_deg = longitude + (double)gps1_offy / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI) / cos(latitude * M_PI / 180.0); - sensor_gps.timestamp = hrt_absolute_time(); - _sensor_gps_pub2.publish(sensor_gps); + device_id.devid_s.address = 1; + gps1.device_id = device_id.devid; + + gps1.latitude_deg = latitude + (double)gps1_offx / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI); + gps1.longitude_deg = longitude + (double)gps1_offy / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI) / cos(latitude * M_PI / 180.0); + + publishWithFailures(1, gps1, _last_gps1, _sensor_gps_pub2); } } perf_end(_loop_perf); } +void SensorGpsSim::publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot, + uORB::PublicationMulti &pub) +{ + // Precedence when multiple failure masks are set: BLOCKED > STUCK > WRONG. + if (!isBlocked(instance)) { + if (isStuck(instance)) { + snapshot.timestamp = hrt_absolute_time(); + pub.publish(snapshot); + + } else { + if (isWrong(instance)) { + gps.latitude_deg += 1.0; + gps.longitude_deg += 1.0; + } + + gps.timestamp = hrt_absolute_time(); + snapshot = gps; + pub.publish(gps); + } + } +} + +void SensorGpsSim::check_failure_injection() +{ + int32_t sys_failure_en = 0; + const bool enabled = _param_sys_failure_en != PARAM_INVALID + && param_get(_param_sys_failure_en, &sys_failure_en) == PX4_OK + && sys_failure_en == 1; + + vehicle_command_s vehicle_command; + + if (!enabled) { + // Clear any active failures so disabling the param at runtime + // stops injection instead of latching the previous state. + _gps_blocked_mask = 0; + _gps_stuck_mask = 0; + _gps_wrong_mask = 0; + + // Drain queued commands so re-enabling doesn't replay them. + while (_vehicle_command_sub.update(&vehicle_command)) {} + + return; + } + + while (_vehicle_command_sub.update(&vehicle_command)) { + const int failure_unit = static_cast(lroundf(vehicle_command.param1)); + const int failure_type = static_cast(lroundf(vehicle_command.param2)); + + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE + || failure_unit != vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) { + continue; + } + + // param3: 0 = all instances, otherwise 1-based instance index + const int requested_instance = static_cast(lroundf(vehicle_command.param3)); + + if (requested_instance < 0 || requested_instance > GPS_MAX_INSTANCES) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + continue; + } + + const uint8_t target_mask = (requested_instance == 0) + ? static_cast((1u << GPS_MAX_INSTANCES) - 1u) + : static_cast(1u << (requested_instance - 1)); + + bool supported = true; + const char *action = nullptr; + + switch (failure_type) { + case vehicle_command_s::FAILURE_TYPE_OK: + _gps_blocked_mask &= ~target_mask; + _gps_stuck_mask &= ~target_mask; + _gps_wrong_mask &= ~target_mask; + action = "ok"; + break; + + case vehicle_command_s::FAILURE_TYPE_OFF: + _gps_blocked_mask |= target_mask; + action = "off"; + break; + + case vehicle_command_s::FAILURE_TYPE_STUCK: + _gps_stuck_mask |= target_mask; + action = "stuck"; + break; + + case vehicle_command_s::FAILURE_TYPE_WRONG: + _gps_wrong_mask |= target_mask; + action = "wrong"; + break; + + default: + supported = false; + break; + } + + if (action != nullptr) { + for (int i = 0; i < GPS_MAX_INSTANCES; i++) { + if (target_mask & (1u << i)) { + PX4_INFO("CMD_INJECT_FAILURE, GPS %d %s", i + 1, action); + } + } + } + + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } +} + int SensorGpsSim::task_spawn(int argc, char *argv[]) { SensorGpsSim *instance = new SensorGpsSim(); diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index 4c96805565f..5b9d543aeb1 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -34,15 +34,19 @@ #pragma once #include +#include #include #include #include #include +#include #include #include #include #include #include +#include +#include #include #include @@ -68,8 +72,19 @@ public: bool init(); private: + static constexpr int GPS_MAX_INSTANCES = 2; + void Run() override; + void check_failure_injection(); + + void publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot, + uORB::PublicationMulti &pub); + + bool isBlocked(int instance) const { return (_gps_blocked_mask >> instance) & 1u; } + bool isStuck(int instance) const { return (_gps_stuck_mask >> instance) & 1u; } + bool isWrong(int instance) const { return (_gps_wrong_mask >> instance) & 1u; } + // generate white Gaussian noise sample with std=1 static float generate_wgn(); @@ -79,12 +94,23 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; uORB::PublicationMulti _sensor_gps_pub2{ORB_ID(sensor_gps)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + param_t _param_sys_failure_en{PARAM_INVALID}; + + uint8_t _gps_blocked_mask{0}; + uint8_t _gps_stuck_mask{0}; + uint8_t _gps_wrong_mask{0}; + + sensor_gps_s _last_gps0{}; + sensor_gps_s _last_gps1{}; + // GPS Markov process noise state float _gps_pos_noise_n{0.0f}; float _gps_pos_noise_e{0.0f};