Tiltrotor: move spin up tilt to control allocation (#21991)

EffectivenessTiltrotor: link time when to tilt motors to MC position to COM_SPOOLUP_TIME
- remove VT_TILT_SPINUP and special spin up tilt handling form the VTOL module
- now handle the spoolup in the allocation, directly linked to COM_SPOOLUP_TIME
- leave tilts at disarmed value during spoolup

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -48,8 +48,19 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModulePar
 	  _mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::Configurable, true),
 	  _control_surfaces(this), _tilts(this)
 {
+	_param_handles.com_spoolup_time = param_find("COM_SPOOLUP_TIME");
+
+	updateParams();
 	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
+
+void ActuatorEffectivenessTiltrotorVTOL::updateParams()
+{
+	ModuleParams::updateParams();
+
+	param_get(_param_handles.com_spoolup_time, &_param_spoolup_time);
+}
+
 bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)
@@ -139,7 +150,14 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 
 			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;
+
+					// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
+					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) {
+						actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
+
+					} else {
+						actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
+					}
 				}
 			}
 
@@ -213,3 +231,15 @@ void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index,
 		status.unallocated_torque[2] = 0.f;
 	}
 }
+
+bool ActuatorEffectivenessTiltrotorVTOL::throttleSpoolupFinished()
+{
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+		_armed_time = vehicle_status.armed_time;
+	}
+
+	return _armed && hrt_elapsed_time(&_armed_time) > _param_spoolup_time * 1_s;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -46,9 +46,11 @@
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 #include "ActuatorEffectivenessTilts.hpp"
 
+#include <px4_platform_common/module_params.h>
+
 #include <uORB/topics/normalized_unsigned_setpoint.h>
 #include <uORB/topics/tiltrotor_extra_controls.h>
-
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/Subscription.hpp>
 
 class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness
@@ -111,4 +113,22 @@ protected:
 	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
 
 	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
+
+private:
+
+	void updateParams() override;
+
+	struct ParamHandles {
+		param_t com_spoolup_time;
+	};
+
+	ParamHandles _param_handles{};
+
+	float _param_spoolup_time{1.f};
+
+	// Tilt handling during motor spoolup: leave the tilts in their disarmed position unitil 1s after arming
+	bool throttleSpoolupFinished();
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	bool _armed{false};
+	uint64_t _armed_time{0};
 };