From b8c541dd7277ed735139d7d1bfb829d61fbe29fb Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Thu, 10 Aug 2023 13:02:11 +0200
Subject: [PATCH] TiltrotorEffectiveness: limit thrust axis tilt to z
 effectiveness scaling
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle
the effectiveness of the thrust axis in z is apporaching 0, and by that is increasing
the motor output to max.
Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid
a thrust spike when the transition is initiated (as then the tilt is fully forward).

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

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
index 592c157f5a94..9fc8919edef9 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
@@ -137,6 +137,17 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				_last_collective_tilt_control = control_collective_tilt;
 			}
 
+			// During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle the effectiveness
+			// of the thrust axis in z is apporaching 0, and by that is increasing the motor output to max.
+			// Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid
+			// a thrust spike when the transition is initiated (as then the tilt is fully forward).
+			if (_flight_phase == FlightPhase::TRANSITION_HF_TO_FF) {
+				_last_collective_tilt_control = math::constrain(_last_collective_tilt_control, -1.f, 0.f);
+
+			} else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) {
+				_last_collective_tilt_control = -1.f;
+			}
+
 			for (int i = 0; i < _tilts.count(); ++i) {
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
 					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;