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

@@ -198,8 +198,7 @@ public:
 	 */
 	virtual void 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, bool preflight_check_running = false,
-				    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) {}
+				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
 
 	/**
 	 * Get a bitmask of motors to be stopped
@@ -220,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())) {

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

@@ -450,12 +450,20 @@ ControlAllocator::Run()
 			// Do allocation
 			_control_allocation[i]->allocate();
 
-			float preflight_check_tilt_sp = preflight_check_get_tilt_control();
+			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(),
-								_preflight_check_running, preflight_check_tilt_sp, 0.0f);
+								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
 
 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);
@@ -483,7 +491,8 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
-void ControlAllocator::preflight_check_update_state() {
+void ControlAllocator::preflight_check_update_state()
+{
 
 	// bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
 	bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
@@ -492,6 +501,7 @@ void ControlAllocator::preflight_check_update_state() {
 	// negative torque setpoints.
 
 	int n_axes = 3;
+
 	if (tiltrotor) {
 		n_axes = 4;
 	}
@@ -499,6 +509,7 @@ void ControlAllocator::preflight_check_update_state() {
 	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?
@@ -506,7 +517,9 @@ void ControlAllocator::preflight_check_update_state() {
 	}
 }
 
-void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) {
+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;
@@ -528,7 +541,8 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float,
 
 }
 
-float ControlAllocator::preflight_check_get_tilt_control() {
+float ControlAllocator::preflight_check_get_tilt_control()
+{
 
 	int axis = _preflight_check_phase / 2;
 	int negative = _preflight_check_phase % 2;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -61,8 +61,7 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
 
 void ActuatorEffectivenessCustom::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -47,8 +47,7 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "Custom"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -63,8 +63,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 
 void ActuatorEffectivenessFixedWing::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -53,8 +53,7 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 private:
 	ActuatorEffectivenessRotors _rotors;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -130,8 +130,7 @@ bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &conf
 
 void ActuatorEffectivenessHelicopter::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	_saturation_flags = {};
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -80,8 +80,7 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp

@@ -104,8 +104,7 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio
 
 void ActuatorEffectivenessHelicopterCoaxial::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	_saturation_flags = {};
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp

@@ -71,8 +71,7 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

@@ -78,8 +78,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 
 void ActuatorEffectivenessMCTilt::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	actuator_sp += _tilt_offsets;
 	// TODO: dynamic matrix update

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -57,8 +57,7 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "MC Tilt"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp

@@ -51,8 +51,7 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi
 
 void ActuatorEffectivenessRoverAckermann::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp

@@ -45,8 +45,7 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "Rover (Ackermann)"; }
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -85,8 +85,7 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt,
 
 void ActuatorEffectivenessStandardVTOL::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	if (matrix_index == 0) {
 		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -77,8 +77,7 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -90,8 +90,7 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d
 
 void ActuatorEffectivenessTailsitterVTOL::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	if (matrix_index == 0) {
 		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -74,8 +74,7 @@ public:
 
 	void 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, bool preflight_check_running,
-			    float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -124,106 +124,122 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
 
 }
 
-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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt,
+		float collective_thrust)
 {
-	// apply tilt
-
-	// if preflight_check_running, we alter the behaviour in these two ways:
+	// 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
+	//   before calling this function using setBypassTiltrotorControls
 	// - collective tilt is added to actuator_sp even if
 	//   (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT)
 	//   evaluates to false
 
-	if (matrix_index == 0) {
-		tiltrotor_extra_controls_s tiltrotor_extra_controls;
+	_bypass_tiltrotor_controls = bypass;
+	_collective_tilt_normalized_setpoint = collective_tilt;
+	_collective_thrust_normalized_setpoint = collective_thrust;
+}
 
-		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || preflight_check_running) {
+void ActuatorEffectivenessTiltrotorVTOL::processTiltrotorControls(ActuatorVector &actuator_sp,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 
-			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;
+{
+	tiltrotor_extra_controls_s tiltrotor_extra_controls;
 
-			if (preflight_check_running) {
-				control_collective_tilt = collective_tilt_normalized_setpoint * 2.f - 1.f;
-				control_collective_thrust = collective_thrust_normalized_setpoint;
-			}
+	if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || _bypass_tiltrotor_controls) {
 
-			// 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;
+		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;
 
-			// initialize _last_collective_tilt_control
-			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
-				_last_collective_tilt_control = control_collective_tilt;
+		if (_bypass_tiltrotor_controls) {
+			control_collective_tilt = _collective_tilt_normalized_setpoint * 2.f - 1.f;
+			control_collective_thrust = _collective_thrust_normalized_setpoint;
+		}
 
-			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
-				_collective_tilt_updated = true;
-				_last_collective_tilt_control = control_collective_tilt;
-			}
+		// 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;
 
-			// 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);
+		// initialize _last_collective_tilt_control
+		if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+			_last_collective_tilt_control = control_collective_tilt;
 
-			} else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) {
-				_last_collective_tilt_control = -1.f;
-			}
+		} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+			_collective_tilt_updated = true;
+			_last_collective_tilt_control = control_collective_tilt;
+		}
 
-			bool yaw_saturated_positive = true;
-			bool yaw_saturated_negative = true;
+		// 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);
 
-			for (int i = 0; i < _tilts.count(); ++i) {
-				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
+		} else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) {
+			_last_collective_tilt_control = -1.f;
+		}
 
-					// 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;
+		bool yaw_saturated_positive = true;
+		bool yaw_saturated_negative = true;
 
-					} else {
-						actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
-					}
-				}
+		for (int i = 0; i < _tilts.count(); ++i) {
+			if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
 
-				// 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) {
+				// 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;
 
-					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;
-					}
+				} else {
+					actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position
 				}
 			}
 
-			_yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative;
-			_yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive;
+			// 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) {
 
-			// 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;
+				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)
+{
+	if (matrix_index == 0) {
+
+		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

@@ -82,13 +82,19 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "VTOL Tiltrotor"; }
 
 	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;
@@ -115,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;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp

@@ -57,8 +57,7 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati
 
 void ActuatorEffectivenessUUV::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, bool preflight_check_running,
-		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp

@@ -56,8 +56,7 @@ public:
 
 	void 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, bool preflight_check_running = true,
-			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "UUV"; }