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/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -60,8 +60,14 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// MC motors
 	configuration.selected_matrix = 0;
 	_mc_rotors.enableYawControl(!_tilts.hasYawControl());
-	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, _last_tilt_control)
+
+	// Update matrix with tilts in vertical position when update is triggered by a manual
+	// configuration (parameter) change (=force update). This is to make sure the normalization
+	// scales are tilt-invariant. Note: force update only possible when disarm.
+	const float tilt_control_applied = force ? -1.f : _last_tilt_control;
+	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied)
 			    << configuration.num_actuators[(int)ActuatorType::MOTORS];
+
 	_mc_rotors.getEffectivenessMatrix(configuration, true);
 
 	// Control Surfaces
@@ -75,7 +81,10 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	_tilts.updateTorqueSign(_mc_rotors.geometry(), true /* disable pitch to avoid configuration errors */);
 	_tilts.getEffectivenessMatrix(configuration, true);
 
-	_updated = false;
+	// If it was a forced update (thus coming from a config change), then make sure to update matrix in
+	// the next iteration again with the correct tilt (but without updating the normalization scale).
+	_updated = force;
+
 	return true;
 }
 

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();

src/modules/control_allocator/ControlAllocator.cpp

@@ -496,7 +496,7 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force)
 			// Assign control effectiveness matrix
 			int total_num_actuators = config.num_actuators_matrix[i];
 			_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],
-					config.linearization_point[i], total_num_actuators);
+					config.linearization_point[i], total_num_actuators, force);
 		}
 
 		trims.timestamp = hrt_absolute_time();