Enable directly injecting motor failures using e.g. failure motor off -i 1

Only if SYS_FAILURE_EN is enabled and CA_FAILURE_MODE is > 0.

src/modules/commander/Commander.cpp

@@ -1956,6 +1956,7 @@ void Commander::run()
 			fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
 			fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
 			fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
+			fd_status.motor_stop_mask = _failure_detector.getMotorStopMask();
 			fd_status.timestamp = hrt_absolute_time();
 			_failure_detector_status_pub.publish(fd_status);
 		}

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -89,6 +89,7 @@ public:
 	const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; }
 	float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
 	uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; }
+	uint16_t getMotorStopMask() { return _failure_injector.getMotorStopMask(); }
 
 private:
 	void updateAttitudeStatus(const vehicle_status_s &vehicle_status);

src/modules/commander/failure_detector/FailureInjector.cpp

@@ -33,10 +33,23 @@
 
 #include "FailureInjector.hpp"
 
+#include <parameters/param.h>
 #include <uORB/topics/actuator_motors.h>
 
+FailureInjector::FailureInjector()
+{
+	int32_t param_sys_failure_en = 0;
+
+	if ((param_get(param_find("SYS_FAILURE_EN"), &param_sys_failure_en) == PX4_OK)
+	    && (param_sys_failure_en == 1)) {
+		_failure_injection_enabled = true;
+	}
+}
+
 void FailureInjector::update()
 {
+	if (!_failure_injection_enabled) { return; }
+
 	vehicle_command_s vehicle_command;
 
 	while (_vehicle_command_sub.update(&vehicle_command)) {
@@ -59,14 +72,21 @@ void FailureInjector::update()
 			switch (failure_type) {
 			case vehicle_command_s::FAILURE_TYPE_OK:
 				supported = true;
-				PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i);
+				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 no esc telemetry", i);
+				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;
 
@@ -91,6 +111,8 @@ void FailureInjector::update()
 
 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;

src/modules/commander/failure_detector/FailureInjector.hpp

@@ -42,13 +42,18 @@
 class FailureInjector
 {
 public:
+	FailureInjector();
+
 	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<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
 
+	bool _failure_injection_enabled = false;
+	uint32_t _motor_stop_mask{};
 	uint32_t _esc_telemetry_blocked_mask{};
 	uint32_t _esc_telemetry_wrong_mask{};
 };