From 2a779663f63437e268538a40d3b95547fdfb8d4d Mon Sep 17 00:00:00 2001
From: Balduin <balduin@auterion.com>
Date: Mon, 17 Feb 2025 11:40:41 +0100
Subject: [PATCH] preflight check: tilt collective actuation

we "intercept" the tiltrotor_extra_controls message, which is read again
right after in ActuatorEffectivenessTiltrotorVTOL::updateSetpoint.
---
 .../control_allocator/ControlAllocator.cpp    | 45 +++++++++++++++----
 .../control_allocator/ControlAllocator.hpp    |  3 ++
 2 files changed, 39 insertions(+), 9 deletions(-)

diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp
index bf3fb05695..afbc2a572a 100644
--- a/src/modules/control_allocator/ControlAllocator.cpp
+++ b/src/modules/control_allocator/ControlAllocator.cpp
@@ -502,6 +502,15 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float,
 
 	int max_phase = 3 * 2;
 
+	// is this the proper way to do it?
+	// bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+	bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
+	PX4_INFO("tiltrotor = %d", (int) tiltrotor);
+
+	if (tiltrotor) {
+		max_phase = 4 * 2;
+	}
+
 	hrt_abstime now = hrt_absolute_time();
 	if (now - _last_preflight_check_update >= 500_ms) {
 		_preflight_check_phase++;
@@ -512,16 +521,34 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float,
 	int axis = _preflight_check_phase / 2;
 	int negative = _preflight_check_phase % 2;
 
-	c[0](0) = 0.;
-	c[0](1) = 0.;
-	c[0](2) = 0.;
-	c[0](axis) = negative ? -1.f : 1.f;
+	if (axis < 3) {
+		c[0](0) = 0.;
+		c[0](1) = 0.;
+		c[0](2) = 0.;
+		c[0](axis) = negative ? -1.f : 1.f;
 
-	if (_num_control_allocation > 1) {
-		c[1](0) = 0.;
-		c[1](1) = 0.;
-		c[1](2) = 0.;
-		c[1](axis) = negative ? -1.f : 1.f;
+		if (_num_control_allocation > 1) {
+			c[1](0) = 0.;
+			c[1](1) = 0.;
+			c[1](2) = 0.;
+			c[1](axis) = negative ? -1.f : 1.f;
+		}
+
+	} else {
+		// axis 4 = tiltrotor. Here, we modify the
+		// tiltrotor_extra_controls message, which is received in
+		// ActuatorEffectivenessTiltrotorVTOL::updateSetpoint
+
+		float modified_tilt_control = negative ? -1.f : 1.f;
+
+		tiltrotor_extra_controls_s tiltrotor_extra_controls;
+
+		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
+			tiltrotor_extra_controls.collective_tilt_normalized_setpoint = modified_tilt_control;
+			tiltrotor_extra_controls.timestamp = hrt_absolute_time();
+			_tiltrotor_extra_controls_pub.publish(tiltrotor_extra_controls);
+
+		}
 	}
 
 	// PX4_INFO("_torque_sp: %f, %f, %f", (double) _torque_sp(0), (double) _torque_sp(1), (double) _torque_sp(2));
diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp
index 7d49e55d4f..5bc530e53c 100644
--- a/src/modules/control_allocator/ControlAllocator.hpp
+++ b/src/modules/control_allocator/ControlAllocator.hpp
@@ -181,6 +181,7 @@ private:
 
 	uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1};  /**< vehicle torque setpoint subscription (2. instance) */
 	uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */
+	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
 
 	// Outputs
 	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
@@ -189,6 +190,8 @@ private:
 	uORB::Publication<actuator_servos_s>	_actuator_servos_pub{ORB_ID(actuator_servos)};
 	uORB::Publication<actuator_servos_trim_s>	_actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)};
 
+	uORB::Publication<tiltrotor_extra_controls_s>	_tiltrotor_extra_controls_pub{ORB_ID(tiltrotor_extra_controls)};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};