From f937bd5818cadc5b588727ae60a519e0059f700a Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 30 Jun 2026 15:33:39 +0200 Subject: [PATCH] refactor(failure_injection): centralize MAV_CMD_INJECT_FAILURE behind a manager module (#27572) * feat(failure_injection): add failure_injection topic and helper library * feat(failure_injection): add failure injection manager module * refactor(commander): route motor failure injection through the manager * refactor(simulation): route sensor sim failure injection through the manager * refactor(simulation): route SimulatorMavlink failure injection through the manager * feat(failure_injection): start the manager in the SITL startup * docs(failure_injection): document per-simulator failure support * feat(failure_injection): enhance failure injection management and configuration * fix(rebase): baro failure injectionb * refactor(failure_injection): simplify FailureInjectionManager and FailureTable implementations * refactor(failure_injection): replace Subscriber with Config in various modules and add update method * update(uorb): FailureInjection.msg to docs standard * docs(docs): subedit * fix(failure_injection): correct comment formatting in FailureInjection.msg * feat(failure_injection): add failure injection manager support across multiple boards and modules * feat(mavlink): implement failure injection functionality --------- Co-authored-by: Claudio Chies Co-authored-by: Hamish Willee --- ROMFS/px4fmu_common/init.d-posix/rcS | 5 + boards/corvon/743v1/default.px4board | 1 + boards/micoair/h743-aio/default.px4board | 1 + boards/micoair/h743-lite/default.px4board | 1 + boards/micoair/h743-v2/default.px4board | 1 + boards/micoair/h743/default.px4board | 1 + boards/px4/sitl/default.px4board | 1 + boards/px4/sitl/neural.px4board | 1 + boards/px4/sitl/raptor.px4board | 1 + boards/px4/sitl/vtest-moving.px4board | 1 + cmake/px4_add_gtest.cmake | 4 +- docs/en/debug/failure_injection.md | 26 +- msg/CMakeLists.txt | 1 + msg/FailureInjection.msg | 45 +++ src/lib/CMakeLists.txt | 1 + src/lib/failure_injection/CMakeLists.txt | 34 ++ .../failure_injection/FailureInjection.cpp | 79 ++++ .../failure_injection/FailureInjection.hpp | 149 ++++++++ src/lib/systemlib/system_params.yaml | 9 - .../commander/failure_detector/CMakeLists.txt | 1 + .../failure_detector/FailureInjector.cpp | 93 ++--- .../failure_detector/FailureInjector.hpp | 15 +- .../failure_injection_manager/CMakeLists.txt | 49 +++ .../FailureInjectionConfigTest.cpp | 227 ++++++++++++ .../FailureInjectionManager.cpp | 181 +++++++++ .../FailureInjectionManager.hpp | 87 +++++ .../FailureInjectionManagerTest.cpp | 198 ++++++++++ .../FailureTable.cpp | 207 +++++++++++ .../FailureTable.hpp | 108 ++++++ src/modules/failure_injection_manager/Kconfig | 7 + .../failure_injection_manager_params.yaml | 14 + src/modules/mavlink/mavlink_main.h | 15 +- .../battery_simulator/BatterySimulator.cpp | 58 +-- .../battery_simulator/BatterySimulator.hpp | 9 +- .../battery_simulator/CMakeLists.txt | 1 + .../sensor_airspeed_sim/CMakeLists.txt | 1 + .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 64 +--- .../sensor_airspeed_sim/SensorAirspeedSim.hpp | 10 +- .../simulation/sensor_gps_sim/CMakeLists.txt | 1 + .../sensor_gps_sim/SensorGpsSim.cpp | 81 +--- .../sensor_gps_sim/SensorGpsSim.hpp | 23 +- .../simulator_mavlink/CMakeLists.txt | 1 + .../simulator_mavlink/SimulatorMavlink.cpp | 349 +++--------------- .../simulator_mavlink/SimulatorMavlink.hpp | 11 +- src/systemcmds/failure/Kconfig | 1 + 45 files changed, 1561 insertions(+), 613 deletions(-) create mode 100644 msg/FailureInjection.msg create mode 100644 src/lib/failure_injection/CMakeLists.txt create mode 100644 src/lib/failure_injection/FailureInjection.cpp create mode 100644 src/lib/failure_injection/FailureInjection.hpp create mode 100644 src/modules/failure_injection_manager/CMakeLists.txt create mode 100644 src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp create mode 100644 src/modules/failure_injection_manager/FailureInjectionManager.cpp create mode 100644 src/modules/failure_injection_manager/FailureInjectionManager.hpp create mode 100644 src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp create mode 100644 src/modules/failure_injection_manager/FailureTable.cpp create mode 100644 src/modules/failure_injection_manager/FailureTable.hpp create mode 100644 src/modules/failure_injection_manager/Kconfig create mode 100644 src/modules/failure_injection_manager/failure_injection_manager_params.yaml diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 8ce56eb640..82f916710b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -245,6 +245,11 @@ fi load_mon start +if param compare SYS_FAILURE_EN 1 +then + failure_injection_manager start +fi + if param greater SIM_BAT_DRAIN 0 then battery_simulator start diff --git a/boards/corvon/743v1/default.px4board b/boards/corvon/743v1/default.px4board index 109e7e788a..e71df67a64 100644 --- a/boards/corvon/743v1/default.px4board +++ b/boards/corvon/743v1/default.px4board @@ -35,6 +35,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board index 7ddff044b7..5d39ad3ca0 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -57,6 +57,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/micoair/h743-lite/default.px4board b/boards/micoair/h743-lite/default.px4board index 9d8999b053..905d317bae 100644 --- a/boards/micoair/h743-lite/default.px4board +++ b/boards/micoair/h743-lite/default.px4board @@ -49,6 +49,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index d2da73f08b..e731c3f273 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -51,6 +51,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board index 87117d8206..dfc0310b87 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -54,6 +54,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 3e2edf7ea2..a12a562a08 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -17,6 +17,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/sitl/neural.px4board b/boards/px4/sitl/neural.px4board index e81a14e5ed..61f510ed64 100644 --- a/boards/px4/sitl/neural.px4board +++ b/boards/px4/sitl/neural.px4board @@ -17,6 +17,7 @@ CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/sitl/raptor.px4board b/boards/px4/sitl/raptor.px4board index fbfbda84ae..2c1a1c7d77 100644 --- a/boards/px4/sitl/raptor.px4board +++ b/boards/px4/sitl/raptor.px4board @@ -29,6 +29,7 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/boards/px4/sitl/vtest-moving.px4board b/boards/px4/sitl/vtest-moving.px4board index cf43b2efeb..26f390bca1 100644 --- a/boards/px4/sitl/vtest-moving.px4board +++ b/boards/px4/sitl/vtest-moving.px4board @@ -17,6 +17,7 @@ CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FAILURE_INJECTION_MANAGER=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y diff --git a/cmake/px4_add_gtest.cmake b/cmake/px4_add_gtest.cmake index 2cb605e061..647e46dfa6 100644 --- a/cmake/px4_add_gtest.cmake +++ b/cmake/px4_add_gtest.cmake @@ -123,7 +123,9 @@ function(px4_add_functional_gtest) uorb_msgs fuzztest::fuzztest # Do not use link_fuzztest() here because that # also links to fuzztest_gtest_main - test_stubs) # put test_stubs last + # whole-archive so the daemon app-map stubs link even for small + # test libs, where link order would otherwise drop them + -Wl,--whole-archive test_stubs -Wl,--no-whole-archive) target_compile_definitions(${TESTNAME} PRIVATE MODULE_NAME="${TESTNAME}") diff --git a/docs/en/debug/failure_injection.md b/docs/en/debug/failure_injection.md index 5c2b02c9c9..c470cfbb6b 100644 --- a/docs/en/debug/failure_injection.md +++ b/docs/en/debug/failure_injection.md @@ -5,16 +5,14 @@ This enables easier testing of [safety failsafe](../config/safety.md) behaviour, Failure injection is disabled by default, and can be enabled using the [SYS_FAILURE_EN](../advanced_config/parameter_reference.md#SYS_FAILURE_EN) parameter. -:::warning -Failure injection still in development. -At time of writing (PX4 v1.14): +Failure injection must also be be supported by the current simulator, and the set of supported failures is simulator-dependent. -- Support may vary by failure type and between simulatiors and real vehicle. -- It requires support in the simulator. - It is supported in Gazebo Classic -- Many failure types are not broadly implemented. - In those cases the command will return with an "unsupported" message. +::: info +PX4 may accept a command to set a particular failure mode even it that mode is not supported by your simulator. +All [MAV_CMD_INJECT_FAILURE](https://mavlink.io/en/messages/common.html#MAV_CMD_INJECT_FAILURE) commands are handled internally by the failure-injection module, which acknowledges each command and republishes the active failures for the sensor/actuator simulators to apply. +The failure-injection module will NACK the command with [MAV_RESULT_UNSUPPORTED](https://mavlink.io/en/messages/common.html#MAV_RESULT_UNSUPPORTED) for failure combinations that are not implemented by PX4 or any simulator. +However it the module will accept (respond with [MAV_MISSION_ACCEPTED](https://mavlink.io/en/messages/common.html#MAV_MISSION_ACCEPTED)) for any other failure-type, even if it is not supported by your _particular_ simulator. ::: ## Failure System Command @@ -72,19 +70,19 @@ It is used in [PX4 Integration Testing](../test_and_ci/integration_testing_mavsd The plugin API is a direct mapping of the failure command shown above, with a few additional error signals related to the connection. -## Example: RC signal +## Example: GPS -To simulate losing RC signal without having to turn off your RC controller: +To test the GPS failsafe by stopping GPS: 1. Enable the [SYS_FAILURE_EN](../advanced_config/parameter_reference.md#SYS_FAILURE_EN) parameter. 2. Enter the following commands on the MAVLink console or SITL _pxh shell_: ```sh - # Fail RC (turn publishing off) - failure rc_signal off + # Stop GPS publishing + failure gps off - # Restart RC publishing - failure rc_signal ok + # Restart GPS publishing + failure gps ok ``` ## Example: Motor diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 77c3f75e57..5600608494 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -91,6 +91,7 @@ set(msg_files FigureEightStatus.msg FailsafeFlags.msg FailureDetectorStatus.msg + FailureInjection.msg FlightPhaseEstimation.msg FollowTarget.msg FollowTargetEstimator.msg diff --git a/msg/FailureInjection.msg b/msg/FailureInjection.msg new file mode 100644 index 0000000000..d749730823 --- /dev/null +++ b/msg/FailureInjection.msg @@ -0,0 +1,45 @@ +#Failure injection configuration. +# +# Currently active failure-injection configuration, published by the failure +# injection manager (the sole subscriber to vehicle_command INJECT_FAILURE). +# Republished only when the configuration changes, so command spam on +# vehicle_command cannot propagate to the consumers applying the failures. + +uint64 timestamp # [us] Time since system start + +uint8 MAX_FAILURES = 4 # maximum number of simultaneous failures + +uint8 count # number of valid entries in the arrays below + +uint8[4] unit # [@enum FAILURE_UNIT] Affected component per entry +# Failure unit (affected component). Mirrors MAVLink FAILURE_UNIT and the +# FAILURE_UNIT_* values in vehicle_command. +uint8 FAILURE_UNIT_SENSOR_GYRO = 0 +uint8 FAILURE_UNIT_SENSOR_ACCEL = 1 +uint8 FAILURE_UNIT_SENSOR_MAG = 2 +uint8 FAILURE_UNIT_SENSOR_BARO = 3 +uint8 FAILURE_UNIT_SENSOR_GPS = 4 +uint8 FAILURE_UNIT_SENSOR_OPTICAL_FLOW = 5 +uint8 FAILURE_UNIT_SENSOR_VIO = 6 +uint8 FAILURE_UNIT_SENSOR_DISTANCE_SENSOR = 7 +uint8 FAILURE_UNIT_SENSOR_AIRSPEED = 8 +uint8 FAILURE_UNIT_SYSTEM_BATTERY = 100 +uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101 +uint8 FAILURE_UNIT_SYSTEM_SERVO = 102 +uint8 FAILURE_UNIT_SYSTEM_AVOIDANCE = 103 +uint8 FAILURE_UNIT_SYSTEM_RC_SIGNAL = 104 +uint8 FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL = 105 + +uint16[4] instance_mask # Bit i targets instance (i+1); 0xFFFF = all instances + +uint8[4] failure_type # [@enum FAILURE_TYPE] failure mode per entry +# Failure mode. +# Mirrors MAVLink FAILURE_TYPE and the FAILURE_TYPE_* values in vehicle_command. +uint8 FAILURE_TYPE_OK = 0 +uint8 FAILURE_TYPE_OFF = 1 +uint8 FAILURE_TYPE_STUCK = 2 +uint8 FAILURE_TYPE_GARBAGE = 3 +uint8 FAILURE_TYPE_WRONG = 4 +uint8 FAILURE_TYPE_SLOW = 5 +uint8 FAILURE_TYPE_DELAYED = 6 +uint8 FAILURE_TYPE_INTERMITTENT = 7 diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 23e971064d..8caf918504 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -49,6 +49,7 @@ add_subdirectory(crc EXCLUDE_FROM_ALL) add_subdirectory(crypto EXCLUDE_FROM_ALL) add_subdirectory(dataman_client EXCLUDE_FROM_ALL) add_subdirectory(drivers EXCLUDE_FROM_ALL) +add_subdirectory(failure_injection EXCLUDE_FROM_ALL) add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL) add_subdirectory(geo EXCLUDE_FROM_ALL) add_subdirectory(gnss EXCLUDE_FROM_ALL) diff --git a/src/lib/failure_injection/CMakeLists.txt b/src/lib/failure_injection/CMakeLists.txt new file mode 100644 index 0000000000..91bf985769 --- /dev/null +++ b/src/lib/failure_injection/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(failure_injection FailureInjection.cpp) diff --git a/src/lib/failure_injection/FailureInjection.cpp b/src/lib/failure_injection/FailureInjection.cpp new file mode 100644 index 0000000000..4763b19c8b --- /dev/null +++ b/src/lib/failure_injection/FailureInjection.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "FailureInjection.hpp" + +namespace failure_injection +{ + +bool Config::update() +{ + failure_injection_s cfg; + + if (_sub.update(&cfg)) { + set(cfg); + return true; + } + + return false; +} + +void Config::set(const failure_injection_s &cfg) +{ + _count = (cfg.count <= failure_injection_s::MAX_FAILURES) ? cfg.count : failure_injection_s::MAX_FAILURES; + + for (uint8_t i = 0; i < _count; i++) { + _unit[i] = cfg.unit[i]; + _instance_mask[i] = cfg.instance_mask[i]; + _failure_type[i] = static_cast(cfg.failure_type[i]); + } +} + +Mode Config::mode(uint8_t unit, uint8_t instance) const +{ + for (uint8_t i = 0; i < _count; i++) { + if (_unit[i] != unit) { + continue; + } + + // instance == 0 matches any instance of the unit; otherwise match the + // 1-based instance against the bitmask (0xFFFF covers all instances). + if (instance == 0 || (_instance_mask[i] & (1u << (instance - 1)))) { + return _failure_type[i]; + } + } + + return Mode::Ok; +} + +} // namespace failure_injection diff --git a/src/lib/failure_injection/FailureInjection.hpp b/src/lib/failure_injection/FailureInjection.hpp new file mode 100644 index 0000000000..d0546fb05f --- /dev/null +++ b/src/lib/failure_injection/FailureInjection.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * 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 FailureInjection.hpp + * + * Shared helper for applying failure injection where a message is produced. + * + * The failure_injection manager publishes the deduplicated failure_injection + * topic. Each producer caches it in a Config (update() per loop), looks up the + * Mode for its (unit, instance), and applies it: process() covers the + * generic Off (suppress) / Stuck (replay last sample) for single-message + * producers; value-mutating and multi-instance cases are driver-specific. + */ + +#pragma once + +#include + +#include +#include + +namespace failure_injection +{ + +enum class Mode : uint8_t { + Ok = failure_injection_s::FAILURE_TYPE_OK, + Off = failure_injection_s::FAILURE_TYPE_OFF, + Stuck = failure_injection_s::FAILURE_TYPE_STUCK, + Garbage = failure_injection_s::FAILURE_TYPE_GARBAGE, + Wrong = failure_injection_s::FAILURE_TYPE_WRONG, + Slow = failure_injection_s::FAILURE_TYPE_SLOW, + Delayed = failure_injection_s::FAILURE_TYPE_DELAYED, + Intermittent = failure_injection_s::FAILURE_TYPE_INTERMITTENT, +}; + +class Config +{ +public: + /** + * Poll the failure_injection subscription and, when a new failure_injection + * message arrives, refresh the cached failure config that mode() / any_active() + * read. Call once per loop iteration. + * @return true if an updated failure config was applied, false if unchanged. + */ + bool update(); + + /** Cache a failure_injection message directly (used by tests). */ + void set(const failure_injection_s &cfg); + + /** + * Active failure mode for a (unit, instance), or Mode::Ok if none. + * @param unit one of failure_injection_s::FAILURE_UNIT_* + * @param instance 1-based instance, or 0 to match any instance of the unit + */ + Mode mode(uint8_t unit, uint8_t instance) const; + + /** True if any failure is currently active (free no-failure early-out). */ + bool any_active() const { return _count > 0; } + +private: + uORB::Subscription _sub{ORB_ID(failure_injection)}; + + uint8_t _count{0}; + uint8_t _unit[failure_injection_s::MAX_FAILURES] {}; + uint16_t _instance_mask[failure_injection_s::MAX_FAILURES] {}; + Mode _failure_type[failure_injection_s::MAX_FAILURES] {}; +}; + +/** + * Per-(consumer, instance) state for stateful failures. Holds the last good + * sample so Stuck can replay it. + */ +template +struct Stuck { + bool valid{false}; + MsgT value{}; +}; + +/** + * Generic whole-message processor for the Ok / Off / Stuck mechanics, for consumers + * that publish a single message and want the message-agnostic behaviour. + * + * @return false if the message must be suppressed (Off); true otherwise. For + * Stuck the message is overwritten with the last good sample. + */ +template +bool process(Mode mode, MsgT &msg, Stuck &stuck) +{ + switch (mode) { + case Mode::Off: + return false; + + case Mode::Stuck: + if (stuck.valid) { + const uint64_t timestamp = msg.timestamp; + msg = stuck.value; + msg.timestamp = timestamp; + } + + return true; + + default: + stuck.value = msg; + stuck.valid = true; + return true; + } +} + +/** + * Convenience overload to fix the fact that the uORB instance is 0-based but the failure_injection instance is 1-based. + */ +template +bool process(const Config &config, uint8_t unit, uint8_t uorb_instance, MsgT &msg, Stuck &stuck) +{ + return process(config.mode(unit, uorb_instance + 1), msg, stuck); +} + +} // namespace failure_injection diff --git a/src/lib/systemlib/system_params.yaml b/src/lib/systemlib/system_params.yaml index ba38fec97b..32cbb9d318 100644 --- a/src/lib/systemlib/system_params.yaml +++ b/src/lib/systemlib/system_params.yaml @@ -233,12 +233,3 @@ parameters: type: boolean default: 0 reboot_required: true - SYS_FAILURE_EN: - description: - short: Enable failure injection - long: |- - If enabled allows MAVLink INJECT_FAILURE commands. - - WARNING: the failures can easily cause crashes and are to be used with caution! - type: boolean - default: 0 diff --git a/src/modules/commander/failure_detector/CMakeLists.txt b/src/modules/commander/failure_detector/CMakeLists.txt index 9e391573b9..117912d9e6 100644 --- a/src/modules/commander/failure_detector/CMakeLists.txt +++ b/src/modules/commander/failure_detector/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_library(failure_detector FailureDetector.cpp FailureInjector.cpp ) +target_link_libraries(failure_detector PUBLIC failure_injection) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/failure_detector_params.yaml) px4_add_functional_gtest(SRC FailureDetectorAltitudeLossTest.cpp diff --git a/src/modules/commander/failure_detector/FailureInjector.cpp b/src/modules/commander/failure_detector/FailureInjector.cpp index 9bbab47342..3a40506ce5 100644 --- a/src/modules/commander/failure_detector/FailureInjector.cpp +++ b/src/modules/commander/failure_detector/FailureInjector.cpp @@ -33,87 +33,44 @@ #include "FailureInjector.hpp" -#include -#include +#include #include -FailureInjector::FailureInjector() -{ - int32_t param_sys_failure_en = 0; - - if ((param_get(param_find("SYS_FAILURE_EN"), ¶m_sys_failure_en) == PX4_OK) - && (param_sys_failure_en == 1)) { - _failure_injection_enabled = true; - } -} - void FailureInjector::update() { - if (!_failure_injection_enabled) { return; } + if (_failure_config.update()) { + rebuildMasks(); + } +} - vehicle_command_s vehicle_command; +void FailureInjector::rebuildMasks() +{ + _motor_stop_mask = 0; + _esc_telemetry_blocked_mask = 0; + _esc_telemetry_wrong_mask = 0; - 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)); + for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { + switch (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR, i + 1)) { + case failure_injection::Mode::Off: + _motor_stop_mask |= 1u << i; + break; - if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE - || failure_unit != vehicle_command_s::FAILURE_UNIT_SYSTEM_MOTOR) { - continue; + case failure_injection::Mode::Stuck: + _esc_telemetry_blocked_mask |= 1u << i; + break; + + case failure_injection::Mode::Wrong: + _esc_telemetry_wrong_mask |= 1u << i; + break; + + default: + break; } - - const int instance = static_cast(lroundf(vehicle_command.param3)); - bool supported = false; - - for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { - if (instance != 0 && i != (instance - 1)) { - continue; - } - - switch (failure_type) { - case vehicle_command_s::FAILURE_TYPE_OK: - supported = true; - PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i + 1); - _motor_stop_mask &= ~(1 << i); - _esc_telemetry_blocked_mask &= ~(1 << i); - _esc_telemetry_wrong_mask &= ~(1 << i); - break; - - case vehicle_command_s::FAILURE_TYPE_OFF: - supported = true; - PX4_INFO("CMD_INJECT_FAILURE, motor %d off", i + 1); - _motor_stop_mask |= 1 << i; - break; - - case vehicle_command_s::FAILURE_TYPE_STUCK: - supported = true; - PX4_INFO("CMD_INJECT_FAILURE, motor %d no esc telemetry", i + 1); - _esc_telemetry_blocked_mask |= 1 << i; - break; - - case vehicle_command_s::FAILURE_TYPE_WRONG: - supported = true; - PX4_INFO("CMD_INJECT_FAILURE, motor %d esc telemetry wrong", i); - _esc_telemetry_wrong_mask |= 1 << i; - 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); } } void FailureInjector::manipulateEscStatus(esc_status_s &status) { - if (!_failure_injection_enabled) { return; } - if (_esc_telemetry_blocked_mask != 0 || _esc_telemetry_wrong_mask != 0) { for (int i = 0; i < status.esc_count; i++) { const unsigned i_esc = status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1; diff --git a/src/modules/commander/failure_detector/FailureInjector.hpp b/src/modules/commander/failure_detector/FailureInjector.hpp index 86f2199723..094607c7f7 100644 --- a/src/modules/commander/failure_detector/FailureInjector.hpp +++ b/src/modules/commander/failure_detector/FailureInjector.hpp @@ -33,26 +33,25 @@ #pragma once -#include -#include +#include #include -#include -#include +#include class FailureInjector { public: - FailureInjector(); + FailureInjector() = default; void update(); void manipulateEscStatus(esc_status_s &status); uint32_t getMotorStopMask() { return _motor_stop_mask; } private: - uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; - uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + // Rebuild the motor masks from the active failure_injection configuration. + void rebuildMasks(); + + failure_injection::Config _failure_config; - bool _failure_injection_enabled = false; uint32_t _motor_stop_mask{}; uint32_t _esc_telemetry_blocked_mask{}; uint32_t _esc_telemetry_wrong_mask{}; diff --git a/src/modules/failure_injection_manager/CMakeLists.txt b/src/modules/failure_injection_manager/CMakeLists.txt new file mode 100644 index 0000000000..f5c62c8648 --- /dev/null +++ b/src/modules/failure_injection_manager/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__failure_injection_manager + MAIN failure_injection_manager + SRCS + FailureInjectionManager.cpp + FailureInjectionManager.hpp + FailureTable.cpp + FailureTable.hpp + MODULE_CONFIG + failure_injection_manager_params.yaml + DEPENDS + px4_work_queue + ) + +px4_add_unit_gtest(SRC FailureInjectionManagerTest.cpp EXTRA_SRCS FailureTable.cpp LINKLIBS failure_injection) +px4_add_functional_gtest(SRC FailureInjectionConfigTest.cpp LINKLIBS failure_injection) diff --git a/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp b/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp new file mode 100644 index 0000000000..0dbda52c72 --- /dev/null +++ b/src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +// Functional tests for the consumer-side failure_injection helpers: the Config +// lookup, the process<>() mechanics, and the real uORB publish -> Config::update() +// round-trip. Config owns a uORB subscription, so this needs the uORB runtime and +// runs as a functional gtest. + +#include + +#include +#include + +// FailureInjection.hpp transitively pulls in px4_platform_common/defines.h (via +// uORB Subscription), which defines an OK macro that would clash with the local +// OK constant below. +#undef OK + +using namespace failure_injection; + +namespace +{ + +// Minimal uORB-like message: every uORB message starts with a uint64 timestamp. +struct FakeMsg { + uint64_t timestamp; + float value; +}; + +failure_injection_s make_config(uint8_t unit, uint16_t instance_mask, uint8_t failure_type) +{ + failure_injection_s cfg{}; + cfg.count = 1; + cfg.unit[0] = unit; + cfg.instance_mask[0] = instance_mask; + cfg.failure_type[0] = failure_type; + return cfg; +} + +constexpr uint8_t GYRO = failure_injection_s::FAILURE_UNIT_SENSOR_GYRO; +constexpr uint8_t GPS = failure_injection_s::FAILURE_UNIT_SENSOR_GPS; +constexpr uint8_t MOTOR = failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR; + +constexpr uint8_t OK = failure_injection_s::FAILURE_TYPE_OK; +constexpr uint8_t OFF = failure_injection_s::FAILURE_TYPE_OFF; +constexpr uint8_t STUCK = failure_injection_s::FAILURE_TYPE_STUCK; +constexpr uint8_t WRONG = failure_injection_s::FAILURE_TYPE_WRONG; + +} // namespace + +// =========================================================================== +// Config lookup (set / mode) +// =========================================================================== + +TEST(FailureInjectionConfig, EmptyConfigReturnsOk) +{ + Config config; + EXPECT_FALSE(config.any_active()); + EXPECT_EQ(config.mode(GYRO, 1), Mode::Ok); +} + +TEST(FailureInjectionConfig, SpecificInstanceMatch) +{ + Config config; + // instance 2 -> bit (2 - 1) = 0x2 + config.set(make_config(GYRO, 0x2, OFF)); + + EXPECT_TRUE(config.any_active()); + EXPECT_EQ(config.mode(GYRO, 2), Mode::Off); + EXPECT_EQ(config.mode(GYRO, 1), Mode::Ok); + EXPECT_EQ(config.mode(GYRO, 3), Mode::Ok); + // Different unit, same instance -> no match. + EXPECT_EQ(config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL, 2), Mode::Ok); + // instance 0 matches any instance of the unit. + EXPECT_EQ(config.mode(GYRO, 0), Mode::Off); +} + +TEST(FailureInjectionConfig, AllInstancesMask) +{ + Config config; + config.set(make_config(failure_injection_s::FAILURE_UNIT_SENSOR_MAG, 0xFFFF, STUCK)); + + EXPECT_EQ(config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_MAG, 1), Mode::Stuck); + EXPECT_EQ(config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_MAG, 8), Mode::Stuck); + EXPECT_EQ(config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_MAG, 16), Mode::Stuck); +} + +TEST(FailureInjectionConfig, MultipleEntriesResolveIndependently) +{ + failure_injection_s cfg{}; + cfg.count = 2; + cfg.unit[0] = MOTOR; + cfg.instance_mask[0] = 0x5; // motors 1 and 3 + cfg.failure_type[0] = OFF; + cfg.unit[1] = GPS; + cfg.instance_mask[1] = 0xFFFF; + cfg.failure_type[1] = WRONG; + + Config config; + config.set(cfg); + + EXPECT_EQ(config.mode(MOTOR, 1), Mode::Off); + EXPECT_EQ(config.mode(MOTOR, 2), Mode::Ok); + EXPECT_EQ(config.mode(MOTOR, 3), Mode::Off); + EXPECT_EQ(config.mode(GPS, 1), Mode::Wrong); +} + +TEST(FailureInjectionConfig, CountIsClampedToMax) +{ + failure_injection_s cfg{}; + cfg.count = 200; // absurd, must be clamped to MAX_FAILURES without reading OOB + + for (int i = 0; i < failure_injection_s::MAX_FAILURES; i++) { + cfg.unit[i] = failure_injection_s::FAILURE_UNIT_SENSOR_BARO; + cfg.instance_mask[i] = 0xFFFF; + cfg.failure_type[i] = OFF; + } + + Config config; + config.set(cfg); + EXPECT_TRUE(config.any_active()); + EXPECT_EQ(config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_BARO, 1), Mode::Off); +} + +// =========================================================================== +// uORB round-trip: publish -> Config::update() -> mode() +// =========================================================================== + +TEST(FailureInjectionConfig, UpdatePicksUpPublishedSample) +{ + uORB::Publication pub{ORB_ID(failure_injection)}; + pub.advertise(); + + Config config; + // Subscribed against the freshly advertised (empty) topic: nothing new yet. + EXPECT_FALSE(config.update()); + EXPECT_FALSE(config.any_active()); + + failure_injection_s msg = make_config(GYRO, 0x2, OFF); + msg.timestamp = 1; + pub.publish(msg); + + EXPECT_TRUE(config.update()); + EXPECT_TRUE(config.any_active()); + EXPECT_EQ(config.mode(GYRO, 2), Mode::Off); + + // No new sample -> update() reports no change but the cache is retained. + EXPECT_FALSE(config.update()); + EXPECT_EQ(config.mode(GYRO, 2), Mode::Off); +} + +// =========================================================================== +// process<>() Off / Stuck mechanics (single-message helper) +// =========================================================================== + +TEST(FailureInjectionConfig, ProcessOffSuppresses) +{ + FakeMsg msg{100, 1.0f}; + Stuck stuck; + EXPECT_FALSE(process(Mode::Off, msg, stuck)); +} + +TEST(FailureInjectionConfig, ProcessStuckReplaysValueButAdvancesTimestamp) +{ + Stuck stuck; + + // A good sample is recorded while no failure is active. + FakeMsg good{100, 1.0f}; + EXPECT_TRUE(process(Mode::Ok, good, stuck)); + ASSERT_TRUE(stuck.valid); + + // A later sample arrives while Stuck: the value is frozen, the timestamp advances. + FakeMsg later{200, 2.0f}; + EXPECT_TRUE(process(Mode::Stuck, later, stuck)); + EXPECT_FLOAT_EQ(later.value, 1.0f); + EXPECT_EQ(later.timestamp, 200u); +} + +TEST(FailureInjectionConfig, ProcessStuckWithoutSnapshotLeavesMessageUnchanged) +{ + Stuck stuck; // never recorded a good sample + FakeMsg msg{200, 2.0f}; + EXPECT_TRUE(process(Mode::Stuck, msg, stuck)); + EXPECT_FLOAT_EQ(msg.value, 2.0f); + EXPECT_EQ(msg.timestamp, 200u); +} + +TEST(FailureInjectionConfig, ProcessOkRecordsLastGood) +{ + Stuck stuck; + FakeMsg msg{100, 5.0f}; + EXPECT_TRUE(process(Mode::Ok, msg, stuck)); + EXPECT_TRUE(stuck.valid); + EXPECT_FLOAT_EQ(stuck.value.value, 5.0f); + // Message itself is untouched. + EXPECT_FLOAT_EQ(msg.value, 5.0f); +} diff --git a/src/modules/failure_injection_manager/FailureInjectionManager.cpp b/src/modules/failure_injection_manager/FailureInjectionManager.cpp new file mode 100644 index 0000000000..1a34fb0629 --- /dev/null +++ b/src/modules/failure_injection_manager/FailureInjectionManager.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "FailureInjectionManager.hpp" + +#include + +#include +#include + +ModuleBase::Descriptor FailureInjectionManager::desc{task_spawn, custom_command, print_usage}; + +FailureInjectionManager::FailureInjectionManager() : + WorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ +} + +bool FailureInjectionManager::init() +{ + if (!_vehicle_command_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + _failure_injection_pub.advertise(); + + return true; +} + +void FailureInjectionManager::Run() +{ + if (should_exit()) { + _vehicle_command_sub.unregisterCallback(); + exit_and_cleanup(desc); + return; + } + + vehicle_command_s cmd; + + while (_vehicle_command_sub.update(&cmd)) { + if (cmd.command == vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + handleCommand(cmd); + } + } + + // Republish only when the configuration actually changed + if (_table.changed()) { + failure_injection_s msg{}; + _table.fill(msg); + msg.timestamp = hrt_absolute_time(); + _failure_injection_pub.publish(msg); + _table.clearChanged(); + } +} + +void FailureInjectionManager::handleCommand(const vehicle_command_s &cmd) +{ + const uint8_t unit = static_cast(lroundf(cmd.param1)); + const uint8_t type = static_cast(lroundf(cmd.param2)); + const uint8_t instance = static_cast(lroundf(cmd.param3)); + + uint8_t result; + + switch (_table.inject(unit, type, instance)) { + case failure_injection::FailureTable::AckResult::Accepted: + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + break; + + case failure_injection::FailureTable::AckResult::Rejected: + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + break; + + case failure_injection::FailureTable::AckResult::Unsupported: + default: + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + break; + } + + publishAck(cmd, result); +} + +void FailureInjectionManager::publishAck(const vehicle_command_s &cmd, uint8_t result) +{ + vehicle_command_ack_s ack{}; + ack.command = cmd.command; + ack.from_external = false; + ack.result = result; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); +} + +int FailureInjectionManager::task_spawn(int argc, char *argv[]) +{ + FailureInjectionManager *instance = new FailureInjectionManager(); + + if (instance) { + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + desc.object.store(nullptr); + desc.task_id = -1; + + return PX4_ERROR; +} + +int FailureInjectionManager::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FailureInjectionManager::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +The failure injection manager is the single subscriber to `vehicle_command` for +`MAV_CMD_INJECT_FAILURE`. It maintains the set of currently active failures and +publishes the `failure_injection` topic, republishing only when the configuration +changes so that command spam cannot propagate to the consumers that apply the +failures. It also produces the central `vehicle_command_ack`. + +Failure injection is gated by the `SYS_FAILURE_EN` parameter, which the startup +script checks before starting this module. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("failure_injection_manager", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int failure_injection_manager_main(int argc, char *argv[]) +{ + return ModuleBase::main(FailureInjectionManager::desc, argc, argv); +} diff --git a/src/modules/failure_injection_manager/FailureInjectionManager.hpp b/src/modules/failure_injection_manager/FailureInjectionManager.hpp new file mode 100644 index 0000000000..18a53d4253 --- /dev/null +++ b/src/modules/failure_injection_manager/FailureInjectionManager.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 FailureInjectionManager.hpp + * + * The failure injection manager: the single subscriber to vehicle_command for + * MAV_CMD_INJECT_FAILURE. It maintains the active failure table and republishes + * the failure_injection topic only when the configuration changes, so a burst + * of commands on vehicle_command cannot propagate to the consumers that apply + * the failures (the bulkhead). It also produces the central vehicle_command_ack. + */ + +#pragma once + +#include "FailureTable.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class FailureInjectionManager : public ModuleBase, public px4::WorkItem +{ +public: + FailureInjectionManager(); + ~FailureInjectionManager() override = default; + + static Descriptor desc; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + void handleCommand(const vehicle_command_s &cmd); + void publishAck(const vehicle_command_s &cmd, uint8_t result); + + uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)}; + + uORB::Publication _failure_injection_pub{ORB_ID(failure_injection)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + failure_injection::FailureTable _table; +}; diff --git a/src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp b/src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp new file mode 100644 index 0000000000..46bc36c3e7 --- /dev/null +++ b/src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + +#include + +#include "FailureTable.hpp" + +using namespace failure_injection; +using AckResult = FailureTable::AckResult; + +namespace +{ + +constexpr uint8_t GYRO = failure_injection_s::FAILURE_UNIT_SENSOR_GYRO; +constexpr uint8_t GPS = failure_injection_s::FAILURE_UNIT_SENSOR_GPS; +constexpr uint8_t MOTOR = failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR; + +constexpr uint8_t OK = failure_injection_s::FAILURE_TYPE_OK; +constexpr uint8_t OFF = failure_injection_s::FAILURE_TYPE_OFF; +constexpr uint8_t STUCK = failure_injection_s::FAILURE_TYPE_STUCK; +constexpr uint8_t WRONG = failure_injection_s::FAILURE_TYPE_WRONG; +constexpr uint8_t GARBAGE = failure_injection_s::FAILURE_TYPE_GARBAGE; + +} // namespace + +TEST(FailureTable, SupportedCatalogueMatchesInventory) +{ + EXPECT_TRUE(FailureTable::isSupported(GYRO, OFF)); + EXPECT_TRUE(FailureTable::isSupported(GYRO, STUCK)); + EXPECT_FALSE(FailureTable::isSupported(GYRO, WRONG)); // no gyro WRONG today + EXPECT_TRUE(FailureTable::isSupported(GPS, WRONG)); + EXPECT_TRUE(FailureTable::isSupported(MOTOR, WRONG)); + EXPECT_FALSE(FailureTable::isSupported(GYRO, GARBAGE)); // GARBAGE unimplemented + // Unimplemented units. + EXPECT_FALSE(FailureTable::isSupported(failure_injection_s::FAILURE_UNIT_SYSTEM_RC_SIGNAL, OFF)); + EXPECT_FALSE(FailureTable::isSupported(failure_injection_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR, OFF)); +} + +TEST(FailureTable, UnsupportedIsRejectedWithoutChange) +{ + FailureTable table; + EXPECT_EQ(table.inject(GYRO, WRONG, 0), AckResult::Unsupported); + EXPECT_FALSE(table.changed()); + EXPECT_EQ(table.count(), 0); +} + +TEST(FailureTable, InjectSpecificInstanceSetsBit) +{ + FailureTable table; + EXPECT_EQ(table.inject(GYRO, OFF, 2), AckResult::Accepted); // 1-based instance 2 + EXPECT_TRUE(table.changed()); + EXPECT_EQ(table.count(), 1); + + failure_injection_s msg{}; + table.fill(msg); + EXPECT_EQ(msg.count, 1); + EXPECT_EQ(msg.unit[0], GYRO); + EXPECT_EQ(msg.failure_type[0], OFF); + EXPECT_EQ(msg.instance_mask[0], 0x2); // bit (2 - 1) +} + +TEST(FailureTable, InjectAllInstancesUsesFullMask) +{ + FailureTable table; + EXPECT_EQ(table.inject(MOTOR, OFF, 0), AckResult::Accepted); // 0 = all + failure_injection_s msg{}; + table.fill(msg); + EXPECT_EQ(msg.instance_mask[0], 0xFFFF); +} + +TEST(FailureTable, DuplicateInjectDoesNotChange) +{ + FailureTable table; + EXPECT_EQ(table.inject(GYRO, OFF, 1), AckResult::Accepted); + EXPECT_TRUE(table.changed()); + table.clearChanged(); + + // Re-inject the exact same failure: accepted, but the table did not change. + EXPECT_EQ(table.inject(GYRO, OFF, 1), AckResult::Accepted); + EXPECT_FALSE(table.changed()); +} + +TEST(FailureTable, OkClearsTheInstance) +{ + FailureTable table; + table.inject(GYRO, OFF, 0); // all gyros off + table.clearChanged(); + + EXPECT_EQ(table.inject(GYRO, OK, 0), AckResult::Accepted); + EXPECT_TRUE(table.changed()); + EXPECT_EQ(table.count(), 0); +} + +TEST(FailureTable, OkClearsOnlyTheAddressedInstance) +{ + FailureTable table; + table.inject(GYRO, OFF, 0); // mask 0xFFFF + + // Clear only instance 2. + EXPECT_EQ(table.inject(GYRO, OK, 2), AckResult::Accepted); + failure_injection_s msg{}; + table.fill(msg); + ASSERT_EQ(msg.count, 1); + EXPECT_EQ(msg.instance_mask[0], 0xFFFF & ~0x2); +} + +TEST(FailureTable, NewModeOnInstanceClearsOldMode) +{ + FailureTable table; + table.inject(GYRO, OFF, 1); // instance 1 OFF + table.inject(GYRO, STUCK, 1); // instance 1 now STUCK -> OFF entry must drop + + failure_injection_s msg{}; + table.fill(msg); + ASSERT_EQ(msg.count, 1); + EXPECT_EQ(msg.unit[0], GYRO); + EXPECT_EQ(msg.failure_type[0], STUCK); + EXPECT_EQ(msg.instance_mask[0], 0x1); +} + +TEST(FailureTable, MotorMaskAccumulatesInstances) +{ + FailureTable table; + table.inject(MOTOR, OFF, 1); + table.inject(MOTOR, OFF, 3); + table.inject(MOTOR, OFF, 5); + + failure_injection_s msg{}; + table.fill(msg); + ASSERT_EQ(msg.count, 1); // single entry, three instances + EXPECT_EQ(msg.instance_mask[0], 0x1 | 0x4 | 0x10); +} + +TEST(FailureTable, OverflowBeyondFourDistinctFailuresIsRejected) +{ + FailureTable table; + EXPECT_EQ(table.inject(GYRO, OFF, 1), AckResult::Accepted); + EXPECT_EQ(table.inject(failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL, OFF, 1), AckResult::Accepted); + EXPECT_EQ(table.inject(failure_injection_s::FAILURE_UNIT_SENSOR_MAG, OFF, 1), AckResult::Accepted); + EXPECT_EQ(table.inject(failure_injection_s::FAILURE_UNIT_SENSOR_BARO, OFF, 1), AckResult::Accepted); + EXPECT_EQ(table.count(), 4); + + // A fifth distinct (unit, type) must be rejected and leave the table intact. + table.clearChanged(); + EXPECT_EQ(table.inject(GPS, OFF, 1), AckResult::Rejected); + EXPECT_FALSE(table.changed()); + EXPECT_EQ(table.count(), 4); +} + +TEST(FailureTable, AddingInstanceToExistingEntryDoesNotCountAsNew) +{ + FailureTable table; + table.inject(GYRO, OFF, 1); + table.inject(failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL, OFF, 1); + table.inject(failure_injection_s::FAILURE_UNIT_SENSOR_MAG, OFF, 1); + table.inject(failure_injection_s::FAILURE_UNIT_SENSOR_BARO, OFF, 1); + ASSERT_EQ(table.count(), 4); + + // Table is full, but extending an existing entry to another instance must succeed. + EXPECT_EQ(table.inject(GYRO, OFF, 2), AckResult::Accepted); + EXPECT_EQ(table.count(), 4); + + failure_injection_s msg{}; + table.fill(msg); + // gyro entry now covers instances 1 and 2 + EXPECT_EQ(msg.instance_mask[0], 0x1 | 0x2); +} diff --git a/src/modules/failure_injection_manager/FailureTable.cpp b/src/modules/failure_injection_manager/FailureTable.cpp new file mode 100644 index 0000000000..4d5e10c1cb --- /dev/null +++ b/src/modules/failure_injection_manager/FailureTable.cpp @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "FailureTable.hpp" + +#include + +namespace failure_injection +{ + +bool FailureTable::isSupported(uint8_t unit, uint8_t type) +{ + // Capability catalogue: the (unit, type) combinations implemented today. + // FAILURE_TYPE_OK is always accepted for a known unit (it clears the unit). + switch (unit) { + case failure_injection_s::FAILURE_UNIT_SENSOR_GYRO: + case failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL: + case failure_injection_s::FAILURE_UNIT_SENSOR_MAG: + case failure_injection_s::FAILURE_UNIT_SENSOR_BARO: + return type == failure_injection_s::FAILURE_TYPE_OK + || type == failure_injection_s::FAILURE_TYPE_OFF + || type == failure_injection_s::FAILURE_TYPE_STUCK; + + case failure_injection_s::FAILURE_UNIT_SENSOR_GPS: + case failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED: + case failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR: + return type == failure_injection_s::FAILURE_TYPE_OK + || type == failure_injection_s::FAILURE_TYPE_OFF + || type == failure_injection_s::FAILURE_TYPE_STUCK + || type == failure_injection_s::FAILURE_TYPE_WRONG; + + case failure_injection_s::FAILURE_UNIT_SENSOR_VIO: + case failure_injection_s::FAILURE_UNIT_SYSTEM_BATTERY: + return type == failure_injection_s::FAILURE_TYPE_OK + || type == failure_injection_s::FAILURE_TYPE_OFF; + + default: + return false; + } +} + +uint16_t FailureTable::instanceToMask(uint8_t instance) +{ + if (instance == 0) { + return 0xFFFF; // all instances + } + + if (instance > 16) { + return 0; // out of range, addressable instances are 1..16 + } + + return static_cast(1u << (instance - 1)); +} + +int FailureTable::findEntry(uint8_t unit, uint8_t type) const +{ + for (uint8_t i = 0; i < _count; i++) { + if (_entries[i].unit == unit && _entries[i].type == type) { + return i; + } + } + + return -1; +} + +void FailureTable::clearInstances(uint8_t unit, uint16_t mask, uint8_t keep_type) +{ + for (uint8_t i = 0; i < _count; i++) { + if (_entries[i].unit == unit && _entries[i].type != keep_type + && (_entries[i].instance_mask & mask) != 0) { + _entries[i].instance_mask &= ~mask; + _changed = true; + } + } + + compact(); +} + +void FailureTable::compact() +{ + uint8_t write = 0; + + for (uint8_t read = 0; read < _count; read++) { + if (_entries[read].instance_mask != 0) { + if (write != read) { + _entries[write] = _entries[read]; + } + + write++; + } + } + + _count = write; + + // Zero the unused tail so dropped entries leave no stale state behind. + for (uint8_t i = _count; i < failure_injection_s::MAX_FAILURES; i++) { + _entries[i] = Entry{}; + } +} + +FailureTable::AckResult FailureTable::inject(uint8_t unit, uint8_t type, uint8_t instance) +{ + // Validate the request against the capability catalogue. + if (!isSupported(unit, type)) { + return AckResult::Unsupported; + } + + const uint16_t mask = instanceToMask(instance); + + if (mask == 0) { + // Out-of-range instance: nothing addressable to do. + return AckResult::Accepted; + } + + // Snapshot for rollback if the request would overflow the table. _changed is + // set at the mutation sites below, so it has to be restored on rollback too. + Entry saved_entries[failure_injection_s::MAX_FAILURES]; + uint8_t saved_count = _count; + bool saved_changed = _changed; + memcpy(saved_entries, _entries, sizeof(_entries)); + + if (type == failure_injection_s::FAILURE_TYPE_OK) { + // Clear the addressed instances from every failure mode of this unit. + clearInstances(unit, mask, 0xFF); + + } else { + // An instance is in at most one mode per unit: drop these instances + // from the unit's other modes before applying the requested one. + clearInstances(unit, mask, type); + + int idx = findEntry(unit, type); + + if (idx < 0) { + if (_count >= failure_injection_s::MAX_FAILURES) { + // Would exceed the cap: roll back and reject. + memcpy(_entries, saved_entries, sizeof(_entries)); + _count = saved_count; + _changed = saved_changed; + return AckResult::Rejected; + } + + idx = _count++; + _entries[idx].unit = unit; + _entries[idx].type = type; + _entries[idx].instance_mask = 0; + _changed = true; + } + + // Only a mask that actually adds instances counts as a change (dedup). + if ((_entries[idx].instance_mask & mask) != mask) { + _entries[idx].instance_mask |= mask; + _changed = true; + } + } + + return AckResult::Accepted; +} + +void FailureTable::fill(failure_injection_s &msg) const +{ + msg.count = _count; + + for (uint8_t i = 0; i < failure_injection_s::MAX_FAILURES; i++) { + if (i < _count) { + msg.unit[i] = _entries[i].unit; + msg.instance_mask[i] = _entries[i].instance_mask; + msg.failure_type[i] = _entries[i].type; + + } else { + msg.unit[i] = 0; + msg.instance_mask[i] = 0; + msg.failure_type[i] = 0; + } + } +} + +} // namespace failure_injection diff --git a/src/modules/failure_injection_manager/FailureTable.hpp b/src/modules/failure_injection_manager/FailureTable.hpp new file mode 100644 index 0000000000..e1d74cc0e3 --- /dev/null +++ b/src/modules/failure_injection_manager/FailureTable.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 FailureTable.hpp + * + * Policy core of the failure injection manager: the set of currently active + * failures. Pure logic (no uORB runtime) so it can be unit tested in isolation. + * + * The manager feeds each MAV_CMD_INJECT_FAILURE request to inject() and, when the + * table changes, serialises it into a failure_injection message with fill(). + * inject() reports the ack result the manager should return: + * - Accepted the request was applied (or was a no-op duplicate), + * - Unsupported the (unit, type) is not in the capability catalogue, + * - Rejected applying it would exceed MAX_FAILURES distinct (unit, type)s. + * + * Addressing is per (unit, type) with a 16-bit instance mask, so a single entry + * covers any number of instances (e.g. "all motors off" or "motors 1 and 3 off" + * is one entry). An instance is in at most one failure mode per unit: injecting + * a new mode on an instance clears it from the unit's other modes. + */ + +#pragma once + +#include + +#include + +namespace failure_injection +{ + +class FailureTable +{ +public: + enum class AckResult { + Accepted, + Unsupported, + Rejected, + }; + + /** + * Apply a MAV_CMD_INJECT_FAILURE request. + * @param unit failure_injection_s::FAILURE_UNIT_* + * @param type failure_injection_s::FAILURE_TYPE_* + * @param instance 1-based component/sensor instance, or 0 for all instances + */ + AckResult inject(uint8_t unit, uint8_t type, uint8_t instance); + + /** True if the table changed since the last clearChanged(). */ + bool changed() const { return _changed; } + void clearChanged() { _changed = false; } + + /** Serialise the active table into a message (timestamp left to the caller). */ + void fill(failure_injection_s &msg) const; + + uint8_t count() const { return _count; } + + /** Capability catalogue: which (unit, type) combinations the system supports. */ + static bool isSupported(uint8_t unit, uint8_t type); + +private: + struct Entry { + uint8_t unit; + uint16_t instance_mask; + uint8_t type; + }; + + static uint16_t instanceToMask(uint8_t instance); + int findEntry(uint8_t unit, uint8_t type) const; + void clearInstances(uint8_t unit, uint16_t mask, uint8_t keep_type); + void compact(); + + Entry _entries[failure_injection_s::MAX_FAILURES] {}; + uint8_t _count{0}; + bool _changed{false}; +}; + +} // namespace failure_injection diff --git a/src/modules/failure_injection_manager/Kconfig b/src/modules/failure_injection_manager/Kconfig new file mode 100644 index 0000000000..02e85fbbf9 --- /dev/null +++ b/src/modules/failure_injection_manager/Kconfig @@ -0,0 +1,7 @@ +menuconfig MODULES_FAILURE_INJECTION_MANAGER + bool "failure_injection_manager" + default n + ---help--- + Enable support for the failure injection manager. The manager is the sole + subscriber to vehicle_command for MAV_CMD_INJECT_FAILURE and publishes the + deduplicated failure_injection topic consumed by the failure apply-sites. diff --git a/src/modules/failure_injection_manager/failure_injection_manager_params.yaml b/src/modules/failure_injection_manager/failure_injection_manager_params.yaml new file mode 100644 index 0000000000..e28db153f0 --- /dev/null +++ b/src/modules/failure_injection_manager/failure_injection_manager_params.yaml @@ -0,0 +1,14 @@ +module_name: failure_injection_manager +parameters: + - group: Failure Injection + definitions: + SYS_FAILURE_EN: + description: + short: Enable failure injection + long: |- + If enabled allows Injection of Failures. + + WARNING: the failures can easily cause crashes and are to be used with caution! + type: boolean + default: 0 + reboot_required: true diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b026bfb1ff..75569e9eea 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -470,7 +470,14 @@ public: bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); } bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); } - bool failure_injection_enabled() const { return _param_sys_failure_injection_enabled.get(); } + bool failure_injection_enabled() const + { +#if defined(CONFIG_MODULES_FAILURE_INJECTION_MANAGER) + return _param_sys_failure_injection_enabled.get(); +#else + return false; +#endif // CONFIG_MODULES_FAILURE_INJECTION_MANAGER + } struct ping_statistics_s { uint64_t last_ping_time; @@ -643,8 +650,10 @@ private: (ParamBool) _param_mav_hash_chk_en, (ParamBool) _param_mav_hb_forw_en, (ParamInt) _param_mav_radio_timeout, - (ParamInt) _param_sys_hitl, - (ParamBool) _param_sys_failure_injection_enabled + (ParamInt) _param_sys_hitl +#if defined(CONFIG_MODULES_FAILURE_INJECTION_MANAGER) + , (ParamBool) _param_sys_failure_injection_enabled +#endif // CONFIG_MODULES_FAILURE_INJECTION_MANAGER ) perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */ diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.cpp b/src/modules/simulation/battery_simulator/BatterySimulator.cpp index 36c9961bc1..8c145cd941 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.cpp @@ -73,7 +73,7 @@ void BatterySimulator::Run() updateParams(); } - updateCommands(); + updateFailureConfig(); if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status; @@ -120,58 +120,14 @@ void BatterySimulator::Run() perf_end(_loop_perf); } -void BatterySimulator::updateCommands() +void BatterySimulator::updateFailureConfig() { - vehicle_command_s vehicle_command; + _failure_config.update(); - while (_vehicle_command_sub.update(&vehicle_command)) { - if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { - continue; - } - - bool handled = false; - bool supported = false; - - const int failure_unit = static_cast(lroundf(vehicle_command.param1)); - const int failure_type = static_cast(lroundf(vehicle_command.param2)); - const int instance = static_cast(lroundf(vehicle_command.param3)); - - if (failure_unit == vehicle_command_s::FAILURE_UNIT_SYSTEM_BATTERY) { - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - handled = true; - PX4_INFO("CMD_INJECT_FAILURE, battery ok"); - supported = false; - - if (instance == 0) { - supported = true; - _force_empty_battery = false; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - // Force battery empty for FAILURE_TYPE_OFF - not perfectly accurate, but what we want to achieve - handled = true; - PX4_WARN("CMD_INJECT_FAILURE, battery empty"); - supported = false; - - if (instance == 0) { - supported = true; - _force_empty_battery = true; - } - } - } - - if (handled) { - 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); - } - } + // Force the battery empty for FAILURE_TYPE_OFF - not perfectly accurate, but + // achieves the intended battery failsafe. + _force_empty_battery = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SYSTEM_BATTERY, 1) + == failure_injection::Mode::Off); } int BatterySimulator::task_spawn(int argc, char *argv[]) diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.hpp b/src/modules/simulation/battery_simulator/BatterySimulator.hpp index 553d5864b2..7ca442a1f8 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.hpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include #include @@ -43,10 +44,9 @@ #include #include #include +#include #include #include -#include -#include using namespace time_literals; @@ -71,7 +71,7 @@ public: private: void Run() override; - void updateCommands(); + void updateFailureConfig(); static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ; @@ -79,8 +79,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; - uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + failure_injection::Config _failure_config; Battery _battery; diff --git a/src/modules/simulation/battery_simulator/CMakeLists.txt b/src/modules/simulation/battery_simulator/CMakeLists.txt index 9b81baeeb6..5cd8d120f9 100644 --- a/src/modules/simulation/battery_simulator/CMakeLists.txt +++ b/src/modules/simulation/battery_simulator/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_module( battery_simulator_params.yaml DEPENDS battery + failure_injection mathlib px4_work_queue ) diff --git a/src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt b/src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt index a2a627b704..d9504386b9 100644 --- a/src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt +++ b/src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt @@ -41,5 +41,6 @@ px4_add_module( MODULE_CONFIG parameters.yaml DEPENDS + failure_injection px4_work_queue ) diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 5047c6aacf..2cd489cbee 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -111,7 +111,7 @@ void SensorAirspeedSim::Run() if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated() && _vehicle_attitude_sub.updated()) { - check_failure_injection(); + updateFailureConfig(); if (!_airspeed_disconnected) { @@ -173,59 +173,23 @@ void SensorAirspeedSim::Run() perf_end(_loop_perf); } -void SensorAirspeedSim::check_failure_injection() +void SensorAirspeedSim::updateFailureConfig() { - vehicle_command_s vehicle_command; + _failure_config.update(); - while (_vehicle_command_sub.update(&vehicle_command)) { - if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { - continue; + const failure_injection::Mode mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED, 1); + + _airspeed_disconnected = (mode == failure_injection::Mode::Off); + _airspeed_stuck = (mode == failure_injection::Mode::Stuck); + + if (mode == failure_injection::Mode::Wrong) { + // Simulate pitot blockage: start the ramp on the rising edge and keep it running. + if (_airspeed_blocked_timestamp == 0) { + _airspeed_blocked_timestamp = hrt_absolute_time(); } - bool handled = false; - bool supported = false; - - const int failure_unit = static_cast(std::lround(vehicle_command.param1)); - const int failure_type = static_cast(std::lround(vehicle_command.param2)); - - if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { - - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); - supported = true; - _airspeed_disconnected = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - PX4_WARN("CMD_INJECT_FAILURE, airspeed stuck"); - supported = true; - _airspeed_stuck = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { - PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)"); - supported = true; - _airspeed_blocked_timestamp = hrt_absolute_time(); - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); - supported = true; - _airspeed_disconnected = false; - _airspeed_stuck = false; - _airspeed_blocked_timestamp = 0; - } - } - - if (handled) { - 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); - } + } else { + _airspeed_blocked_timestamp = 0; } } diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp index 2adb3abe33..2eed0f42ec 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp @@ -33,6 +33,7 @@ #pragma once +#include #include #include #include @@ -45,10 +46,9 @@ #include #include #include +#include #include #include -#include -#include #include #include @@ -79,7 +79,7 @@ public: bool init(); - void check_failure_injection(); + void updateFailureConfig(); private: void Run() override; @@ -94,10 +94,10 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; 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 _differential_pressure_pub{ORB_ID(differential_pressure)}; - uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + failure_injection::Config _failure_config; bool _airspeed_disconnected{false}; bool _airspeed_stuck{false}; diff --git a/src/modules/simulation/sensor_gps_sim/CMakeLists.txt b/src/modules/simulation/sensor_gps_sim/CMakeLists.txt index 8f9d64e630..b0fd66f1d8 100644 --- a/src/modules/simulation/sensor_gps_sim/CMakeLists.txt +++ b/src/modules/simulation/sensor_gps_sim/CMakeLists.txt @@ -41,5 +41,6 @@ px4_add_module( MODULE_CONFIG parameters.yaml DEPENDS + failure_injection px4_work_queue ) diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 0663397a35..2d75b44afd 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -108,7 +108,7 @@ void SensorGpsSim::Run() updateParams(); } - check_failure_injection(); + updateFailureConfig(); if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()) { @@ -241,84 +241,9 @@ void SensorGpsSim::publishWithFailures(int instance, sensor_gps_s gps, sensor_gp } } -void SensorGpsSim::check_failure_injection() +void SensorGpsSim::updateFailureConfig() { - 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, 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); - } + _failure_config.update(); } int SensorGpsSim::task_spawn(int argc, char *argv[]) diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index a0608435b1..3a8b361b89 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -33,6 +33,7 @@ #pragma once +#include #include #include #include @@ -42,10 +43,9 @@ #include #include #include +#include #include #include -#include -#include #include #include @@ -75,14 +75,19 @@ private: void Run() override; - void check_failure_injection(); + void updateFailureConfig(); 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; } + // instance is 0-based here; the failure_injection topic addresses 1-based instances. + failure_injection::Mode failureMode(int instance) const + { + return _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GPS, instance + 1); + } + bool isBlocked(int instance) const { return failureMode(instance) == failure_injection::Mode::Off; } + bool isStuck(int instance) const { return failureMode(instance) == failure_injection::Mode::Stuck; } + bool isWrong(int instance) const { return failureMode(instance) == failure_injection::Mode::Wrong; } // generate white Gaussian noise sample with std=1 static float generate_wgn(); @@ -93,17 +98,13 @@ 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")}; - uint8_t _gps_blocked_mask{0}; - uint8_t _gps_stuck_mask{0}; - uint8_t _gps_wrong_mask{0}; + failure_injection::Config _failure_config; sensor_gps_s _last_gps0{}; sensor_gps_s _last_gps1{}; diff --git a/src/modules/simulation/simulator_mavlink/CMakeLists.txt b/src/modules/simulation/simulator_mavlink/CMakeLists.txt index 1b36b769ec..f0e1dce500 100644 --- a/src/modules/simulation/simulator_mavlink/CMakeLists.txt +++ b/src/modules/simulation/simulator_mavlink/CMakeLists.txt @@ -47,6 +47,7 @@ px4_add_module( DEPENDS mavlink_c_generate conversion + failure_injection geo drivers_accelerometer drivers_barometer diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 1e3d469a13..f8498e5f07 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -1302,7 +1302,7 @@ void SimulatorMavlink::send() if (fds_actuator_outputs[0].revents & POLLIN) { // Got new data to read, update all topics. parameters_update(false); - check_failure_injections(); + updateFailureConfig(); _vehicle_status_sub.update(&_vehicle_status); _battery_status_sub.update(&_battery_status); @@ -1494,312 +1494,53 @@ void SimulatorMavlink::run() } } -void SimulatorMavlink::check_failure_injections() +void SimulatorMavlink::updateFailureConfig() { - vehicle_command_s vehicle_command; + _failure_config.update(); - while (_vehicle_command_sub.update(&vehicle_command)) { - if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { - continue; - } - - bool handled = false; - bool supported = false; - - const int failure_unit = static_cast(std::lround(vehicle_command.param1)); - const int failure_type = static_cast(std::lround(vehicle_command.param2)); - const int instance = static_cast(std::lround(vehicle_command.param3)); - - if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, GPS off"); - supported = true; - _gps_blocked = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, GPS ok"); - supported = true; - _gps_blocked = false; - _gps_stuck = false; - _gps_wrong = false; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - _gps_stuck = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { - supported = true; - _gps_wrong = true; - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i); - _accel_blocked[i] = true; - _accel_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1); - _accel_blocked[instance - 1] = true; - _accel_stuck[instance - 1] = false; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i); - _accel_blocked[i] = false; - _accel_stuck[i] = true; - } - - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1); - _accel_blocked[instance - 1] = false; - _accel_stuck[instance - 1] = true; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { - PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i); - _accel_blocked[i] = false; - _accel_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { - PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1); - _accel_blocked[instance - 1] = false; - _accel_stuck[instance - 1] = false; - } - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i); - _gyro_blocked[i] = true; - _gyro_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1); - _gyro_blocked[instance - 1] = true; - _gyro_stuck[instance - 1] = false; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i); - _gyro_blocked[i] = false; - _gyro_stuck[i] = true; - } - - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1); - _gyro_blocked[instance - 1] = false; - _gyro_stuck[instance - 1] = true; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { - PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i); - _gyro_blocked[i] = false; - _gyro_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { - PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1); - _gyro_blocked[instance - 1] = false; - _gyro_stuck[instance - 1] = false; - } - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d off", i); - _mag_blocked[i] = true; - _mag_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d off", instance - 1); - _mag_blocked[instance - 1] = true; - _mag_stuck[instance - 1] = false; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", i); - _mag_blocked[i] = false; - _mag_stuck[i] = true; - } - - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", instance - 1); - _mag_blocked[instance - 1] = false; - _mag_stuck[instance - 1] = true; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", i); - _mag_blocked[i] = false; - _mag_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", instance - 1); - _mag_blocked[instance - 1] = false; - _mag_stuck[instance - 1] = false; - } - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < BARO_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, baro %d off", i); - _baro_blocked[i] = true; - _baro_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= BARO_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, baro %d off", instance - 1); - _baro_blocked[instance - 1] = true; - _baro_stuck[instance - 1] = false; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < BARO_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, baro %d stuck", i); - _baro_blocked[i] = false; - _baro_stuck[i] = true; - } - - } else if (instance >= 1 && instance <= BARO_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, baro %d stuck", instance - 1); - _baro_blocked[instance - 1] = false; - _baro_stuck[instance - 1] = true; - } - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - supported = true; - - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < BARO_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILURE, baro %d ok", i); - _baro_blocked[i] = false; - _baro_stuck[i] = false; - } - - } else if (instance >= 1 && instance <= BARO_COUNT_MAX) { - PX4_WARN("CMD_INJECT_FAILURE, baro %d ok", instance - 1); - _baro_blocked[instance - 1] = false; - _baro_stuck[instance - 1] = false; - } - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); - supported = true; - _airspeed_disconnected = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { - PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)"); - supported = true; - _airspeed_blocked_timestamp = hrt_absolute_time(); - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); - supported = true; - _airspeed_disconnected = false; - _airspeed_blocked_timestamp = 0; - } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, vio off"); - supported = true; - _vio_blocked = true; - - } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, vio ok"); - supported = true; - _vio_blocked = false; - } - } - - if (handled) { - 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); - } + for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + const failure_injection::Mode mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL, i + 1); + _accel_blocked[i] = (mode == failure_injection::Mode::Off); + _accel_stuck[i] = (mode == failure_injection::Mode::Stuck); } + + for (int i = 0; i < GYRO_COUNT_MAX; i++) { + const failure_injection::Mode mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GYRO, i + 1); + _gyro_blocked[i] = (mode == failure_injection::Mode::Off); + _gyro_stuck[i] = (mode == failure_injection::Mode::Stuck); + } + + for (int i = 0; i < MAG_COUNT_MAX; i++) { + const failure_injection::Mode mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_MAG, i + 1); + _mag_blocked[i] = (mode == failure_injection::Mode::Off); + _mag_stuck[i] = (mode == failure_injection::Mode::Stuck); + } + + for (int i = 0; i < BARO_COUNT_MAX; i++) { + const failure_injection::Mode mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_BARO, i + 1); + _baro_blocked[i] = (mode == failure_injection::Mode::Off); + _baro_stuck[i] = (mode == failure_injection::Mode::Stuck); + } + + const failure_injection::Mode gps_mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GPS, 1); + _gps_blocked = (gps_mode == failure_injection::Mode::Off); + _gps_stuck = (gps_mode == failure_injection::Mode::Stuck); + _gps_wrong = (gps_mode == failure_injection::Mode::Wrong); + + const failure_injection::Mode airspeed_mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED, 1); + _airspeed_disconnected = (airspeed_mode == failure_injection::Mode::Off); + + if (airspeed_mode == failure_injection::Mode::Wrong) { + // Simulate pitot blockage: start the ramp on the rising edge and keep it running. + if (_airspeed_blocked_timestamp == 0) { + _airspeed_blocked_timestamp = hrt_absolute_time(); + } + + } else { + _airspeed_blocked_timestamp = 0; + } + + _vio_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_VIO, 1) == failure_injection::Mode::Off); } int SimulatorMavlink::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink) diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 66ffc2ac9a..dcdba888b4 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -80,8 +82,6 @@ #include #include #include -#include -#include #include #include @@ -172,7 +172,7 @@ public: private: SimulatorMavlink(); - void check_failure_injections(); + void updateFailureConfig(); int publish_distance_topic(const mavlink_distance_sensor_t *dist); @@ -218,8 +218,6 @@ private: uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; - uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; - uORB::PublicationMulti *_dist_pubs[ORB_MULTI_MAX_INSTANCES] {}; uint32_t _dist_sensor_ids[ORB_MULTI_MAX_INSTANCES] {}; @@ -296,7 +294,6 @@ private: actuator_outputs_s _actuator_outputs{}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -308,6 +305,8 @@ private: vehicle_status_s _vehicle_status{}; battery_status_s _battery_status{}; + failure_injection::Config _failure_config; + bool _accel_blocked[ACCEL_COUNT_MAX] {}; bool _accel_stuck[ACCEL_COUNT_MAX] {}; sensor_accel_fifo_s _last_accel_fifo{}; diff --git a/src/systemcmds/failure/Kconfig b/src/systemcmds/failure/Kconfig index dce0a8a99f..107fd5bfdc 100644 --- a/src/systemcmds/failure/Kconfig +++ b/src/systemcmds/failure/Kconfig @@ -1,6 +1,7 @@ menuconfig SYSTEMCMDS_FAILURE bool "failure" default n + depends on MODULES_FAILURE_INJECTION_MANAGER ---help--- Enable support for failure