TiltrotorVTOL: allow stopping front tilted motors in fast forward flight

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -265,6 +265,17 @@ Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_di
 	return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
 }
 
+uint32_t ActuatorEffectivenessRotors::getMotors() const
+{
+	uint32_t motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		motors |= 1u << i;
+	}
+
+	return motors;
+}
+
 uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
 {
 	uint32_t upwards_motors = 0;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -126,6 +126,7 @@ public:
 
 	void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
 
+	uint32_t getMotors() const;
 	uint32_t getUpwardsMotors() const;
 	uint32_t getForwardsMotors() const;
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -68,10 +68,11 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
 	const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ?
 			-1.f : _last_collective_tilt_control;
-	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
+	_untiltable_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
-			    << configuration.num_actuators[(int)ActuatorType::MOTORS];
+			     << configuration.num_actuators[(int)ActuatorType::MOTORS];
 
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
+	_motors = _mc_rotors.getMotors();
 
 	// Control Surfaces
 	configuration.selected_matrix = 1;
@@ -118,7 +119,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 {
 	// apply tilt
 	if (matrix_index == 0) {
-
 		tiltrotor_extra_controls_s tiltrotor_extra_controls;
 
 		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
@@ -145,12 +145,15 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 
 			// 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;
 				}
 			}
 		}
+
+		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
+		}
 	}
 
 	// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
@@ -180,7 +183,7 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 	// update stopped motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:
-		_stopped_motors_mask = _nontilted_motors;
+		_stopped_motors_mask |= _untiltable_motors;
 		break;
 
 	case FlightPhase::HOVER_FLIGHT:

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -84,8 +84,6 @@ public:
 
 	const char *name() const override { return "VTOL Tiltrotor"; }
 
-	uint32_t getStoppedMotors() const override { return _stopped_motors; }
-
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 
 protected:
@@ -94,8 +92,8 @@ protected:
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 	ActuatorEffectivenessTilts _tilts;
 
-	uint32_t _nontilted_motors{}; ///< motors that are not tiltable
+	uint32_t _motors{};
-	uint32_t _stopped_motors{}; ///< currently stopped motors
+	uint32_t _untiltable_motors{};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 	int _first_tilt_idx{0}; ///< applies to matrix 0