control_allocator: change SequentialDesaturation to existing MC mixer

And limit the operations to the number of configured outputs.

Only using the number of configured actuators reduces CPU load by ~2% on
F7 @1khz.

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp

@@ -45,45 +45,84 @@
 
 #include "ControlAllocationPseudoInverse.hpp"
 
-class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse
+#include <px4_platform_common/module_params.h>
+
+class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams
 {
 public:
 
-	ControlAllocationSequentialDesaturation() = default;
+	ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {}
 	virtual ~ControlAllocationSequentialDesaturation() = default;
 
 	void allocate() override;
 
+	void updateParameters() override;
 private:
 
 	/**
-	 * List of control axis used for desaturating the actuator vector. The desaturation logic will sequentially
-	 * go through this list and if needed apply corrections to the demand of the corresponding axis in order to desaturate
-	 * the actuator vector.
+	 * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
+	 * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
+	 * acceleration on a specific axis.
+	 * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
+	 * saturation will be minimized by shifting the vertical thrust setpoint, without changing the
+	 * roll/pitch/yaw accelerations.
+	 *
+	 * Note that as we only slide along the given axis, in extreme cases outputs can still contain values
+	 * outside of [min_output, max_output].
+	 *
+	 * @param actuator_sp Actuator setpoint, vector that is modified
+	 * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
+	 * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector
 	 */
-	ControlAxis _axis_prio_increasing [NUM_AXES] = {ControlAxis::YAW, ControlAxis::THRUST_Y, ControlAxis::THRUST_X, ControlAxis::THRUST_Z, ControlAxis::PITCH, ControlAxis::ROLL};
+	void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector,
+				 bool increase_only = false);
 
 	/**
-	 * Desaturate actuator setpoint vector.
+	 * Computes the gain k by which desaturation_vector has to be multiplied
+	 * in order to unsaturate the output that has the greatest saturation.
 	 *
-	 * @return Desaturated actuator setpoint vector.
-	 */
-	void desaturateActuators(ActuatorVector &actuator_sp, const ControlAxis &axis);
-
-	/**
-	 * Get desaturation vector.
-	 *
-	 * @param 	axis 			Control axis
-	 * @return 	ActuatorVector 	Column of the pseudo-inverse matrix corresponding to the given control axis.
-	 */
-	ActuatorVector getDesaturationVector(const ControlAxis &axis);
-
-	/**
-	 * Compute desaturation gain.
-	 *
-	 * @param 	desaturation_vector Column of the pseudo-inverse matrix corresponding to a given control axis.
-	 * @param 	Actuator setpoint vector.
-	 * @return 	Gain which eliminates the saturation of the highest saturated actuator.
+	 * @return desaturation gain
 	 */
 	float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp);
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the actuator setpoint.
+	 *
+	 * Desaturation behavior: airmode for roll/pitch:
+	 * thrust is increased/decreased as much as required to meet the demanded roll/pitch.
+	 * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
+	 */
+	void mixAirmodeRP();
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the actuator setpoint.
+	 *
+	 * Desaturation behavior: full airmode for roll/pitch/yaw:
+	 * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
+	 * while giving priority to roll and pitch over yaw.
+	 */
+	void mixAirmodeRPY();
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the actuator setpoint.
+	 *
+	 * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
+	 * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
+	 * Thrust can be reduced to unsaturate the upper side.
+	 * @see mixYaw() for the exact yaw behavior.
+	 */
+	void mixAirmodeDisabled();
+
+	/**
+	 * Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust).
+	 *
+	 * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
+	 * some yaw control on the upper end. On the lower end thrust will never be increased,
+	 * but yaw is decreased as much as required.
+	 */
+	void mixYaw();
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode   ///< air-mode
+	);
 };