Control Allocator: Add option for metric allocation (skip normalizati…

…on) (#24199)

* add: metric allocation

* add: actual files

* rft: moved metric allocation to pseudo-inverse via flag with public method

* del: removed metric allocation test and added test in pseudo-inverse testing

* rft: deleted extra newline at the end of pseudo inverse test file

* feat: removed unnecessary log include

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp

@@ -51,6 +51,11 @@ ControlAllocationPseudoInverse::setEffectivenessMatrix(
 			update_normalization_scale);
 	_mix_update_needed = true;
 	_normalization_needs_update = update_normalization_scale;
+
+	if (_metric_allocation && update_normalization_scale) {
+		// adding #include <px4_platform_common/log.h> + PX4_WARN leads to failed linking on test
+		_normalization_needs_update = false;
+	}
 }
 
 void
@@ -59,12 +64,15 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 	if (_mix_update_needed) {
 		matrix::geninv(_effectiveness, _mix);
 
-		if (_normalization_needs_update && !_had_actuator_failure) {
-			updateControlAllocationMatrixScale();
-			_normalization_needs_update = false;
+		if (!_metric_allocation) {
+			if (_normalization_needs_update && !_had_actuator_failure) {
+				updateControlAllocationMatrixScale();
+				_normalization_needs_update = false;
+			}
+
+			normalizeControlAllocationMatrix();
 		}
 
-		normalizeControlAllocationMatrix();
 		_mix_update_needed = false;
 	}
 }

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp

@@ -57,11 +57,13 @@ class ControlAllocationPseudoInverse: public ControlAllocation
 	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
 				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
 				    bool update_normalization_scale) override;
+	void setMetricAllocation(bool metric_allocation) { _metric_allocation = metric_allocation; }
 
 protected:
 	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
 
 	bool _mix_update_needed{false};
+	bool _metric_allocation{false};
 
 	/**
 	 * Recalculate pseudo inverse if required.

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp

@@ -67,3 +67,27 @@ TEST(ControlAllocationTest, AllZeroCase)
 	EXPECT_EQ(actuator_sp, actuator_sp_expected);
 	EXPECT_EQ(control_allocated, control_allocated_expected);
 }
+
+TEST(ControlAllocationMetricTest, AllZeroCase)
+{
+	ControlAllocationPseudoInverse method;
+
+	matrix::Vector<float, 6> control_sp;
+	matrix::Vector<float, 6> control_allocated;
+	matrix::Vector<float, 6> control_allocated_expected;
+	matrix::Matrix<float, 6, 16> effectiveness;
+	matrix::Vector<float, 16> actuator_sp;
+	matrix::Vector<float, 16> actuator_trim;
+	matrix::Vector<float, 16> linearization_point;
+	matrix::Vector<float, 16> actuator_sp_expected;
+
+	method.setMetricAllocation(true);
+	method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
+	method.setControlSetpoint(control_sp);
+	method.allocate();
+	actuator_sp = method.getActuatorSetpoint();
+	control_allocated_expected = method.getAllocatedControl();
+
+	EXPECT_EQ(actuator_sp, actuator_sp_expected);
+	EXPECT_EQ(control_allocated, control_allocated_expected);
+}