Tailsitter: Stop motors if throttle low in FW

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -56,8 +56,11 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
 
 	// MC motors
 	configuration.selected_matrix = 0;
+
+	_mc_motors_needed_for_rate_control = _mc_rotors.geometry().num_rotors > 3;
+
 	// enable MC yaw control if more than 3 rotors
-	_mc_rotors.enableYawByDifferentialThrust(_mc_rotors.geometry().num_rotors > 3);
+	_mc_rotors.enableYawByDifferentialThrust(_mc_motors_needed_for_rate_control);
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
 
 	// Control Surfaces
@@ -88,6 +91,35 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d
 	}
 }
 
+void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
+{
+	// If the "MC" motors are not needed for fixed-wing rate control, switch them off on low thrust.
+	// The threshold of 2% was determined empirically (RC stick inaccuracy)
+	if (!_mc_motors_needed_for_rate_control && _flight_phase == FlightPhase::FORWARD_FLIGHT) {
+
+		const int num_rotors = _mc_rotors.geometry().num_rotors;
+
+		// Find out whether *all* forward rotors have low thrust. If only a subset has low thrust,
+		// swiching the subset off would produce unpredictable torque response
+		bool all_forwards_motors_low_thrust = true;
+
+		for (int i = 0; i < num_rotors; i++) {
+			if ((_forwards_motors_mask & (1 << i)) && actuator_sp(i) > 0.02f) {
+				all_forwards_motors_low_thrust = false;
+			}
+		}
+
+		// Check inside of the loop to always be close to worst case performance
+		for (int i = 0; i < num_rotors; i++) {
+			if (all_forwards_motors_low_thrust && _forwards_motors_mask & (1 << i)) {
+				// NaN is later translated to disarmed PWM
+				actuator_sp(i) = NAN;
+			}
+		}
+	}
+}
+
 void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
 {
 	if (_flight_phase == flight_phase) {

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -72,6 +72,9 @@ 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 ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
+
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	const char *name() const override { return "VTOL Tailsitter"; }
@@ -84,6 +87,9 @@ protected:
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 
+	// Default true, if false we allow turning off motors in fixed-wing
+	bool _mc_motors_needed_for_rate_control{true};
+
 	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
 	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 };