From 96fc4a06739e57b75b01a5e6a9c485c3031ae683 Mon Sep 17 00:00:00 2001
From: Balduin <balduin@auterion.com>
Date: Tue, 18 Feb 2025 11:31:29 +0100
Subject: [PATCH] preflight check: modify actuator_sp if running

this adds a new flag to ActuatorEffectivenessTiltrotorVTOL:
_preflight_check_running. We set it from ControlAllocator, and if true
we always add collective tilt to the actuator setpoint.
---
 src/modules/control_allocator/ControlAllocator.cpp         | 7 +++++++
 .../ActuatorEffectivenessTiltrotorVTOL.cpp                 | 2 +-
 .../ActuatorEffectivenessTiltrotorVTOL.hpp                 | 2 ++
 3 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp
index 6ee33b7553..631f6429a2 100644
--- a/src/modules/control_allocator/ControlAllocator.cpp
+++ b/src/modules/control_allocator/ControlAllocator.cpp
@@ -447,6 +447,13 @@ ControlAllocator::Run()
 
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
+			ActuatorEffectivenessTiltrotorVTOL *casted = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL *>
+					(_actuator_effectiveness);
+
+			if (casted != nullptr) {
+				casted->_preflight_check_running = _preflight_check_running;
+			}
+
 			_control_allocation[i]->setControlSetpoint(c[i]);
 
 			// Do allocation
diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
index 93ac6783d6..92f7119161 100644
--- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
@@ -166,7 +166,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
 
 					// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
-					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) {
+					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || _preflight_check_running) {
 						actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 
 					} else {
diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
index 512bd41f41..5446ee55ee 100644
--- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
@@ -88,6 +88,8 @@ public:
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 
+	bool _preflight_check_running{false};
+
 protected:
 	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;