From 6db92b4011ab174ff35967be3f8c00e775a18fbd Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Mon, 3 Oct 2022 18:33:11 +0200
Subject: [PATCH] ControlAllocation: remove actuator trim value from
 actuator_sp to calculate allocated control

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 .../control_allocator/ControlAllocation/ControlAllocation.hpp   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
index cc6d76486c..55b5a870fa 100644
--- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
+++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
@@ -143,7 +143,7 @@ public:
 	 * @return Control vector
 	 */
 	matrix::Vector<float, NUM_AXES> getAllocatedControl() const
-	{ return (_effectiveness * _actuator_sp).emult(_control_allocation_scale); }
+	{ return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); }
 
 	/**
 	 * Get the control effectiveness matrix