feat(failure_injection): enhance failure injection with RC switch support and instance bitmasking

feat(failure_injection): add disabled failure injection manager and system command support for v5x and v6x boards

Author: Claudio Chies

ROMFS/px4fmu_common/init.d/rcS

@@ -714,6 +714,14 @@ else
 		unset RC_VTXTABLE
 	fi
 
+	#
+	# Start the failure injection manager. SYS_FAILURE_EN only exists when the module is built
+	#
+	if param compare -s SYS_FAILURE_EN 1
+	then
+		failure_injection_manager start
+	fi
+
 	#
 	# Set additional parameters and env variables for selected AUTOSTART.
 	#

boards/auterion/fmu-v6x/default.px4board

@@ -48,6 +48,7 @@ CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
+CONFIG_MODULES_FAILURE_INJECTION_MANAGER=n
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
@@ -82,6 +83,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_FAILURE=n
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y

boards/px4/fmu-v5x/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=n
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
@@ -90,6 +91,7 @@ CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_FAILURE=n
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y

docs/en/debug/failure_injection.md

@@ -5,7 +5,7 @@ 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.
 
-Failure injection must also be be supported by the current simulator, and the set of supported failures is simulator-dependent.
+Failures can be injected both in simulation and on real hardware. In simulation the available failures depend on the simulator. On hardware the `off` (stop publishing) and `stuck` (freeze the last value) types are supported for the `gyro`, `accel`, `mag`, `baro`, `distance_sensor` and `gps` components; this requires firmware built with the failure-injection module.
 
 ::: info
 PX4 may accept a command to set a particular failure mode even it that mode is not supported by your simulator.
@@ -24,7 +24,7 @@ Failures can be injected using the [failure system command](../modules/modules_c
 The full syntax of the [failure](../modules/modules_command.md#failure) command is:
 
 ```sh
-failure <component> <failure_type> [-i <instance_number>]
+failure <component> <failure_type> [-i <instance_number>] [-m <instance_bitmask>]
 ```
 
 where:
@@ -58,11 +58,23 @@ where:
   - `intermittent`: Publish intermittently
 - _instance number_ (optional): Instance number of affected sensor.
   0 (default) indicates all sensors of specified type.
+- _instance bitmask_ (optional): address several instances at once (bit 0 = first instance, bit 1 = second, …; decimal or `0x` hex). Used only when `-i` is omitted. Example: `-m 0x5` targets instances 1 and 3.
 
 ::: info
 The simulated GPS (SITL) implements only the `off`, `stuck`, and `wrong` failure modes; the other failure types have no effect on it.
 :::
 
+## RC Switch Trigger
+
+A failure can also be injected from an RC switch, without a console or telemetry link. This is useful for in-flight hardware testing. It is configured with the following parameters:
+
+- [SYS_FAIL_RC_SRC](../advanced_config/parameter_reference.md#SYS_FAIL_RC_SRC): the auxiliary RC input that triggers the failure — `0` disables it, `1`–`6` select AUX1–AUX6 (mapped via `RC_MAP_AUXn`).
+- [SYS_FAIL_RC_UNIT](../advanced_config/parameter_reference.md#SYS_FAIL_RC_UNIT): the affected component (the `FAILURE_UNIT` value; e.g. `101` = motor).
+- [SYS_FAIL_RC_MODE](../advanced_config/parameter_reference.md#SYS_FAIL_RC_MODE): the failure type (the `FAILURE_TYPE` value; e.g. `1` = off).
+- [SYS_FAIL_RC_INST](../advanced_config/parameter_reference.md#SYS_FAIL_RC_INST): the affected instance (1-based; `0` = all instances).
+
+While the selected aux switch is on the configured failure is injected; switching it back off clears the failure. The injection goes through the same path as the console/MAVLink commands, so for a motor it stops the motor exactly as `failure motor off` does (which also requires [CA_FAILURE_MODE](../advanced_config/parameter_reference.md#CA_FAILURE_MODE)).
+
 ## MAVSDK Failure Plugin
 
 The [MAVSDK failure plugin](https://mavsdk.mavlink.io/main/en/cpp/api_reference/classmavsdk_1_1_failure.html) can be used to programmatically inject failures.

msg/FailureInjection.msg

@@ -29,6 +29,7 @@ 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
+uint8 FAILURE_UNIT_SYSTEM_ESC = 106
 
 uint16[4] instance_mask # Bit i targets instance (i+1); 0xFFFF = all instances
 

msg/versioned/VehicleCommand.msg

@@ -170,6 +170,7 @@ 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
+uint8 FAILURE_UNIT_SYSTEM_ESC = 106
 
 uint8 FAILURE_TYPE_OK = 0
 uint8 FAILURE_TYPE_OFF = 1

src/modules/failure_injection_manager/FailureInjectionManager.cpp

@@ -36,19 +36,26 @@
 #include <cmath>
 
 #include <drivers/drv_hrt.h>
+#include <px4_platform_common/defines.h>
 #include <px4_platform_common/log.h>
 
 ModuleBase::Descriptor FailureInjectionManager::desc{task_spawn, custom_command, print_usage};
 
 FailureInjectionManager::FailureInjectionManager() :
+	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
 {
 }
 
 bool FailureInjectionManager::init()
 {
 	if (!_vehicle_command_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
+		PX4_ERR("vehicle_command callback registration failed");
+		return false;
+	}
+
+	if (!_manual_control_setpoint_sub.registerCallback()) {
+		PX4_ERR("manual_control_setpoint callback registration failed");
 		return false;
 	}
 
@@ -61,6 +68,7 @@ void FailureInjectionManager::Run()
 {
 	if (should_exit()) {
 		_vehicle_command_sub.unregisterCallback();
+		_manual_control_setpoint_sub.unregisterCallback();
 		exit_and_cleanup(desc);
 		return;
 	}
@@ -73,6 +81,8 @@ void FailureInjectionManager::Run()
 		}
 	}
 
+	evaluateRcInjection();
+
 	// Republish only when the configuration actually changed
 	if (_table.changed()) {
 		failure_injection_s msg{};
@@ -83,15 +93,64 @@ void FailureInjectionManager::Run()
 	}
 }
 
+void FailureInjectionManager::evaluateRcInjection()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	float value = NAN;
+
+	if (manual_control_setpoint.valid) {
+		switch (_param_sys_fail_rc_src.get()) {
+		case 1: value = manual_control_setpoint.aux1; break;
+
+		case 2: value = manual_control_setpoint.aux2; break;
+
+		case 3: value = manual_control_setpoint.aux3; break;
+
+		case 4: value = manual_control_setpoint.aux4; break;
+
+		case 5: value = manual_control_setpoint.aux5; break;
+
+		case 6: value = manual_control_setpoint.aux6; break;
+
+		default: break; // 0 = disabled, or out of range
+		}
+	}
+
+	const bool triggered = value > 0.5f;
+
+	if (triggered && !_rc_active) {
+		_rc_active_unit = static_cast<uint8_t>(_param_sys_fail_rc_unit.get());
+		_rc_active_instance = static_cast<uint8_t>(_param_sys_fail_rc_inst.get());
+		_rc_active = true;
+
+		_table.inject(_rc_active_unit, static_cast<uint8_t>(_param_sys_fail_rc_mode.get()), _rc_active_instance);
+
+	} else if (!triggered && _rc_active) {
+		_table.inject(_rc_active_unit, failure_injection_s::FAILURE_TYPE_OK, _rc_active_instance);
+		_rc_active = false;
+	}
+}
+
 void FailureInjectionManager::handleCommand(const vehicle_command_s &cmd)
 {
 	const uint8_t unit = static_cast<uint8_t>(lroundf(cmd.param1));
 	const uint8_t type = static_cast<uint8_t>(lroundf(cmd.param2));
-	const uint8_t instance = static_cast<uint8_t>(lroundf(cmd.param3));
+
+	failure_injection::FailureTable::AckResult ack;
+
+	if (PX4_ISFINITE(cmd.param3)) {
+		ack = _table.inject(unit, type, static_cast<uint8_t>(lroundf(cmd.param3)));
+
+	} else {
+		const uint16_t mask = PX4_ISFINITE(cmd.param4) ? static_cast<uint16_t>(lroundf(cmd.param4)) : 0;
+		ack = _table.injectMask(unit, type, mask);
+	}
 
 	uint8_t result;
 
-	switch (_table.inject(unit, type, instance)) {
+	switch (ack) {
 	case failure_injection::FailureTable::AckResult::Accepted:
 		result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 		break;
@@ -157,14 +216,20 @@ int FailureInjectionManager::print_usage(const char *reason)
 		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`.
+Central module for handling failure injection. It collects failure requests, tracks
+the set of active failures, and publishes them on the `failure_injection` topic for
+the apply-sites to act on.
+
+Failures can be triggered through:
+- `MAV_CMD_INJECT_FAILURE` over MAVLink (e.g. from MAVSDK)
+- the `failure` console command
+- an RC switch: `SYS_FAIL_RC_SRC` selects the aux input, and `SYS_FAIL_RC_UNIT` /
+  `SYS_FAIL_RC_MODE` / `SYS_FAIL_RC_INST` define the failure applied while it is on
+
+Requires `SYS_FAILURE_EN` to be set; the startup script only starts this module when it is.
 
-Failure injection is gated by the `SYS_FAILURE_EN` parameter, which the startup
-script checks before starting this module.
+Failures can be applied both in simulation and on real hardware, where the apply-sites are
+compiled in alongside this module.
 
 )DESCR_STR");
 

src/modules/failure_injection_manager/FailureInjectionManager.hpp

@@ -46,14 +46,16 @@
 #include "FailureTable.hpp"
 
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/failure_injection.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
-class FailureInjectionManager : public ModuleBase, public px4::WorkItem
+class FailureInjectionManager : public ModuleBase, public ModuleParams, public px4::WorkItem
 {
 public:
 	FailureInjectionManager();
@@ -78,10 +80,24 @@ class FailureInjectionManager : public ModuleBase, public px4::WorkItem
 	void handleCommand(const vehicle_command_s &cmd);
 	void publishAck(const vehicle_command_s &cmd, uint8_t result);
 
+	void evaluateRcInjection();
+
 	uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)};
+	uORB::SubscriptionCallbackWorkItem _manual_control_setpoint_sub{this, ORB_ID(manual_control_setpoint)};
 
 	uORB::Publication<failure_injection_s>  _failure_injection_pub{ORB_ID(failure_injection)};
 	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	failure_injection::FailureTable _table;
+
+	bool    _rc_active{false};
+	uint8_t _rc_active_unit{0};
+	uint8_t _rc_active_instance{0};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SYS_FAIL_RC_SRC>) _param_sys_fail_rc_src,
+		(ParamInt<px4::params::SYS_FAIL_RC_UNIT>) _param_sys_fail_rc_unit,
+		(ParamInt<px4::params::SYS_FAIL_RC_MODE>) _param_sys_fail_rc_mode,
+		(ParamInt<px4::params::SYS_FAIL_RC_INST>) _param_sys_fail_rc_inst
+	)
 };

src/modules/failure_injection_manager/FailureInjectionManagerTest.cpp

@@ -62,9 +62,12 @@ TEST(FailureTable, SupportedCatalogueMatchesInventory)
 	EXPECT_TRUE(FailureTable::isSupported(GPS, WRONG));
 	EXPECT_TRUE(FailureTable::isSupported(MOTOR, WRONG));
 	EXPECT_FALSE(FailureTable::isSupported(GYRO, GARBAGE)); // GARBAGE unimplemented
+	// Distance sensor (rangefinder) supports OFF/STUCK on hardware, but not WRONG.
+	EXPECT_TRUE(FailureTable::isSupported(failure_injection_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR, OFF));
+	EXPECT_TRUE(FailureTable::isSupported(failure_injection_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR, STUCK));
+	EXPECT_FALSE(FailureTable::isSupported(failure_injection_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR, WRONG));
 	// 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)
@@ -196,3 +199,33 @@ TEST(FailureTable, AddingInstanceToExistingEntryDoesNotCountAsNew)
 	// gyro entry now covers instances 1 and 2
 	EXPECT_EQ(msg.instance_mask[0], 0x1 | 0x2);
 }
+
+TEST(FailureTable, InjectMaskSetsMultipleInstances)
+{
+	FailureTable table;
+	// bits 0 and 2 -> instances 1 and 3 (MAV_CMD_INJECT_FAILURE param4)
+	EXPECT_EQ(table.injectMask(GYRO, OFF, 0x5), AckResult::Accepted);
+	EXPECT_TRUE(table.changed());
+	ASSERT_EQ(table.count(), 1);
+
+	failure_injection_s msg{};
+	table.fill(msg);
+	EXPECT_EQ(msg.unit[0], GYRO);
+	EXPECT_EQ(msg.failure_type[0], OFF);
+	EXPECT_EQ(msg.instance_mask[0], 0x5);
+}
+
+TEST(FailureTable, InjectMaskZeroIsAcceptedNoOp)
+{
+	FailureTable table;
+	EXPECT_EQ(table.injectMask(GYRO, OFF, 0), AckResult::Accepted);
+	EXPECT_FALSE(table.changed());
+	EXPECT_EQ(table.count(), 0);
+}
+
+TEST(FailureTable, InjectMaskUnsupportedIsRejected)
+{
+	FailureTable table;
+	EXPECT_EQ(table.injectMask(GYRO, WRONG, 0x1), AckResult::Unsupported);
+	EXPECT_EQ(table.count(), 0);
+}

src/modules/failure_injection_manager/FailureTable.cpp

@@ -47,6 +47,7 @@ bool FailureTable::isSupported(uint8_t unit, uint8_t type)
 	case failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL:
 	case failure_injection_s::FAILURE_UNIT_SENSOR_MAG:
 	case failure_injection_s::FAILURE_UNIT_SENSOR_BARO:
+	case failure_injection_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR:
 		return type == failure_injection_s::FAILURE_TYPE_OK
 		       || type == failure_injection_s::FAILURE_TYPE_OFF
 		       || type == failure_injection_s::FAILURE_TYPE_STUCK;
@@ -129,14 +130,17 @@ void FailureTable::compact()
 }
 
 FailureTable::AckResult FailureTable::inject(uint8_t unit, uint8_t type, uint8_t instance)
+{
+	return injectMask(unit, type, instanceToMask(instance));
+}
+
+FailureTable::AckResult FailureTable::injectMask(uint8_t unit, uint8_t type, uint16_t mask)
 {
 	// 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;