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/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>
-ControlAllocation::clipActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator) const
+void
+ControlAllocation::clipActuatorSetpoint(matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator) const
 {
-	matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> actuator_clipped;
-
-	for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) {
+	for (int i = 0; i < _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)) {
-			actuator_clipped(i) = _actuator_min(i);
+		} else if (actuator(i) < _actuator_min(i)) {
+			actuator(i) = _actuator_min(i);
 
-		} else if (actuator_clipped(i) > _actuator_max(i)) {
-			actuator_clipped(i) = _actuator_max(i);
-
-		} else {
-			actuator_clipped(i) = actuator(i);
+		} else if (actuator(i) > _actuator_max(i)) {
+			actuator(i) = _actuator_max(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
-	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
+	switch (_param_mc_airmode.get()) {
+	case 1:
+		mixAirmodeRP();
+		break;
 
-	// go through control axes from lowest to highest priority and unsaturate the actuators
-	for (unsigned i = 0; i < NUM_AXES; i++) {
-		desaturateActuators(_actuator_sp, _axis_prio_increasing[i]);
+	case 2:
+		mixAirmodeRPY();
+		break;
+
+	default:
+		mixAirmodeDisabled();
+		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,31 +72,24 @@ 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;
-
-	gain = computeDesaturationGain(desaturation_vector, actuator_sp);
-
-	actuator_sp = actuator_sp + 0.5f * gain * desaturation_vector;
-}
-
-ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector(
-	const ControlAxis &axis)
-{
-	ActuatorVector ret;
-
-	for (unsigned i = 0; i < NUM_ACTUATORS; i++) {
-		ret(i) = _mix(i, axis);
+	if (increase_only && gain < 0.f) {
+		return;
 	}
 
-	return ret;
-}
+	for (int i = 0; i < _num_actuators; i++) {
+		actuator_sp(i) += gain * desaturation_vector(i);
+	}
 
+	gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp);
+
+	for (int i = 0; i < _num_actuators; i++) {
+		actuator_sp(i) += gain * desaturation_vector(i);
+	}
+}
 
 float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector,
 		const ActuatorVector &actuator_sp)
@@ -96,7 +97,7 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act
 	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
-	 * 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
+	);
 };