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

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;
 		}