diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 05fcaf675e46..20f81d0e2b06 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/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: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index c7a588954f53..37e2f2e692b9 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -75,6 +75,7 @@ class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public Actuator void updateSetpoint(const matrix::Vector &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)}; }; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 8b159d24818f..5f3759e077bb 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/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]; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 96727f0c536f..b4d7f38d1b8e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/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);