From b09340cc98e2e0345870c600eaecaee22569d4f5 Mon Sep 17 00:00:00 2001
From: Pedro Roque <padr@kth.se>
Date: Mon, 20 Jan 2025 13:30:51 +0100
Subject: [PATCH] Control Allocator: Add option for metric allocation (skip
 normalization) (#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
---
 .../ControlAllocationPseudoInverse.cpp        | 16 +++++++++----
 .../ControlAllocationPseudoInverse.hpp        |  2 ++
 .../ControlAllocationPseudoInverseTest.cpp    | 24 +++++++++++++++++++
 3 files changed, 38 insertions(+), 4 deletions(-)

diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
index 5d86814d7e..0dc512b4ed 100644
--- a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
+++ b/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;
 	}
 }
diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp
index 27c367376b..d4527cc1f4 100644
--- a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp
+++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp
@@ -57,11 +57,13 @@ public:
 	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.
diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp
index 89faab8c92..0eafbc6c26 100644
--- a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp
+++ b/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);
+}