ControlAllocator: Add control surface preflight check

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::setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust)
+{
+	// 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 setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust);
+
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
 	uint32_t _stopped_motors_mask{0};

src/modules/commander/Commander.cpp

@@ -302,6 +302,38 @@ int Commander::custom_command(int argc, char *argv[])
 		return 0;
 	}
 
+	if (!strcmp(argv[0], "cs_check")) {
+
+		// hardcoded values. intended just for testing, remove before PR.
+
+		if (argc > 1) {
+			if (!strcmp(argv[1], "roll")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_ROLL, 1.0);
+
+			} else if (!strcmp(argv[1], "pitch")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_PITCH, 1.0);
+
+			} else if (!strcmp(argv[1], "yaw")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_YAW, 1.0);
+
+			} else if (!strcmp(argv[1], "tilt")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK, vehicle_command_s::AXIS_COLLECTIVE_TILT,
+						     1.0);
+
+			} 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 +1549,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 +3045,7 @@ 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_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;
 
@@ -340,6 +342,10 @@ ControlAllocator::Run()
 
 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 
+			if (_armed) {
+				preflight_check_abort();
+			}
+
 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
 
 			// Check if the current flight phase is HOVER or FIXED_WING
@@ -373,6 +379,8 @@ ControlAllocator::Run()
 		}
 	}
 
+	preflight_check_handle_command();
+
 	// 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);
@@ -424,12 +432,18 @@ ControlAllocator::Run()
 			}
 		}
 
+		preflight_check_update_state();
+		preflight_check_overwrite_torque_sp(c);
+
 		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();
+
 			_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 +474,171 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
+void ControlAllocator::preflight_check_handle_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) {
+				// currently this does not check prearmed status. if not prearmed, it will just do nothing.
+				// should we output some sort of mild warning in that case?
+
+				preflight_check_start(vehicle_command);
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
+
+			} else {
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				PX4_INFO("Control surface preflight check rejected (armed)");
+			}
+
+			preflight_check_send_ack(result);
+		}
+	}
+}
+
+void ControlAllocator::preflight_check_start(vehicle_command_s &cmd)
+{
+
+	// If one is running, abort it. Depending on the use case we may prefer
+	// to instead "silently" overwrite the value.
+	if (_preflight_check_running) {
+		preflight_check_abort();
+	}
+
+	int axis = (uint8_t) lroundf(cmd.param1);
+	float input = cmd.param2;
+
+	_preflight_check_running = true;
+	_preflight_check_axis = axis;
+	_preflight_check_input = input;
+	_preflight_check_started = hrt_absolute_time();
+	_last_preflight_check_command = cmd;
+}
+
+void ControlAllocator::preflight_check_send_ack(uint8_t result)
+{
+
+	hrt_abstime now = hrt_absolute_time();
+
+	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()
+{
+	_preflight_check_running = false;
+	preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED);
+
+}
+
+void ControlAllocator::preflight_check_finish()
+{
+	_preflight_check_running = false;
+	preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+}
+
+
+void ControlAllocator::preflight_check_update_state()
+{
+	static uint32_t PREFLIGHT_CHECK_DURATION = 500_ms;
+	static uint32_t PREFLIGHT_CHECK_ACK_PERIOD = 1000_ms;
+
+	if (_preflight_check_running) {
+		hrt_abstime now = hrt_absolute_time();
+
+		if (now - _preflight_check_started >= PREFLIGHT_CHECK_DURATION) {
+			// alternative: never stop it here and instead let the GS send a stop message?
+			preflight_check_finish();
+		}
+
+		if (now - _preflight_check_last_ack >= PREFLIGHT_CHECK_ACK_PERIOD) {
+			preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS);
+		}
+	}
+}
+
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES>
+		(&c)[ActuatorEffectiveness::MAX_NUM_MATRICES])
+{
+	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;
+	}
+
+	c[0](0) = 0.;
+	c[0](1) = 0.;
+	c[0](2) = 0.;
+	c[0](axis) = _preflight_check_input;
+
+	if (_num_control_allocation > 1) {
+		c[1](0) = 0.;
+		c[1](1) = 0.;
+		c[1](2) = 0.;
+		c[1](axis) = _preflight_check_input;
+	}
+}
+
+void ControlAllocator::preflight_check_handle_tilt_control()
+{
+	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) {
+				float modified_tilt_control = math::constrain(_preflight_check_input, 0.f, 1.f);
+
+				_actuator_effectiveness->setBypassTiltrotorControls(true, modified_tilt_control, 0.0f);
+
+			} else {
+				// Commanded collective tilt axis but the vehicle is not a tiltrotor. Abort
+				_preflight_check_running = false;
+				preflight_check_send_ack(vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
+
+			}
+		}
+
+	} 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->setBypassTiltrotorControls(false, 0.0f, 0.0f);
+	}
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {

src/modules/control_allocator/ControlAllocator.hpp

@@ -77,6 +77,8 @@
 #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
@@ -138,6 +140,15 @@ private:
 
 	void publish_actuator_controls();
 
+	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]);
+	void preflight_check_handle_command();
+	void preflight_check_update_state();
+	void preflight_check_handle_tilt_control();
+	void preflight_check_start(vehicle_command_s &cmd);
+	void preflight_check_send_ack(uint8_t result);
+	void preflight_check_abort();
+	void preflight_check_finish();
+
 	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 +191,8 @@ 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)};
+
 	// Outputs
 	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
 
@@ -201,6 +214,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,121 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
 
 }
 
+void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt,
+		float collective_thrust)
+{
+	// if _bypass_tiltrotor_controls (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 setBypassTiltrotorControls
+	// - collective tilt is added to actuator_sp even if
+	//   (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT)
+	//   evaluates to false
+
+	_bypass_tiltrotor_controls = bypass;
+	_collective_tilt_normalized_setpoint = collective_tilt;
+	_collective_thrust_normalized_setpoint = collective_thrust;
+}
+
+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) || _bypass_tiltrotor_controls) {
+
+		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 (_bypass_tiltrotor_controls) {
+			control_collective_tilt = _collective_tilt_normalized_setpoint * 2.f - 1.f;
+			control_collective_thrust = _collective_thrust_normalized_setpoint;
+		}
+
+		// 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 || _bypass_tiltrotor_controls) {
+					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 setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust) 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,10 @@ protected:
 
 	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
 
+	bool _bypass_tiltrotor_controls{false};
+	float _collective_tilt_normalized_setpoint{0.5f};
+	float _collective_thrust_normalized_setpoint{0.0f};
+
 private:
 
 	void updateParams() override;