ControlAllocation: update normalization scale only if matrix updated is forced

The forced flag is used to distinguish between updates due to a configuration
(parameter) change (only enabled when disarmed), and matrix updates due to motor
tilt change. Only update the normalization scale if the forced flag is true, and
use a tilt angle of vertical position for it to have the scales tilt-invariant.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp

@@ -51,7 +51,8 @@ ControlAllocation::ControlAllocation()
 void
 ControlAllocation::setEffectivenessMatrix(
 	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
-	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators)
+	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+	bool update_normalization_scale)
 {
 	_effectiveness = effectiveness;
 	ActuatorVector linearization_point_clipped = linearization_point;

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -104,7 +104,8 @@ public:
 	 * @param B Effectiveness matrix
 	 */
 	virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
-					    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators);
+					    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+					    bool update_normalization_scale);
 
 	/**
 	 * Get the allocated actuator vector

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp

@@ -44,10 +44,13 @@
 void
 ControlAllocationPseudoInverse::setEffectivenessMatrix(
 	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
-	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators)
+	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+	bool update_normalization_scale)
 {
-	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators);
+	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators,
+			update_normalization_scale);
 	_mix_update_needed = true;
+	_normalization_needs_update = update_normalization_scale;
 }
 
 void
@@ -55,33 +58,31 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 {
 	if (_mix_update_needed) {
 		matrix::geninv(_effectiveness, _mix);
+
+		if (_normalization_needs_update) {
+			updateControlAllocationMatrixScale();
+			_normalization_needs_update = false;
+		}
+
 		normalizeControlAllocationMatrix();
 		_mix_update_needed = false;
 	}
 }
 
 void
-ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
+ControlAllocationPseudoInverse::updateControlAllocationMatrixScale()
 {
+	// Same scale on roll and pitch
 	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);
-		}
-
 		// 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;
@@ -106,16 +107,31 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
 		_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;
 	}
+}
 
+void
+ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
+{
+	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(2) > FLT_EPSILON) {
+		_mix.col(2) /= _control_allocation_scale(2);
+	}
+
+	if (_control_allocation_scale(3) > FLT_EPSILON) {
+		_mix.col(3) /= _control_allocation_scale(3);
+		_mix.col(4) /= _control_allocation_scale(4);
+		_mix.col(5) /= _control_allocation_scale(5);
+	}
 
 	// Set all the small elements to 0 to avoid issues
 	// in the control allocation algorithms

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp

@@ -55,7 +55,8 @@ public:
 
 	void allocate() override;
 	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
-				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators) override;
+				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
+				    bool update_normalization_scale) override;
 
 protected:
 	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
@@ -70,4 +71,6 @@ protected:
 
 private:
 	void normalizeControlAllocationMatrix();
+	void updateControlAllocationMatrixScale();
+	bool _normalization_needs_update{false};
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp

@@ -57,7 +57,7 @@ TEST(ControlAllocationTest, AllZeroCase)
 	matrix::Vector<float, 16> linearization_point;
 	matrix::Vector<float, 16> actuator_sp_expected;
 
-	method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16);
+	method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
 	method.setControlSetpoint(control_sp);
 	method.allocate();
 	method.clipActuatorSetpoint();