From 9bdc898ceb10b34225136e1fa1af60e4123b1e3e Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Thu, 6 Jun 2024 11:34:26 +0200
Subject: [PATCH] CA: extend status print

Print not only effectiveness matrix, but also raw and normalized
mixing matrix (inverted effectiveness).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 .../ControlAllocation/ControlAllocation.hpp   |  5 ++++
 .../ControlAllocationPseudoInverse.hpp        |  2 --
 .../control_allocator/ControlAllocator.cpp    | 24 +++++++++++++++++--
 3 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
index 857b5c3aa6..def22686bf 100644
--- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
+++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
@@ -220,6 +220,9 @@ public:
 	void setNormalizeAsPlanarMC(bool normalize_as_mc) { _normalize_matrix_as_planar_mc = normalize_as_mc; }
 	void setAirmode(const int airmode) {_airmode = airmode; }
 
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> getMixMatrix() {return _mix; }
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> getNormalizedMixMatrix() {return _normalized_mix; }
+
 protected:
 	friend class ControlAllocator; // for _actuator_sp
 
@@ -236,4 +239,6 @@ protected:
 	int _num_actuators{0};
 	bool _normalize_matrix_as_planar_mc{false};		///< if true, normalize roll, pitch and yaw columns optimized for planar MC
 	int _airmode{0};					///< 0: disabled, 1: RP airmode, 2: RPY airmode
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
+	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _normalized_mix;
 };
diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp
index e7b5466ad9..215ce360a1 100644
--- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp
+++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp
@@ -59,8 +59,6 @@ public:
 				    bool update_normalization_scale) override;
 
 protected:
-	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
-	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _normalized_mix;
 
 	bool _mix_update_needed{false};
 
diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp
index 7ebc8e5562..80f981a0c6 100644
--- a/src/modules/control_allocator/ControlAllocator.cpp
+++ b/src/modules/control_allocator/ControlAllocator.cpp
@@ -931,8 +931,28 @@ int ControlAllocator::print_status()
 			PX4_INFO("Instance: %i", i);
 		}
 
-		PX4_INFO("  Effectiveness.T =");
-		effectiveness.T().print();
+		// const size_t num_actuators = _control_allocation[i]->numConfiguredActuators();
+		const size_t num_actuators = 6;
+
+		matrix::Matrix<float, num_actuators, NUM_AXES> effectiveness_sliced_transposed(
+			effectiveness.T().slice<num_actuators, NUM_AXES>(0, 0));
+
+		const matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> mix_raw = _control_allocation[i]->getMixMatrix();
+		const matrix::Matrix<float, num_actuators, NUM_AXES> mix_raw_sliced(mix_raw.slice<num_actuators, NUM_AXES>(0, 0));
+
+		const matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> mix_normalized = _control_allocation[i]->getNormalizedMixMatrix();
+		const matrix::Matrix<float, num_actuators, NUM_AXES> mix_normalized_sliced(
+			mix_normalized.slice<num_actuators, NUM_AXES>(0, 0));
+
+		PX4_INFO("  Effectiveness.T (sliced) =");
+		effectiveness_sliced_transposed.print();
+
+		PX4_INFO(" Mixer matrix raw (sliced) =");
+		mix_raw_sliced.print();
+
+		PX4_INFO(" Mixer matrix normalized (sliced) =");
+		mix_normalized_sliced.print();
+
 		PX4_INFO("  minimum =");
 		_control_allocation[i]->getActuatorMin().T().print();
 		PX4_INFO("  maximum =");