control_allocator: update matrix normalization

- only normalize rpy for MC matrices
- for thrust use the 3D vector, so it works for FW and tilt rotors as well

This keeps MC unchanged.

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -132,6 +132,16 @@ public:
 		}
 	}
 
+	/**
+	 * Query if the roll, pitch and yaw columns of the mixing matrix should be normalized
+	 */
+	virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const
+	{
+		for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
+			normalize[i] = false;
+		}
+	}
+
 	/**
 	 * Get the control effectiveness matrix if updated
 	 *

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -50,6 +50,11 @@ public:
 		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
 	}
 
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+	}
+
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp) override;
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -85,6 +85,11 @@ public:
 		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
 	}
 
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+	}
+
 	static int computeEffectivenessMatrix(const Geometry &geometry,
 					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -66,6 +66,12 @@ public:
 		allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
 	}
 
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+		normalize[1] = false;
+	}
+
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp) override;
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -63,6 +63,12 @@ public:
 		allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
 	}
 
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+		normalize[1] = false;
+	}
+
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	const char *name() const override { return "VTOL Tailsitter"; }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -66,6 +66,12 @@ public:
 		allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
 	}
 
+	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
+	{
+		normalize[0] = true;
+		normalize[1] = false;
+	}
+
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -216,6 +216,8 @@ public:
 
 	int numConfiguredActuators() const { return _num_actuators; }
 
+	void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; }
+
 protected:
 	friend class ControlAllocator; // for _actuator_sp
 
@@ -230,4 +232,5 @@ protected:
 	matrix::Vector<float, NUM_AXES> _control_sp;   		///< Control setpoint
 	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
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -63,46 +63,60 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 void
 ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
 {
-	// Same scale on roll and pitch
-	const float roll_norm_sq = _mix.col(0).norm_squared();
-	const float pitch_norm_sq = _mix.col(1).norm_squared();
-	_control_allocation_scale(0) = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f));
-	_control_allocation_scale(1) = _control_allocation_scale(0);
+	if (_normalize_rpy) {
+		// Same scale on roll and pitch
+		const float roll_norm_sq = _mix.col(0).norm_squared();
+		const float pitch_norm_sq = _mix.col(1).norm_squared();
+		_control_allocation_scale(0) = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f));
+		_control_allocation_scale(1) = _control_allocation_scale(0);
 
-	if (_control_allocation_scale(0) > FLT_EPSILON) {
-		_mix.col(0) /= _control_allocation_scale(0);
-		_mix.col(1) /= _control_allocation_scale(1);
+		if (_control_allocation_scale(0) > FLT_EPSILON) {
+			_mix.col(0) /= _control_allocation_scale(0);
+			_mix.col(1) /= _control_allocation_scale(1);
+		}
+
+		// Scale yaw separately
+		_control_allocation_scale(2) = _mix.col(2).max();
+
+		if (_control_allocation_scale(2) > FLT_EPSILON) {
+			_mix.col(2) /= _control_allocation_scale(2);
+		}
+
+	} else {
+		_control_allocation_scale(0) = 1.f;
+		_control_allocation_scale(1) = 1.f;
+		_control_allocation_scale(2) = 1.f;
 	}
 
-	// Scale yaw separately
-	_control_allocation_scale(2) = _mix.col(2).max();
-
-	if (_control_allocation_scale(2) > FLT_EPSILON) {
-		_mix.col(2) /= _control_allocation_scale(2);
-	}
-
-	// Same scale on X and Y
-	_control_allocation_scale(3) = fmaxf(_mix.col(3).max(), _mix.col(4).max());
-	_control_allocation_scale(4) = _control_allocation_scale(3);
-
-	if (_control_allocation_scale(3) > FLT_EPSILON) {
-		_mix.col(3) /= _control_allocation_scale(3);
-		_mix.col(4) /= _control_allocation_scale(4);
-	}
-
-	// Scale Z thrust separately
-	float z_sum = 0.f;
-	auto z_col = _mix.col(5);
+	// Scale thrust by the sum of the norm of the thrust vectors (which is invariant to rotation)
+	int num_non_zero_thrust = 0;
+	float norm_sum = 0.f;
 
 	for (int i = 0; i < _num_actuators; i++) {
-		z_sum += z_col(i, 0);
+		float norm = _mix.slice<1, 3>(i, 3).norm();
+		norm_sum += norm;
+
+		if (norm > FLT_EPSILON) {
+			++num_non_zero_thrust;
+		}
 	}
 
-	if ((-z_sum > FLT_EPSILON) && (_num_actuators > 0)) {
-		_control_allocation_scale(5) = -z_sum / _num_actuators;
+	if (num_non_zero_thrust > 0) {
+		norm_sum /= num_non_zero_thrust;
+		_control_allocation_scale(3) = norm_sum;
+		_control_allocation_scale(4) = norm_sum;
+		_control_allocation_scale(5) = norm_sum;
+		_mix.col(3) /= _control_allocation_scale(3);
+		_mix.col(4) /= _control_allocation_scale(4);
 		_mix.col(5) /= _control_allocation_scale(5);
+
+	} else {
+		_control_allocation_scale(3) = 1.f;
+		_control_allocation_scale(4) = 1.f;
+		_control_allocation_scale(5) = 1.f;
 	}
 
+
 	// Set all the small elements to 0 to avoid issues
 	// in the control allocation algorithms
 	for (int i = 0; i < _num_actuators; i++) {

src/modules/control_allocator/ControlAllocator.cpp

@@ -157,6 +157,9 @@ ControlAllocator::update_allocation_method(bool force)
 		AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES];
 		_actuator_effectiveness->getDesiredAllocationMethod(desired_methods);
 
+		bool normalize_rpy[ActuatorEffectiveness::MAX_NUM_MATRICES];
+		_actuator_effectiveness->getNormalizeRPY(normalize_rpy);
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 			AllocationMethod method = configured_method;
 
@@ -183,6 +186,7 @@ ControlAllocator::update_allocation_method(bool force)
 				_num_control_allocation = 0;
 
 			} else {
+				_control_allocation[i]->setNormalizeRPY(normalize_rpy[i]);
 				_control_allocation[i]->setActuatorSetpoint(actuator_sp[i]);
 			}
 		}