preflight check: only change nav_mode once

we now use optional args to give
ActuatorEffectivenessTiltrotorVTOL::updateSetpoint the necessary info
about the preflight check. All the other
ActuatorEffectiveness*::updateSetpoint functions can be used exactly
like before, by ignoring the optional arguments.

Rather than the previous solution of setting a member variable only when
_actuator_effectiveness is an instance of
ActuatorEffectivenessTiltrotorVTOL, this also allows us to lose the ugly
dynamic_cast (poor cpp man's instanceof).

the other dynamic_cast is replaced by checking _effectiveness_source_id
which should be equivalent.

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -101,8 +101,3 @@ 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,7 +198,8 @@ 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) {}
+				    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) {}
 
 	/**
 	 * Get a bitmask of motors to be stopped
@@ -219,18 +220,6 @@ 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,6 +408,7 @@ 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);
@@ -415,6 +416,7 @@ 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]);
 			}
@@ -842,10 +844,6 @@ 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;
@@ -1574,6 +1572,42 @@ 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,7 +65,6 @@ 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,
@@ -158,11 +157,6 @@ 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,20 +450,12 @@ ControlAllocator::Run()
 			// 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);
-
-			}
+			float preflight_check_tilt_sp = preflight_check_get_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());
+								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax(),
+								_preflight_check_running, preflight_check_tilt_sp, 0.0f);
 
 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);
@@ -491,8 +483,7 @@ 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;
@@ -501,7 +492,6 @@ void ControlAllocator::preflight_check_update_state()
 	// negative torque setpoints.
 
 	int n_axes = 3;
-
 	if (tiltrotor) {
 		n_axes = 4;
 	}
@@ -509,7 +499,6 @@ 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?
@@ -517,9 +506,7 @@ 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;
@@ -541,8 +528,7 @@ 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,7 +61,8 @@ 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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -47,7 +47,8 @@ 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) override;
+			    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 char *name() const override { return "Custom"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -53,7 +53,8 @@ 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) override;
+			    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;
 
 private:
 	ActuatorEffectivenessRotors _rotors;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -80,7 +80,8 @@ 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) override;
+			    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;
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp

@@ -71,7 +71,8 @@ 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) override;
+			    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;
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -57,7 +57,8 @@ 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) override;
+			    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 char *name() const override { return "MC Tilt"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp

@@ -45,7 +45,8 @@ 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) override;
+			    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 char *name() const override { return "Rover (Ackermann)"; }
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -77,7 +77,8 @@ 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) override;
+			    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;
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -124,121 +124,105 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
 
 }
 
-void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt,
-		float 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, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
-	// if _bypass_tiltrotor_controls (used for control surface preflight
-	// check), we alter the behaviour of processTiltrotorControls in these
-	// two ways:
+	// apply tilt
+
+	// if preflight_check_running, we alter the behaviour 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
+	//   before calling this function
 	// - 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)
-{
 	if (matrix_index == 0) {
+		tiltrotor_extra_controls_s tiltrotor_extra_controls;
 
-		processTiltrotorControls(actuator_sp, actuator_min, actuator_max);
+		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || preflight_check_running) {
+
+			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 (preflight_check_running) {
+				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 || 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) = control_collective_thrust;
+				}
+			}
+		}
 
 		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
 			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -82,19 +82,13 @@ 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) override;
+			    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 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;
@@ -121,10 +115,6 @@ 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,7 +57,8 @@ 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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp

@@ -56,7 +56,8 @@ 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) override;
+			    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 char *name() const override { return "UUV"; }