cs_check: remove unnecessary constrain

instead only leaving tilt. adjust function / variable names accordingly.

msg/versioned/VehicleCommand.msg

@@ -77,6 +77,7 @@ uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVL
 uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal.
 
 uint16 VEHICLE_CMD_MISSION_START = 300 # Start running a mission. |first_item: the first mission item to run|last_item: the last mission item to run (after this item is run, the mission ends)|
+uint16 VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK = 309 # Do control surface preflight check. Axis|value [-1, 1]|Unused|Unused|Unused|Unused|Unused|
 uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command. |[@range -1,1] value|[s] timeout|Unused|Unused|output function|
 uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command. |configuration|Unused|Unused|Unused|output function|
 uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component. |1 to arm, 0 to disarm.
@@ -177,6 +178,12 @@ int8 ARMING_ACTION_ARM = 1
 uint8 GRIPPER_ACTION_RELEASE = 0
 uint8 GRIPPER_ACTION_GRAB = 1
 
+# param1 in VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK
+uint8 AXIS_ROLL = 0
+uint8 AXIS_PITCH = 1
+uint8 AXIS_YAW = 2
+uint8 AXIS_COLLECTIVE_TILT = 3
+
 uint8 ORB_QUEUE_LENGTH = 8
 
 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -101,3 +101,8 @@ void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_mo
 		}
 	}
 }
+
+void ActuatorEffectiveness::overrideCollectiveTilt(bool do_override, float collective_tilt)
+{
+	// empty implementation to be overridden if needed.
+}

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -219,6 +219,20 @@ public:
 	 */
 	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
 
+	/**
+	 * If overridden by derived classes, optionally bypass the info usually
+	 * contained in tiltrotor_extra_controls -- normalised collective thrust
+	 * and tilt setpoints.
+	 *
+	 * Base class implementation is empty.
+	 *
+	 * @param bypass Flag indicating whether or not to use the other
+	 * arguments to bypass setpoints
+	 * @param collective_tilt Collective tilt normalized setpoint, in [0, 1]. 0: vertical, 1: horizontal.
+	 * @param collective_thrust Collective thrust normalized setpoint, in [0, 1].
+	 */
+	virtual void overrideCollectiveTilt(bool do_override, float collective_tilt);
+
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
 	uint32_t _stopped_motors_mask{0};

src/modules/commander/Commander.cpp

@@ -302,6 +302,51 @@ int Commander::custom_command(int argc, char *argv[])
 		return 0;
 	}
 
+	if (!strcmp(argv[0], "cs_check")) {
+
+		if (argc > 1) {
+
+			float torque = 1.0;
+
+			if (argc > 2) {
+				const float user_torque = std::atof(argv[2]);
+
+				// If instead argv[2] is not a float at all, we get torque=0 and nothing happens
+				if (!std::isnan(user_torque) && PX4_ISFINITE(user_torque)) {
+					torque = user_torque;
+
+				} else {
+					PX4_WARN("cs_check: torque \"%s\" is invalid. Using default +1.0.", argv[2]);
+				}
+			}
+
+			if (!strcmp(argv[1], "roll")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_ROLL, torque);
+
+			} else if (!strcmp(argv[1], "pitch")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_PITCH, torque);
+
+			} else if (!strcmp(argv[1], "yaw")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_YAW, torque);
+
+			} else if (!strcmp(argv[1], "tilt")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_COLLECTIVE_TILT,
+						     torque);
+
+			} else {
+				PX4_ERR("argument %s unsupported.", argv[1]);
+				return 1;
+			}
+
+			return 0;
+
+		} else {
+			PX4_ERR("missing argument");
+		}
+
+		return 0;
+	}
+
 	if (!strcmp(argv[0], "arm")) {
 		float param2 = 0.f;
 
@@ -1517,6 +1562,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
 	case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE:
 	case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION:
+	case vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
@@ -3012,6 +3058,9 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false);
 	PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("cs_check", "Run control surface preflight checks");
+	PRINT_MODULE_USAGE_ARG("roll|pitch|yaw|tilt", "Axis", false);
+	PRINT_MODULE_USAGE_ARG("torque", "Normalized torque command [-1.0, +1.0], default +1.0", true);
 	PRINT_MODULE_USAGE_COMMAND("arm");
 	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true);
 	PRINT_MODULE_USAGE_COMMAND("disarm");

src/modules/control_allocator/ControlAllocator.cpp

@@ -46,6 +46,8 @@
 #include <mathlib/math/Limits.hpp>
 #include <mathlib/math/Functions.hpp>
 
+#include <math.h>
+
 using namespace matrix;
 using namespace time_literals;
 
@@ -333,6 +335,12 @@ ControlAllocator::Run()
 		return;
 	}
 
+	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+	const hrt_abstime now = hrt_absolute_time();
+	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
+
+	bool is_vtol = false;
+
 	{
 		vehicle_status_s vehicle_status;
 
@@ -340,6 +348,10 @@ ControlAllocator::Run()
 
 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 
+			if (_armed && _preflight_check_running) {
+				preflight_check_abort(now);
+			}
+
 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
 
 			// Check if the current flight phase is HOVER or FIXED_WING
@@ -360,6 +372,9 @@ ControlAllocator::Run()
 				}
 			}
 
+			// Set this here to avoid another class member
+			is_vtol = vehicle_status.is_vtol;
+
 			// Forward to effectiveness source
 			_actuator_effectiveness->setFlightPhase(flight_phase);
 		}
@@ -373,9 +388,7 @@ ControlAllocator::Run()
 		}
 	}
 
-	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
-	const hrt_abstime now = hrt_absolute_time();
-	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
+	preflight_check_handle_command(now);
 
 	bool do_update = false;
 	vehicle_torque_setpoint_s vehicle_torque_setpoint;
@@ -424,12 +437,18 @@ ControlAllocator::Run()
 			}
 		}
 
+		preflight_check_update_state(now);
+		preflight_check_overwrite_torque_sp(c, is_vtol);
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
 			_control_allocation[i]->setControlSetpoint(c[i]);
 
 			// Do allocation
 			_control_allocation[i]->allocate();
+
+			preflight_check_handle_tilt_control(now);
+
 			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
 			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
 								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
@@ -460,6 +479,177 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
+void ControlAllocator::preflight_check_handle_command(hrt_abstime now)
+{
+
+	// Only start the check if prearmed but not armed. Otherwise, output a
+	// warning message and reject the command.
+
+	vehicle_command_s vehicle_command;
+
+	if (_vehicle_command_sub.update(&vehicle_command)) {
+
+		uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK) {
+			if (!_armed) {
+
+				actuator_armed_s armed;
+
+				if (_armed_sub.copy(&armed))  {
+
+					const bool prearmed = armed.prearmed;
+
+					if (prearmed) {
+						// currently this does not check prearmed status. if not prearmed, it will just do nothing.
+						preflight_check_start(vehicle_command, now);
+						result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
+
+					} else {
+						result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+						PX4_WARN("Control surface preflight check rejected (not prearmed)");
+					}
+
+				} else {
+					result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					PX4_WARN("Control surface preflight check rejected (failed to get prearmed state)");
+				}
+
+			} else {
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				PX4_WARN("Control surface preflight check rejected (armed)");
+			}
+
+			preflight_check_send_ack(result, now);
+		}
+	}
+}
+
+void ControlAllocator::preflight_check_start(vehicle_command_s &cmd, hrt_abstime now)
+{
+
+	// If already running, abort it. this is only to send the correct ack.
+	if (_preflight_check_running) {
+		preflight_check_abort(now);
+	}
+
+	const int axis = (uint8_t) lroundf(cmd.param1);
+	const float input = cmd.param2;
+
+	_preflight_check_running = true;
+	_preflight_check_axis = axis;
+	_preflight_check_input = input;
+	_preflight_check_started = now;
+	_last_preflight_check_command = cmd;
+}
+
+void ControlAllocator::preflight_check_send_ack(uint8_t result, hrt_abstime now)
+{
+	if (_last_preflight_check_command.from_external) {
+		vehicle_command_ack_s command_ack{};
+		command_ack.timestamp = now;
+		command_ack.command = _last_preflight_check_command.command;
+		command_ack.result = result;
+		command_ack.target_system = _last_preflight_check_command.source_system;
+		command_ack.target_component = _last_preflight_check_command.source_component;
+
+		uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
+		command_ack_pub.publish(command_ack);
+	}
+
+	_preflight_check_last_ack = now;
+}
+
+void ControlAllocator::preflight_check_abort(hrt_abstime now)
+{
+	_preflight_check_running = false;
+	preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
+
+}
+
+void ControlAllocator::preflight_check_finish(hrt_abstime now)
+{
+	_preflight_check_running = false;
+	preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED, now);
+}
+
+
+void ControlAllocator::preflight_check_update_state(hrt_abstime now)
+{
+	if (_preflight_check_running) {
+
+		if (now - _preflight_check_started >= PREFLIGHT_CHECK_DURATION) {
+			preflight_check_finish(now);
+		}
+
+		if (now - _preflight_check_last_ack >= PREFLIGHT_CHECK_ACK_PERIOD) {
+			preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS, now);
+		}
+	}
+}
+
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES>
+		(&c)[ActuatorEffectiveness::MAX_NUM_MATRICES], bool is_vtol)
+{
+	if (!_preflight_check_running) { return; }
+
+	int axis = -1;
+
+	switch (_preflight_check_axis) {
+	case vehicle_command_s::AXIS_ROLL:
+		axis = 0;
+		break;
+
+	case vehicle_command_s::AXIS_PITCH:
+		axis = 1;
+		break;
+
+	case vehicle_command_s::AXIS_YAW:
+		axis = 2;
+		break;
+
+	default:
+		// If none of roll, pitch, yaw, we do nothing here
+		return;
+	}
+
+	// If VTOL, instance 1 is the fixed wing part, otherwise instance 0.
+	const int instance = is_vtol ? 1 : 0;
+
+	c[instance](0) = 0.;
+	c[instance](1) = 0.;
+	c[instance](2) = 0.;
+	c[instance](axis) = _preflight_check_input;
+}
+
+void ControlAllocator::preflight_check_handle_tilt_control(hrt_abstime now)
+{
+	const bool is_tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+
+	if (_preflight_check_running) {
+
+		if (_preflight_check_axis == vehicle_command_s::AXIS_COLLECTIVE_TILT) {
+
+			if (is_tiltrotor) {
+				_actuator_effectiveness->overrideCollectiveTilt(true, _preflight_check_input);
+
+			} else {
+				// Commanded collective tilt axis but the vehicle is not a tiltrotor. Abort
+				_preflight_check_running = false;
+				PX4_WARN("Control surface preflight check rejected (tilt commanded but not available)");
+				preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED, now);
+
+			}
+		}
+
+	} else {
+		// strictly speaking this is only necessary if is_tiltrotor but
+		// can't hurt to call it always in case other
+		// ActuatorEffectiveness* classes implement similar things
+		_actuator_effectiveness->overrideCollectiveTilt(false, 0.0f);
+	}
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {

src/modules/control_allocator/ControlAllocator.hpp

@@ -68,6 +68,8 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/actuator_servos.h>
 #include <uORB/topics/actuator_servos_trim.h>
@@ -77,8 +79,11 @@
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/failure_detector_status.h>
 
+
 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -88,6 +93,9 @@ public:
 	static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS;
 	static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS;
 
+	static constexpr uint32_t PREFLIGHT_CHECK_DURATION = 500_ms;
+	static constexpr uint32_t PREFLIGHT_CHECK_ACK_PERIOD = 1000_ms;
+
 	using ActuatorVector = ActuatorEffectiveness::ActuatorVector;
 
 	ControlAllocator();
@@ -138,6 +146,16 @@ private:
 
 	void publish_actuator_controls();
 
+	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES],
+			const bool is_vtol);
+	void preflight_check_handle_command(const hrt_abstime now);
+	void preflight_check_update_state(const hrt_abstime now);
+	void preflight_check_handle_tilt_control(const hrt_abstime now);
+	void preflight_check_start(vehicle_command_s &cmd, const hrt_abstime now);
+	void preflight_check_send_ack(uint8_t result, const hrt_abstime now);
+	void preflight_check_abort(const hrt_abstime now);
+	void preflight_check_finish(const hrt_abstime now);
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -180,6 +198,9 @@ 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 _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+
 	// Outputs
 	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
 
@@ -201,6 +222,14 @@ private:
 	// For example, the system might report two motor failures, but only the first one is handled by CA
 	uint16_t _handled_motor_failure_bitmask{0};
 
+	bool _preflight_check_running{false};
+
+	uint8_t _preflight_check_axis{0};
+	float _preflight_check_input{0.0f};
+	vehicle_command_s _last_preflight_check_command{0};
+	hrt_abstime _preflight_check_started{0};
+	hrt_abstime _preflight_check_last_ack{0};
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -124,88 +124,118 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
 
 }
 
+void ActuatorEffectivenessTiltrotorVTOL::overrideCollectiveTilt(bool do_override, float collective_tilt)
+{
+	// if _do_override_collective_tilt (used for control surface preflight
+	// check), we alter the behaviour of processTiltrotorControls in these
+	// two ways:
+	// - collective tilt and thrust setpoints are NOT taken from uOrb
+	//   message, but from class member variable, which we can arbitrarily set
+	//   before calling this function using overrideCollectiveTilt
+	// - collective tilt is added to actuator_sp even if
+	//   (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT)
+	//   evaluates to false
+
+	_do_override_collective_tilt = do_override;
+	_collective_tilt_normalized_setpoint = collective_tilt;
+}
+
+void ActuatorEffectivenessTiltrotorVTOL::processTiltrotorControls(ActuatorVector &actuator_sp,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+
+{
+	tiltrotor_extra_controls_s tiltrotor_extra_controls;
+
+	if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || _do_override_collective_tilt) {
+
+		float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
+		float control_collective_thrust = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
+
+		if (_do_override_collective_tilt) {
+			control_collective_tilt = _collective_tilt_normalized_setpoint * 2.f - 1.f;
+		}
+
+		// set control_collective_tilt to exactly -1 or 1 if close to these end points
+		control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
+		control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
+
+		// initialize _last_collective_tilt_control
+		if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+			_last_collective_tilt_control = control_collective_tilt;
+
+		} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+			_collective_tilt_updated = true;
+			_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;
+		}
+
+		bool yaw_saturated_positive = true;
+		bool yaw_saturated_negative = true;
+
+		for (int i = 0; i < _tilts.count(); ++i) {
+			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 || _do_override_collective_tilt) {
+					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
+
+				} else {
+					actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
+				}
+			}
+
+			// custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit
+			if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) {
+
+				if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+					yaw_saturated_positive = false;
+				}
+
+				if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+					yaw_saturated_negative = false;
+				}
+
+			} else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) {
+				if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
+					yaw_saturated_negative = false;
+				}
+
+				if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
+					yaw_saturated_positive = false;
+				}
+			}
+		}
+
+		_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
+		_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
+
+		// in FW directly use throttle sp
+		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+			for (int i = 0; i < _first_tilt_idx; ++i) {
+				actuator_sp(i) = control_collective_thrust;
+			}
+		}
+	}
+}
+
 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
-	// apply tilt
 	if (matrix_index == 0) {
-		tiltrotor_extra_controls_s tiltrotor_extra_controls;
 
-		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
-			float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
-
-			// set control_collective_tilt to exactly -1 or 1 if close to these end points
-			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
-			control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
-
-			// initialize _last_collective_tilt_control
-			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
-				_last_collective_tilt_control = control_collective_tilt;
-
-			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
-				_collective_tilt_updated = true;
-				_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;
-			}
-
-			bool yaw_saturated_positive = true;
-			bool yaw_saturated_negative = true;
-
-			for (int i = 0; i < _tilts.count(); ++i) {
-				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) {
-						actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
-
-					} else {
-						actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
-					}
-				}
-
-				// custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit
-				if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) {
-
-					if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
-						yaw_saturated_positive = false;
-					}
-
-					if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
-						yaw_saturated_negative = false;
-					}
-
-				} else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) {
-					if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) {
-						yaw_saturated_negative = false;
-					}
-
-					if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) {
-						yaw_saturated_positive = false;
-					}
-				}
-			}
-
-			_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
-			_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
-
-			// in FW directly use throttle sp
-			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
-				for (int i = 0; i < _first_tilt_idx; ++i) {
-					actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
-				}
-			}
-		}
+		processTiltrotorControls(actuator_sp, actuator_min, actuator_max);
 
 		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
 			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -88,6 +88,13 @@ public:
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 
+	void overrideCollectiveTilt(bool do_override, float collective_tilt) override;
+
+	void processTiltrotorControls(ActuatorVector &actuator_sp,
+				      const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+				      const matrix::Vector<float, NUM_ACTUATORS> &actuator_max);
+
+
 protected:
 	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
@@ -114,6 +121,9 @@ protected:
 
 	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
 
+	bool _do_override_collective_tilt{false};
+	float _collective_tilt_normalized_setpoint{0.5f};
+
 private:
 
 	void updateParams() override;