[control allocator] Enhance Tailsitter control allocation to support more complex drone configurations

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -44,13 +44,24 @@ using namespace matrix;
 ActuatorEffectivenessTailsitterVTOL::ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent)
 	: ModuleParams(parent), _mc_rotors(this), _control_surfaces(this)
 {
+	_param_handles.vt_elev_mc_lock = param_find("VT_ELEV_MC_LOCK");
+	_param_handles.vt_ts_cs_hvr_dis = param_find("VT_TS_CS_HVR_DIS");
+	updateParams();
 	setFlightPhase(FlightPhase::HOVER_FLIGHT);
 }
+
+void ActuatorEffectivenessTailsitterVTOL::updateParams()
+{
+	ModuleParams::updateParams();
+	param_get(_param_handles.vt_elev_mc_lock, &_param_vt_elev_mc_lock);
+	param_get(_param_handles.vt_ts_cs_hvr_dis, &_param_vt_ts_cs_hvr_dis);
+}
+
 bool
 ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)
 {
-	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+	if (!_should_update_effectiveness_matrix && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
@@ -65,6 +76,24 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
 	_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
 	const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
 
+	// In HOVER_FLIGHT, selectively disable some control surfaces based on VT_TS_CS_HVR_DIS bitmask.
+	// Note: VT_ELEV_MC_LOCK=0 means control surfaces are unlocked (enabled), while VT_TS_CS_HVR_DIS can still disable specific ones.
+	if (surfaces_added_successfully && _flight_phase == FlightPhase::HOVER_FLIGHT &&
+	    _param_vt_elev_mc_lock == 0 && _param_vt_ts_cs_hvr_dis != 0) {
+		// VT_TS_CS_HVR_DIS bitmask: bit[i]=1 disables surface i, bit[i]=0 keeps it enabled
+		for (int i = 0; i < _control_surfaces.count(); i++) {
+			if ((_param_vt_ts_cs_hvr_dis & (1 << i)) != 0) {
+				// Set the corresponding column in effectiveness matrix to zero
+				for (int row = 0; row < NUM_AXES; row++) {
+					configuration.effectiveness_matrices[1](row, _first_control_surface_idx + i) = 0.f;
+				}
+			}
+		}
+	}
+
+	// Reset flag after update
+	_should_update_effectiveness_matrix = false;
+
 	return (mc_rotors_added_successfully && surfaces_added_successfully);
 }
 
@@ -104,6 +133,11 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig
 
 	ActuatorEffectiveness::setFlightPhase(flight_phase);
 
+	// On flight phase change, check whether unlocked surfaces have any per-surface disables.
+	if (_param_vt_elev_mc_lock == 0 && _param_vt_ts_cs_hvr_dis != 0) {
+		_should_update_effectiveness_matrix = true;
+	}
+
 	// update stopped motors
 	switch (flight_phase) {
 	case FlightPhase::FORWARD_FLIGHT:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -75,6 +75,7 @@ class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public Actuator
 	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 updateParams() override;
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
@@ -88,6 +89,16 @@ class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public Actuator
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 
+	struct ParamHandles {
+		param_t vt_elev_mc_lock;
+		param_t vt_ts_cs_hvr_dis;
+	} _param_handles{};
+
+	int32_t _param_vt_elev_mc_lock{1}; ///< Lock control surfaces in hover (default: locked)
+	int32_t _param_vt_ts_cs_hvr_dis{0}; ///< Bitmask to disable specific surfaces in hover (only effective when VT_ELEV_MC_LOCK=0)
+
+	bool _should_update_effectiveness_matrix{false}; ///< flag to trigger control effectiveness matrix update on flight phase change
+
 	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
 	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 };

src/modules/vtol_att_control/tailsitter.cpp

@@ -284,7 +284,7 @@ void Tailsitter::fill_actuator_outputs()
 	_thrust_setpoint_1->xyz[1] = 0.f;
 	_thrust_setpoint_1->xyz[2] = 0.f;
 
-	// Motors
+	// Motors: Generating force and torque (optional for FW_MODE)
 	if (_vtol_mode == vtol_mode::FW_MODE) {
 
 		_thrust_setpoint_0->xyz[2] = -_vehicle_thrust_setpoint_virtual_fw->xyz[0];
@@ -325,8 +325,16 @@ void Tailsitter::fill_actuator_outputs()
 		_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2];
 	}
 
-	// Control surfaces
-	if (!_param_vt_elev_mc_lock.get() || _vtol_mode != vtol_mode::MC_MODE) {
+	// Control surfaces: Generating torque (optional for MC_MODE)
+	// VT_ELEV_MC_LOCK=0 enables control surfaces in hover, VT_TS_CS_HVR_DIS can still
+	// selectively disable individual surfaces (e.g., those outside propeller slipstream).
+	if (!_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) {
+		// In MC_MODE apply torque from multirotor controller
+		_torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0];
+		_torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1];
+		_torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2];
+
+	} else {
 		_torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0];
 		_torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1];
 		_torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2];

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -393,3 +393,26 @@ PARAM_DEFINE_FLOAT(VT_LND_PITCH_MIN, 0.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_SPOILER_MC_LD, 0.f);
+
+/**
+ * Tailsitter: Disable control surfaces in hover mode
+ *
+ * Bitmask to disable specific control surfaces for attitude control in hover.
+ * Set the bit to 1 to disable the corresponding surface (e.g., surfaces outside
+ * propeller slipstream). Default 0 means all surfaces enabled.
+ *
+ * Only takes effect when VT_ELEV_MC_LOCK=0.
+ *
+ * @min 0
+ * @max 255
+ * @bit 0 Surface 0 (CA_SV_CS0)
+ * @bit 1 Surface 1 (CA_SV_CS1)
+ * @bit 2 Surface 2 (CA_SV_CS2)
+ * @bit 3 Surface 3 (CA_SV_CS3)
+ * @bit 4 Surface 4 (CA_SV_CS4)
+ * @bit 5 Surface 5 (CA_SV_CS5)
+ * @bit 6 Surface 6 (CA_SV_CS6)
+ * @bit 7 Surface 7 (CA_SV_CS7)
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_TS_CS_HVR_DIS, 0);