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.

Author: MaEtUgR

msg/ControlAllocatorStatus.msg

@@ -19,3 +19,4 @@ int8[16] actuator_saturation            # Indicates actuator saturation status.
                                         # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value.
 
 uint16 handled_motor_failure_mask        # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector
+uint16 motor_stop_mask                   # Bitmaks of motors stopped by failure injection

msg/FailureDetectorStatus.msg

@@ -12,3 +12,4 @@ bool fd_motor
 
 float32 imbalanced_prop_metric      # Metric of the imbalanced propeller check (low-passed)
 uint16 motor_failure_mask           # Bit-mask with motor indices, indicating critical motor failures
+uint16 motor_stop_mask              # Bitmaks of motors stopped by failure injection

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 @@ class FailureDetector : public ModuleParams
 	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{};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -641,6 +641,7 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
 
 	// Handled motor failures
 	control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask;
+	control_allocator_status.motor_stop_mask = _motor_stop_mask;
 
 	_control_allocator_status_pub[matrix_index].publish(control_allocator_status);
 }
@@ -665,7 +666,9 @@ ControlAllocator::publish_actuator_controls()
 	int actuator_idx = 0;
 	int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
 
-	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask;
+	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors()
+				  | _handled_motor_failure_bitmask
+				  | _motor_stop_mask;
 
 	// motors
 	int motors_idx;
@@ -716,8 +719,13 @@ ControlAllocator::check_for_motor_failures()
 
 	if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE
 	    && _failure_detector_status_sub.update(&failure_detector_status)) {
-		if (failure_detector_status.fd_motor) {
 
+		if (_motor_stop_mask != failure_detector_status.motor_stop_mask) {
+			_motor_stop_mask = failure_detector_status.motor_stop_mask;
+			PX4_WARN("Stopping motors (%d)", _motor_stop_mask);
+		}
+
+		if (failure_detector_status.fd_motor) {
 			if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) {
 				// motor failure bitmask changed
 				switch ((FailureMode)_param_ca_failure_mode.get()) {

src/modules/control_allocator/ControlAllocator.hpp

@@ -201,6 +201,7 @@ class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParam
 	// Reflects motor failures that are currently handled, not motor failures that are reported.
 	// For example, the system might report two motor failures, but only the first one is handled by CA
 	uint16_t _handled_motor_failure_bitmask{0};
+	uint16_t _motor_stop_mask{0};
 
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */