From 478724c9fe04902901fdb6708667625714ced366 Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Thu, 16 Dec 2021 17:50:48 +0100
Subject: [PATCH] EffectivenessTiltrotor: fix logic for matrix update

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 .../ActuatorEffectivenessTiltrotorVTOL.cpp               | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
index a1b7aa5837..24309f8adb 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
@@ -100,8 +100,13 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
 			float control_tilt = actuator_controls_1.control[4] * 2.f - 1.f;
 
-			if (fabsf(control_tilt - _last_tilt_control) > 0.01f && PX4_ISFINITE(_last_tilt_control)) {
+			// initialize _last_tilt_control
+			if (!PX4_ISFINITE(_last_tilt_control)) {
+				_last_tilt_control = control_tilt;
+
+			} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
 				_updated = true;
+				_last_tilt_control = control_tilt;
 			}
 
 			for (int i = 0; i < _tilts.count(); ++i) {
@@ -109,8 +114,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 					actuator_sp(i + _first_tilt_idx) += control_tilt;
 				}
 			}
-
-			_last_tilt_control = control_tilt;
 		}
 	}
 }