FixedWing: explicitly stop forward motor with zero thrust

This allows PWM and all other output methods to configure
stopped, idling and full thrust points and use them consistently.
The fact that a fixed wing motor can be stopped when zero thrust
is demanded is explicit and could in principle even be disabled.
The mechanism is the same as for a standard VTOL stopping the
multicopter motors in the fixed wing flight phase.

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -53,6 +53,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	// Motors
 	_rotors.enablePropellerTorque(false);
 	const bool rotors_added_successfully = _rotors.addActuators(configuration);
+	_forwards_motors_mask = _rotors.getForwardsMotors();
 
 	// Control Surfaces
 	_first_control_surface_idx = configuration.num_actuators_matrix[0];
@@ -61,6 +62,13 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	return (rotors_added_successfully && surfaces_added_successfully);
 }
 
+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)
+{
+	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+}
+
 void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index,
 		ActuatorVector &actuator_sp)
 {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -51,6 +51,10 @@ public:
 
 	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 
+	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;
+
 private:
 	ActuatorEffectivenessRotors _rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
@@ -59,4 +63,6 @@ private:
 	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
+
+	uint32_t _forwards_motors_mask{};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -280,6 +280,21 @@ uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
 	return upwards_motors;
 }
 
+uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const
+{
+	uint32_t forward_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		const Vector3f &axis = _geometry.rotors[i].axis;
+
+		if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) {
+			forward_motors |= 1u << i;
+		}
+	}
+
+	return forward_motors;
+}
+
 bool
 ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

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