preflight check: enable mode via command

The preflight check can be enabled by switching to nav_state
vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK using
VEHICLE_CMD_SET_NAV_STATE. Thus we can ditch this function and forgo
adding an extra vehicle command.

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,18 @@ 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
+	 */
+	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

@@ -408,7 +408,6 @@ int Commander::custom_command(int argc, char *argv[])
 			} else if (!strcmp(argv[1], "auto:land")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_LAND);
-
 			} else if (!strcmp(argv[1], "auto:precland")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
@@ -416,7 +415,6 @@ int Commander::custom_command(int argc, char *argv[])
 			} else if (!strcmp(argv[1], "ext1")) {
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_EXTERNAL1);
-
 			} else {
 				PX4_ERR("argument %s unsupported.", argv[1]);
 			}
@@ -844,6 +842,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 							break;
 
+						case PX4_CUSTOM_SUB_MODE_AUTO_CS_PREFLIGHT_CHECK:
+							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK;
+							break;
+
 						case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
 							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
 							break;
@@ -1572,42 +1574,6 @@ void Commander::handleCommandsFromModeExecutors()
 	}
 }
 
-unsigned Commander::handleCommandControlTest(const vehicle_command_s &cmd)
-{
-
-	// TODO: trigger this some different way using params for dev purposes...?
-	// TODO: decode from command: do we want to test roll/pitch/yaw(/collective tilt), or
-	// individual control surfaces?
-	// TODO define these somewhere else like all the cool kids do
-	static const int TEST_MODE_INDIVIDUAL = 0;
-	static const int TEST_MODE_RPY = 1;
-
-	int test_mode = TEST_MODE_RPY;
-
-	if (test_mode == TEST_MODE_INDIVIDUAL) {
-		// _user_mode_intention.change()
-		// _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_SERVO_TEST);
-		PX4_INFO("Not implemented");
-		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
-
-	} else if (test_mode == TEST_MODE_RPY) {
-
-		// this nice pattern stolen from handle_command
-		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
-			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
-		} else {
-			printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
-			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-		}
-
-	} else {
-		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
-	}
-
-	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-}
-
 unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
 {
 	if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
@@ -1871,31 +1837,19 @@ void Commander::run()
 			_status_changed = true;
 		}
 
-		if (_param_com_do_cs_check.get()) {
+		// do the control surface preflight check only if pre-armed. this means we need:
+		// COM_PREARM_MODE = 1 (Safety Button) or 2 (Always).
+		if (_actuator_armed.prearmed) {
+			if (_param_com_do_cs_check.get()) {
+				if (_vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK) {
+					_prev_nav_state = _vehicle_status.nav_state;
+					_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK);
+				}
 
-			// directly modify user intention here.
-			// plan is for this to ultimately to be triggered by a mavlink command
-			// through Commander::handle_command
-
-			// this nice pattern stolen from handle_command
-			// if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK, ModeChangeSource::ModeExecutor, false)) {
-			_prev_nav_state = _vehicle_status.nav_state;
-			_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK);
-
-			// no error handling like this for now
-			// if (ret) {
-				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-				// PX4_INFO("mode intention changed");
-
-			// } else {
-				// printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
-				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				// PX4_INFO("mode intention not changed");
-			// }
-
-		} else {
-			if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK) {
-				_user_mode_intention.change(_prev_nav_state);
+			} else {
+				if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK) {
+					_user_mode_intention.change(_prev_nav_state);
+				}
 			}
 		}
 

src/modules/commander/px4_custom_mode.h

@@ -65,6 +65,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
 	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
 	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_AUTO_CS_PREFLIGHT_CHECK,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL1,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL2,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL3,
@@ -157,6 +158,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_CS_PREFLIGHT_CHECK;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;

src/modules/control_allocator/ControlAllocator.cpp

@@ -61,8 +61,6 @@ ControlAllocator::ControlAllocator() :
 	_actuator_servos_pub.advertise();
 	_actuator_servos_trim_pub.advertise();
 
-	_tiltrotor_extra_controls_pub.advertise();
-
 	for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
 		char buffer[17];
 		snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);
@@ -347,7 +345,6 @@ ControlAllocator::Run()
 		if (_vehicle_status_sub.update(&vehicle_status)) {
 
 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
-
 			_preflight_check_running = vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK;
 
 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
@@ -442,22 +439,28 @@ ControlAllocator::Run()
 		}
 
 		if (_preflight_check_running) {
+			preflight_check_update_state();
 			preflight_check_overwrite_torque_sp(c);
 		}
 
 		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
 			_control_allocation[i]->allocate();
+
+			bool is_tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+
+			if (_preflight_check_running && is_tiltrotor) {
+				float preflight_check_tilt_sp = preflight_check_get_tilt_control();
+				_actuator_effectiveness->setBypassTiltrotorControls(true, preflight_check_tilt_sp, 0.0f);
+
+			} else {
+				_actuator_effectiveness->setBypassTiltrotorControls(false, 0.0f, 0.0f);
+
+			}
+
 			_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());
@@ -488,30 +491,17 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
-// void ControlAllocator::test_individual_control_surfaces() {
-	// goal here: modify actuation at the servo level.
+void ControlAllocator::preflight_check_update_state()
+{
 
-	// in here: small state machine cycling through servos (or taking info
-	// from outside about which servo to actuate)
-	// if test running: if enough time passed: go to next thing
-	//                  if last thing: test = not running
-
-	// elsewhere (probably Run()...)
-	// set test running if right message received
-	// if test running,
-
-// }
-
-void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) {
+	// bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
+	bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
 
 	// cycle through roll, pitch, yaw, and for each one inject positive and
 	// negative torque setpoints.
 
-	// is this the proper way to do it?
-	// bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
-	bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
-
 	int n_axes = 3;
+
 	if (tiltrotor) {
 		n_axes = 4;
 	}
@@ -519,17 +509,21 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float,
 	int max_phase = 2 * n_axes;
 
 	hrt_abstime now = hrt_absolute_time();
+
 	if (now - _last_preflight_check_update >= 500_ms) {
 		_preflight_check_phase++;
 		_preflight_check_phase %= max_phase;  // or quit once we did the whole thing once?
 		_last_preflight_check_update = now;
 	}
+}
+
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES>
+		(&c)[ActuatorEffectiveness::MAX_NUM_MATRICES])
+{
 
 	int axis = _preflight_check_phase / 2;
 	int negative = _preflight_check_phase % 2;
 
-	float modified_tilt_control = 0.5f;
-
 	if (axis < 3) {
 		c[0](0) = 0.;
 		c[0](1) = 0.;
@@ -543,24 +537,25 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float,
 			c[1](axis) = negative ? -1.f : 1.f;
 		}
 
-	} else {
-		// axis 4 = tiltrotor.
+	}
+
+}
+
+float ControlAllocator::preflight_check_get_tilt_control()
+{
+
+	int axis = _preflight_check_phase / 2;
+	int negative = _preflight_check_phase % 2;
+
+	float modified_tilt_control = 0.5f;
+
+	if (axis == 3) {
+		// axis 3 = tiltrotor.
 		// collective tilt normalised control goes from 0 to 1.
 		modified_tilt_control = negative ? 0.f : 1.f;
 	}
 
-	tiltrotor_extra_controls_s tiltrotor_extra_controls;
-
-	if (!_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
-		// got no message, make up thrust setpoint
-		tiltrotor_extra_controls.collective_thrust_normalized_setpoint = 0.;
-	}
-
-	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));
+	return modified_tilt_control;
 
 }
 

src/modules/control_allocator/ControlAllocator.hpp

@@ -139,6 +139,8 @@ private:
 	void publish_actuator_controls();
 
 	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]);
+	void preflight_check_update_state();
+	float preflight_check_get_tilt_control();
 
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
@@ -181,7 +183,6 @@ 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)};
@@ -190,8 +191,6 @@ 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)};

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 || _preflight_check_running) {
-						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,7 +88,12 @@ public:
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 
-	bool _preflight_check_running{false};
+	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};
@@ -116,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;