diff --git a/src/modules/simulation/sensor_gps_sim/CMakeLists.txt b/src/modules/simulation/sensor_gps_sim/CMakeLists.txt index 8f9d64e630..e923bfc530 100644 --- a/src/modules/simulation/sensor_gps_sim/CMakeLists.txt +++ b/src/modules/simulation/sensor_gps_sim/CMakeLists.txt @@ -38,6 +38,8 @@ px4_add_module( SRCS SensorGpsSim.cpp SensorGpsSim.hpp + SensorGpsFailureInjector.cpp + SensorGpsFailureInjector.hpp MODULE_CONFIG parameters.yaml DEPENDS diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.cpp new file mode 100644 index 0000000000..4357daa60e --- /dev/null +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +#include "SensorGpsFailureInjector.hpp" + +#include + +SensorGpsFailureInjector::SensorGpsFailureInjector() +{ + // Cache the handle once — param_find() is a string search and is expensive. + // The value itself is re-read each update() cycle so runtime changes take effect. + _param_sys_failure_en = param_find("SYS_FAILURE_EN"); +} + +void SensorGpsFailureInjector::update() +{ + // Re-read the enable gate every cycle so toggling SYS_FAILURE_EN at runtime + // takes effect immediately without restarting the module. + 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); + + if (!enabled) { return; } + + vehicle_command_s vehicle_command; + + 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, 1 = GPS0, 2 = GPS1 + const int requested_instance = static_cast(lroundf(vehicle_command.param3)); + + // Build a bitmask of which instances to affect directly, without a loop. + // requested_instance=0 → all → 0b11, otherwise select the single bit. + const uint8_t target_mask = (requested_instance == 0) + ? static_cast((1u << GPS_MAX_INSTANCES) - 1u) + : static_cast(1u << (requested_instance - 1)); + + bool supported = false; + + switch (failure_type) { + case vehicle_command_s::FAILURE_TYPE_OK: + supported = true; + PX4_INFO("CMD_INJECT_FAILURE, GPS ok (mask=0x%x)", target_mask); + _gps_blocked_mask &= ~target_mask; + _gps_stuck_mask &= ~target_mask; + _gps_wrong_mask &= ~target_mask; + break; + + case vehicle_command_s::FAILURE_TYPE_OFF: + supported = true; + PX4_INFO("CMD_INJECT_FAILURE, GPS off (mask=0x%x)", target_mask); + _gps_blocked_mask |= target_mask; + break; + + case vehicle_command_s::FAILURE_TYPE_STUCK: + supported = true; + PX4_INFO("CMD_INJECT_FAILURE, GPS stuck (mask=0x%x)", target_mask); + _gps_stuck_mask |= target_mask; + break; + + case vehicle_command_s::FAILURE_TYPE_WRONG: + supported = true; + PX4_INFO("CMD_INJECT_FAILURE, GPS wrong (mask=0x%x)", target_mask); + _gps_wrong_mask |= target_mask; + break; + + default: + break; + } + + 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); + } +} diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.hpp new file mode 100644 index 0000000000..06f5e78324 --- /dev/null +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * GPS failure injector for SITL simulation. + * + * Listens for VEHICLE_CMD_INJECT_FAILURE commands targeting FAILURE_UNIT_SENSOR_GPS + * and maintains per-instance state bitmasks that SensorGpsSim consults before + * each publish to suppress or corrupt GPS data. + * + * Mirrors the design of FailureInjector (motor failures) in commander. + * Requires SYS_FAILURE_EN = 1. + * + * Instance mapping (param3): + * 0 = all instances + * 1 = GPS instance 0 (primary) + * 2 = GPS instance 1 (secondary) + */ +class SensorGpsFailureInjector +{ +public: + static constexpr int GPS_MAX_INSTANCES = 2; + + SensorGpsFailureInjector(); + + /** + * Poll vehicle_command and update failure state bitmasks. + * Must be called once per SensorGpsSim::Run() cycle. + */ + void update(); + + /** Returns true if the given GPS instance should stop publishing (FAILURE_TYPE_OFF). */ + bool isBlocked(int instance) const { return (_gps_blocked_mask >> instance) & 1u; } + + /** Returns true if the given GPS instance should freeze on its last position (FAILURE_TYPE_STUCK). */ + bool isStuck(int instance) const { return (_gps_stuck_mask >> instance) & 1u; } + + /** Returns true if the given GPS instance should publish corrupted position data (FAILURE_TYPE_WRONG). */ + bool isWrong(int instance) const { return (_gps_wrong_mask >> instance) & 1u; } + +private: + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + // Cached handle from param_find() — avoids repeated string searches. + // The actual value is re-read in update() so runtime changes take effect. + param_t _param_sys_failure_en{PARAM_INVALID}; + + uint8_t _gps_blocked_mask{0}; ///< FAILURE_TYPE_OFF — stop publishing + uint8_t _gps_stuck_mask{0}; ///< FAILURE_TYPE_STUCK — freeze last position + uint8_t _gps_wrong_mask{0}; ///< FAILURE_TYPE_WRONG — corrupt position +}; diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index bf6c28a101..0906eca337 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -108,6 +108,8 @@ void SensorGpsSim::Run() updateParams(); } + _failure_injector.update(); + if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()) { vehicle_local_position_s lpos{}; @@ -196,17 +198,53 @@ 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); + // --- GPS instance 0 (primary) --- + if (!_failure_injector.isBlocked(0)) { + if (_failure_injector.isStuck(0)) { + // Replay the last known position with a fresh timestamp so the + // consumer sees live-looking data whose position never changes. + _last_gps0.timestamp = hrt_absolute_time(); + _sensor_gps_pub.publish(_last_gps0); + } else { + if (_failure_injector.isWrong(0)) { + // Corrupt position by +1 degree lat/lon (~111 km offset). + // GPS0 and GPS1 will now diverge well beyond the redundancy gate. + sensor_gps.latitude_deg += 1.0; + sensor_gps.longitude_deg += 1.0; + } + + sensor_gps.timestamp = hrt_absolute_time(); + _last_gps0 = sensor_gps; + _sensor_gps_pub.publish(sensor_gps); + } + } + + // --- GPS instance 1 (secondary) --- 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) { - 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); + if (!_failure_injector.isBlocked(1)) { + sensor_gps_s gps1 = sensor_gps; + 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); + + if (_failure_injector.isStuck(1)) { + _last_gps1.timestamp = hrt_absolute_time(); + _sensor_gps_pub2.publish(_last_gps1); + + } else { + if (_failure_injector.isWrong(1)) { + gps1.latitude_deg += 1.0; + gps1.longitude_deg += 1.0; + } + + gps1.timestamp = hrt_absolute_time(); + _last_gps1 = gps1; + _sensor_gps_pub2.publish(gps1); + } + } } } diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index 0a55f5a9ed..fc7b74db67 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -33,6 +33,8 @@ #pragma once +#include "SensorGpsFailureInjector.hpp" + #include #include #include @@ -85,6 +87,12 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + SensorGpsFailureInjector _failure_injector{}; + + // Last good sensor_gps snapshots, used to replay frozen data in FAILURE_TYPE_STUCK + 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};