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/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -99,6 +99,11 @@ public:
 		return _flight_phase;
 	}
 
+	/**
+	 * Get the number of actuators
+	 */
+	virtual int numActuators() const = 0;
+
 protected:
 	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -132,17 +132,19 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NU
 		geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
 		geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
 
-		computeEffectivenessMatrix(geometry, matrix);
+		_num_actuators = computeEffectivenessMatrix(geometry, matrix);
 		return true;
 	}
 
 	return false;
 }
 
-void
+int
 ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry,
 		matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness)
 {
+	int num_actuators = 0;
+
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
 		// Get rotor axis
 		matrix::Vector3f axis(
@@ -173,6 +175,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		float ct = geometry.rotors[i].thrust_coef;
 		float km = geometry.rotors[i].moment_ratio;
 
+		if (fabsf(ct) < FLT_EPSILON) {
+			continue;
+		}
+
 		// Compute thrust generated by this rotor
 		matrix::Vector3f thrust = ct * axis;
 
@@ -184,5 +190,9 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 			effectiveness(j, i) = moment(j);
 			effectiveness(j + 3, i) = thrust(j);
 		}
+
+		num_actuators = i + 1;
 	}
+
+	return num_actuators;
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -70,16 +70,18 @@ public:
 		RotorGeometry rotors[NUM_ROTORS_MAX];
 	} MultirotorGeometry;
 
-	static void computeEffectivenessMatrix(const MultirotorGeometry &geometry,
+	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
-					       matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
+					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
 
 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix) override;
 
+	int numActuators() const override { return _num_actuators; }
 private:
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< parameter updates subscription */
 
 	bool _updated{true};
+	int _num_actuators{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -58,6 +58,7 @@ public:
 	 */
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
+	int numActuators() const override { return 7; }
 protected:
 	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -58,6 +58,7 @@ public:
 	 */
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
+	int numActuators() const override { return 10; }
 protected:
 	bool _updated{true};
 };

src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp

@@ -68,11 +68,13 @@ ControlAllocation::getAllocatedControl() const
 void
 ControlAllocation::setEffectivenessMatrix(
 	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
-	const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_trim)
+	const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_trim, int num_actuators)
 {
 	_effectiveness = effectiveness;
-	_actuator_trim = clipActuatorSetpoint(actuator_trim);
+	_actuator_trim = actuator_trim;
+	clipActuatorSetpoint(_actuator_trim);
 	_control_trim = _effectiveness * _actuator_trim;
+	_num_actuators = num_actuators;
 }
 
 const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &
@@ -115,34 +117,27 @@ ControlAllocation::setActuatorSetpoint(
 	_actuator_sp = actuator_sp;
 
 	// Clip
-	_actuator_sp = clipActuatorSetpoint(_actuator_sp);
+	clipActuatorSetpoint(_actuator_sp);
 
 	// Compute achieved control
 	_control_allocated = _effectiveness * _actuator_sp;
 
 }
 
-matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
+void
-ControlAllocation::clipActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator) const
+ControlAllocation::clipActuatorSetpoint(matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator) const
 {
-	matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> actuator_clipped;
+	for (int i = 0; i < _num_actuators; i++) {
-
-	for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) {
 		if (_actuator_max(i) < _actuator_min(i)) {
-			actuator_clipped(i) = _actuator_trim(i);
+			actuator(i) = _actuator_trim(i);
 
-		} else if (actuator_clipped(i) < _actuator_min(i)) {
+		} else if (actuator(i) < _actuator_min(i)) {
-			actuator_clipped(i) = _actuator_min(i);
+			actuator(i) = _actuator_min(i);
 
-		} else if (actuator_clipped(i) > _actuator_max(i)) {
+		} else if (actuator(i) > _actuator_max(i)) {
-			actuator_clipped(i) = _actuator_max(i);
+			actuator(i) = _actuator_max(i);
-
-		} else {
-			actuator_clipped(i) = actuator(i);
 		}
 	}
-
-	return actuator_clipped;
 }
 
 matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -105,7 +105,7 @@ public:
 	 * @param B Effectiveness matrix
 	 */
 	virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
-					    const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim);
+					    const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim, int num_actuators);
 
 	/**
 	 * Get the allocated actuator vector
@@ -187,10 +187,8 @@ public:
 	 * The output is in the range [min; max]
 	 *
 	 * @param actuator Actuator vector to clip
-	 *
-	 * @return Clipped actuator setpoint
 	 */
-	matrix::Vector<float, NUM_ACTUATORS> clipActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
+	void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
 
 	/**
 	 * Normalize the actuator setpoint between minimum and maximum values.
@@ -204,6 +202,10 @@ public:
 	matrix::Vector<float, NUM_ACTUATORS> normalizeActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator)
 	const;
 
+	virtual void updateParameters() {}
+
+	int numConfiguredActuators() const { return _num_actuators; }
+
 protected:
 	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness;  //< Effectiveness matrix
 	matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; 	//< Neutral actuator values
@@ -213,4 +215,5 @@ protected:
 	matrix::Vector<float, NUM_AXES> _control_sp;   		//< Control setpoint
 	matrix::Vector<float, NUM_AXES> _control_allocated;  	//< Allocated control
 	matrix::Vector<float, NUM_AXES> _control_trim;  	//< Control at trim actuator values
+	int _num_actuators{0};
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -44,9 +44,9 @@
 void
 ControlAllocationPseudoInverse::setEffectivenessMatrix(
 	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
-	const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_trim)
+	const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_trim, int num_actuators)
 {
-	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim);
+	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, num_actuators);
 	_mix_update_needed = true;
 }
 
@@ -69,7 +69,7 @@ ControlAllocationPseudoInverse::allocate()
 	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
 
 	// Clip
-	_actuator_sp = clipActuatorSetpoint(_actuator_sp);
+	clipActuatorSetpoint(_actuator_sp);
 
 	// Compute achieved control
 	_control_allocated = _effectiveness * _actuator_sp;

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp

@@ -55,7 +55,7 @@ public:
 
 	virtual void allocate() override;
 	virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
-					    const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim) override;
+					    const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim, int num_actuators) override;
 
 protected:
 	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -35,28 +35,36 @@
  * @file ControlAllocationSequentialDesaturation.cpp
  *
  * @author Roman Bapst <bapstroman@gmail.com>
+ * @author Beat Küng <beat-kueng@gmx.net>
  */
 
 #include "ControlAllocationSequentialDesaturation.hpp"
 
 
-
 void
 ControlAllocationSequentialDesaturation::allocate()
 {
 	//Compute new gains if needed
 	updatePseudoInverse();
 
-	// Allocate
+	switch (_param_mc_airmode.get()) {
-	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
+	case 1:
+		mixAirmodeRP();
+		break;
+
+	case 2:
+		mixAirmodeRPY();
+		break;
 
-	// go through control axes from lowest to highest priority and unsaturate the actuators
+	default:
-	for (unsigned i = 0; i < NUM_AXES; i++) {
+		mixAirmodeDisabled();
-		desaturateActuators(_actuator_sp, _axis_prio_increasing[i]);
+		break;
 	}
 
+	// TODO: thrust model (THR_MDL_FAC)
+
 	// Clip
-	_actuator_sp = clipActuatorSetpoint(_actuator_sp);
+	clipActuatorSetpoint(_actuator_sp);
 
 	// Compute achieved control
 	_control_allocated = _effectiveness * _actuator_sp;
@@ -64,39 +72,32 @@ ControlAllocationSequentialDesaturation::allocate()
 
 void ControlAllocationSequentialDesaturation::desaturateActuators(
 	ActuatorVector &actuator_sp,
-	const ControlAxis &axis)
+	const ActuatorVector &desaturation_vector, bool increase_only)
 {
-	ActuatorVector desaturation_vector = getDesaturationVector(axis);
-
 	float gain = computeDesaturationGain(desaturation_vector, actuator_sp);
 
-	actuator_sp = actuator_sp + gain * desaturation_vector;
+	if (increase_only && gain < 0.f) {
-
+		return;
-	gain = computeDesaturationGain(desaturation_vector, actuator_sp);
+	}
 
-	actuator_sp = actuator_sp + 0.5f * gain * desaturation_vector;
+	for (int i = 0; i < _num_actuators; i++) {
-}
+		actuator_sp(i) += gain * desaturation_vector(i);
+	}
 
-ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector(
+	gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp);
-	const ControlAxis &axis)
-{
-	ActuatorVector ret;
 
-	for (unsigned i = 0; i < NUM_ACTUATORS; i++) {
+	for (int i = 0; i < _num_actuators; i++) {
-		ret(i) = _mix(i, axis);
+		actuator_sp(i) += gain * desaturation_vector(i);
 	}
-
-	return ret;
 }
 
-
 float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector,
 		const ActuatorVector &actuator_sp)
 {
 	float k_min = 0.f;
 	float k_max = 0.f;
 
-	for (unsigned i = 0; i < NUM_ACTUATORS; i++) {
+	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) {
 			continue;
@@ -122,3 +123,118 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act
 	// Reduce the saturation as much as possible
 	return k_min + k_max;
 }
+
+void
+ControlAllocationSequentialDesaturation::mixAirmodeRP()
+{
+	// Airmode for roll and pitch, but not yaw
+
+	// Mix without yaw
+	ActuatorVector thrust_z;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		_actuator_sp(i) = _actuator_trim(i) +
+				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
+				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
+				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+	}
+
+	desaturateActuators(_actuator_sp, thrust_z);
+
+	// Mix yaw independently
+	mixYaw();
+}
+
+void
+ControlAllocationSequentialDesaturation::mixAirmodeRPY()
+{
+	// Airmode for roll, pitch and yaw
+
+	// Do full mixing
+	ActuatorVector thrust_z;
+	ActuatorVector yaw;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		_actuator_sp(i) = _actuator_trim(i) +
+				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) +
+				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
+				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
+				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+		yaw(i) = _mix(i, ControlAxis::YAW);
+	}
+
+	desaturateActuators(_actuator_sp, thrust_z);
+
+	// Unsaturate yaw (in case upper and lower bounds are exceeded)
+	// to prioritize roll/pitch over yaw.
+	desaturateActuators(_actuator_sp, yaw);
+}
+
+void
+ControlAllocationSequentialDesaturation::mixAirmodeDisabled()
+{
+	// Airmode disabled: never allow to increase the thrust to unsaturate a motor
+
+	// Mix without yaw
+	ActuatorVector thrust_z;
+	ActuatorVector roll;
+	ActuatorVector pitch;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		_actuator_sp(i) = _actuator_trim(i) +
+				  _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
+				  _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
+				  _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
+				  _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
+				  _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
+		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+		roll(i) = _mix(i, ControlAxis::ROLL);
+		pitch(i) = _mix(i, ControlAxis::PITCH);
+	}
+
+	// only reduce thrust
+	desaturateActuators(_actuator_sp, thrust_z, true);
+
+	// Reduce roll/pitch acceleration if needed to unsaturate
+	desaturateActuators(_actuator_sp, roll);
+	desaturateActuators(_actuator_sp, pitch);
+
+	// Mix yaw independently
+	mixYaw();
+}
+
+void
+ControlAllocationSequentialDesaturation::mixYaw()
+{
+	// Add yaw to outputs
+	ActuatorVector yaw;
+	ActuatorVector thrust_z;
+
+	for (int i = 0; i < _num_actuators; i++) {
+		_actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
+		yaw(i) = _mix(i, ControlAxis::YAW);
+		thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
+	}
+
+	// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
+	// and allow some yaw response at maximum thrust
+	ActuatorVector max_prev = _actuator_max;
+	_actuator_max += (_actuator_max - _actuator_min) * 0.15f;
+	desaturateActuators(_actuator_sp, yaw);
+	_actuator_max = max_prev;
+
+	// reduce thrust only
+	desaturateActuators(_actuator_sp, thrust_z, true);
+}
+
+void
+ControlAllocationSequentialDesaturation::updateParameters()
+{
+	updateParams();
+}

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
+	 * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
-	 * go through this list and if needed apply corrections to the demand of the corresponding axis in order to desaturate
+	 * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
-	 * the actuator vector.
+	 * 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.
+	 * @return desaturation gain
 	 */
-	void desaturateActuators(ActuatorVector &actuator_sp, const ControlAxis &axis);
+	float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp);
 
 	/**
-	 * Get desaturation vector.
+	 * Mix roll, pitch, yaw, thrust and set the actuator setpoint.
 	 *
-	 * @param 	axis 			Control axis
+	 * Desaturation behavior: airmode for roll/pitch:
-	 * @return 	ActuatorVector 	Column of the pseudo-inverse matrix corresponding to the given control axis.
+	 * 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.
 	 */
-	ActuatorVector getDesaturationVector(const ControlAxis &axis);
+	void mixAirmodeRP();
 
 	/**
-	 * Compute desaturation gain.
+	 * Mix roll, pitch, yaw, thrust and set the actuator setpoint.
 	 *
-	 * @param 	desaturation_vector Column of the pseudo-inverse matrix corresponding to a given control axis.
+	 * Desaturation behavior: full airmode for roll/pitch/yaw:
-	 * @param 	Actuator setpoint vector.
+	 * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
-	 * @return 	Gain which eliminates the saturation of the highest saturated actuator.
+	 * while giving priority to roll and pitch over yaw.
 	 */
-	float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp);
+	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
+	);
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -249,6 +249,10 @@ ControlAllocator::Run()
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
 
+		if (_control_allocation) {
+			_control_allocation->updateParameters();
+		}
+
 		updateParams();
 		parameters_updated();
 	}
@@ -520,6 +524,7 @@ int ControlAllocator::print_status()
 		const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness = _control_allocation->getEffectivenessMatrix();
 		PX4_INFO("Effectiveness.T =");
 		effectiveness.T().print();
+		PX4_INFO("Configured actuators: %i", _control_allocation->numConfiguredActuators());
 	}
 
 	// Print perf