control_allocator: remove failed motor from effectiveness

- limit to handling only 1 motor failure
- Log which motor failures are handled
- Remove motor from effectiveness matrix without
  recomputing the scale / normalization

msg/control_allocator_status.msg

@@ -19,3 +19,5 @@ int8 ACTUATOR_SATURATION_LOWER     = -2 # The actuator is saturated (with a valu
 int8[16] actuator_saturation            # Indicates actuator saturation status.
                                         # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved.
                                         # 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

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -98,6 +98,15 @@ public:
 	 */
 	virtual void allocate() = 0;
 
+	/**
+	 * Set actuator failure flag
+	 * This prevents a change of the scaling in the matrix normalization step
+	 * in case of a motor failure.
+	 *
+	 * @param failure  Motor failure flag
+	 */
+	void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; }
+
 	/**
 	 * Set the control effectiveness matrix
 	 *
@@ -234,4 +243,5 @@ protected:
 	matrix::Vector<float, NUM_AXES> _control_trim; 		///< Control at trim actuator values
 	int _num_actuators{0};
 	bool _normalize_rpy{false};				///< if true, normalize roll, pitch and yaw columns
+	bool _had_actuator_failure{false};
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -59,7 +59,7 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 	if (_mix_update_needed) {
 		matrix::geninv(_effectiveness, _mix);
 
-		if (_normalization_needs_update) {
+		if (_normalization_needs_update && !_had_actuator_failure) {
 			updateControlAllocationMatrixScale();
 			_normalization_needs_update = false;
 		}

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -92,8 +92,8 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act
 	float k_max = 0.f;
 
 	for (int i = 0; i < _num_actuators; i++) {
-		// Avoid division by zero. If desaturation_vector(i) is zero, there's nothing we can do to unsaturate anyway
-		if (fabsf(desaturation_vector(i)) < FLT_EPSILON) {
+		// Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains
+		if (fabsf(desaturation_vector(i)) < 0.2f) {
 			continue;
 		}
 

src/modules/control_allocator/ControlAllocator.cpp

@@ -205,7 +205,7 @@ ControlAllocator::update_allocation_method(bool force)
 bool
 ControlAllocator::update_effectiveness_source()
 {
-	EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
+	const EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
 
 	if (_effectiveness_source_id != source) {
 
@@ -305,8 +305,12 @@ ControlAllocator::Run()
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
 
-		updateParams();
-		parameters_updated();
+		if (_handled_motor_failure_bitmask == 0) {
+			// We don't update the geometry after an actuator failure, as it could lead to unexpected results
+			// (e.g. a user could add/remove motors, such that the bitmask isn't correct anymore)
+			updateParams();
+			parameters_updated();
+		}
 	}
 
 	if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) {
@@ -376,6 +380,8 @@ ControlAllocator::Run()
 	if (do_update) {
 		_last_run = now;
 
+		check_for_motor_failures();
+
 		update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE);
 
 		// Set control setpoint vector(s)
@@ -503,6 +509,27 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
 			}
 		}
 
+		// Handle failed actuators
+		if (_handled_motor_failure_bitmask) {
+			actuator_idx = 0;
+			memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix));
+
+			for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) {
+				int selected_matrix = _control_allocation_selection_indexes[actuator_idx];
+
+				if (_handled_motor_failure_bitmask & (1 << motors_idx)) {
+					ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix];
+
+					for (int i = 0; i < NUM_AXES; i++) {
+						matrix(i, actuator_idx_matrix[selected_matrix]) = 0.0f;
+					}
+				}
+
+				++actuator_idx_matrix[selected_matrix];
+				++actuator_idx;
+			}
+		}
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 			_control_allocation[i]->setActuatorMin(minimum[i]);
 			_control_allocation[i]->setActuatorMax(maximum[i]);
@@ -557,7 +584,8 @@ ControlAllocator::publish_control_allocator_status()
 	control_allocator_status.allocated_thrust[2] = allocated_control(5);
 
 	// Unallocated control
-	matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[0]->getControlSetpoint() - allocated_control;
+	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[0]->getControlSetpoint() -
+			allocated_control;
 	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
 	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
 	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
@@ -585,6 +613,9 @@ ControlAllocator::publish_control_allocator_status()
 		}
 	}
 
+	// Handled motor failures
+	control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask;
+
 	_control_allocator_status_pub.publish(control_allocator_status);
 }
 
@@ -604,7 +635,7 @@ ControlAllocator::publish_actuator_controls()
 	int actuator_idx = 0;
 	int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {};
 
-	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors();
+	uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask;
 
 	// motors
 	int motors_idx;
@@ -648,6 +679,56 @@ ControlAllocator::publish_actuator_controls()
 	}
 }
 
+void
+ControlAllocator::check_for_motor_failures()
+{
+	failure_detector_status_s failure_detector_status;
+
+	if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE
+	    && _failure_detector_status_sub.update(&failure_detector_status)) {
+		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()) {
+				case FailureMode::REMOVE_FIRST_FAILING_MOTOR: {
+						// Count number of failed motors
+						const int num_motors_failed = math::countSetBits(failure_detector_status.motor_failure_mask);
+
+						// Only handle if it is the first failure
+						if (_handled_motor_failure_bitmask == 0 && num_motors_failed == 1) {
+							_handled_motor_failure_bitmask = failure_detector_status.motor_failure_mask;
+							PX4_WARN("Removing motor from allocation (0x%x)", _handled_motor_failure_bitmask);
+
+							for (int i = 0; i < _num_control_allocation; ++i) {
+								_control_allocation[i]->setHadActuatorFailure(true);
+							}
+
+							update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE);
+						}
+					}
+					break;
+
+				default:
+					break;
+				}
+
+			}
+
+		} else if (_handled_motor_failure_bitmask != 0) {
+			// Clear bitmask completely
+			PX4_INFO("Restoring all motors");
+			_handled_motor_failure_bitmask = 0;
+
+			for (int i = 0; i < _num_control_allocation; ++i) {
+				_control_allocation[i]->setHadActuatorFailure(false);
+			}
+
+			update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE);
+		}
+	}
+}
+
 int ControlAllocator::task_spawn(int argc, char *argv[])
 {
 	ControlAllocator *instance = new ControlAllocator();
@@ -716,6 +797,11 @@ int ControlAllocator::print_status()
 		PX4_INFO("  Configured actuators: %i", _control_allocation[i]->numConfiguredActuators());
 	}
 
+	if (_handled_motor_failure_bitmask) {
+		PX4_INFO("Failed motors: %i (0x%x)", math::countSetBits(_handled_motor_failure_bitmask),
+			 _handled_motor_failure_bitmask);
+	}
+
 	// Print perf
 	perf_print_counter(_loop_perf);
 

src/modules/control_allocator/ControlAllocator.hpp

@@ -74,6 +74,7 @@
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/failure_detector_status.h>
 
 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
 {
@@ -128,6 +129,8 @@ private:
 
 	void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason);
 
+	void check_for_motor_failures();
+
 	void publish_control_allocator_status();
 
 	void publish_actuator_controls();
@@ -152,6 +155,11 @@ private:
 		HELICOPTER = 10,
 	};
 
+	enum class FailureMode {
+		IGNORE = 0,
+		REMOVE_FIRST_FAILING_MOTOR = 1,
+	};
+
 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
 	ActuatorEffectiveness *_actuator_effectiveness{nullptr}; 	///< class providing actuator effectiveness
 
@@ -175,10 +183,15 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
 
 	matrix::Vector3f _torque_sp;
 	matrix::Vector3f _thrust_sp;
 
+	// 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};
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};
@@ -193,6 +206,7 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
 		(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
+		(ParamInt<px4::params::CA_FAILURE_MODE>) _param_ca_failure_mode,
 		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
 	)
 

src/modules/control_allocator/module.yaml

@@ -436,6 +436,19 @@ parameters:
             max: 1
             default: [0.05, 0.15, 0.25, 0.35, 0.45]
 
+        # Others
+        CA_FAILURE_MODE:
+            description:
+                short: Motor failure handling mode
+                long: |
+                    This is used to specify how to handle motor failures
+                    reported by failure detector.
+            type: enum
+            values:
+                0: Ignore
+                1: Remove first failed motor from effectiveness
+            default: 0
+
 # Mixer
 mixer:
     actuator_types: