From 3370b4583a07898dd6eafdbf2af06520681078a0 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 28 May 2024 10:00:15 +0200 Subject: [PATCH] CA: enable differential thrust on matrix 1 in VTOL FW Introduce differential thrust SCALE and WEIGHT params that are used to configure differential thrust on a VTOL vehicle. Signed-off-by: Silvan Fuhrer --- .../init.d-posix/airframes/10042_sihsim_xvert | 1 - .../airframes/1041_gazebo-classic_tailsitter | 1 - .../1045_gazebo-classic_quadtailsitter | 1 - .../airframes/1102_tailsitter_duo_sih.hil | 1 - .../control_allocator/ControlAllocator.cpp | 48 ++++++++++++- .../control_allocator/ControlAllocator.hpp | 8 ++- src/modules/control_allocator/module.yaml | 67 ++++++++++++++++++- src/modules/vtol_att_control/tailsitter.cpp | 13 ---- src/modules/vtol_att_control/tiltrotor.cpp | 4 -- .../vtol_att_control_params.c | 55 --------------- src/modules/vtol_att_control/vtol_type.h | 10 --- 11 files changed, 118 insertions(+), 91 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index 536fc6076c..069693c294 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -21,7 +21,6 @@ param set-default VT_B_TRANS_DUR 5 param set-default VT_ELEV_MC_LOCK 0 param set-default VT_TYPE 0 param set-default VT_FW_DIFTHR_EN 1 -param set-default VT_FW_DIFTHR_S_Y 0.3 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter index 8a3d5d1a49..93220c5999 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter @@ -70,7 +70,6 @@ param set-default MC_PITCH_P 3 param set-default VT_ARSP_TRANS 10 param set-default VT_B_TRANS_DUR 5 param set-default VT_FW_DIFTHR_EN 1 -param set-default VT_FW_DIFTHR_S_Y 1 param set-default VT_F_TRANS_DUR 1.5 param set-default VT_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter index 87afb523b1..14955afad6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter @@ -67,7 +67,6 @@ param set-default MC_PITCHRATE_P 0.3 param set-default VT_ARSP_TRANS 15 param set-default VT_B_TRANS_DUR 5 param set-default VT_FW_DIFTHR_EN 7 -param set-default VT_FW_DIFTHR_S_Y 1 param set-default VT_F_TRANS_DUR 1.5 param set-default VT_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil index 6ac7bea691..d737064266 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -25,7 +25,6 @@ param set-default VT_ELEV_MC_LOCK 0 param set-default VT_MOT_COUNT 2 param set-default VT_TYPE 0 param set-default VT_FW_DIFTHR_EN 1 -param set-default VT_FW_DIFTHR_S_Y 0.3 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index dcd2835bba..8881dcb33d 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -408,6 +408,17 @@ ControlAllocator::Run() update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE); } + float fw_dthr_weight[3] = {_param_ca_fw_dthr_wgt_r.get(), _param_ca_fw_dthr_wgt_p.get(), _param_ca_fw_dthr_wgt_y.get()}; + float fw_dthr_scale[3] = {_param_ca_fw_dthr_sc_r.get(), _param_ca_fw_dthr_sc_p.get(), _param_ca_fw_dthr_sc_y.get()}; + + // VTOL Tailsitters have roll/yaw axis swaped in fixed-wing mode + if (_effectiveness_source_id == EffectivenessSource::TAILSITTER_VTOL) { + fw_dthr_weight[2] = fw_dthr_weight[0]; + fw_dthr_weight[0] = fw_dthr_weight[2]; + fw_dthr_scale[2] = fw_dthr_scale[0]; + fw_dthr_scale[0] = fw_dthr_scale[2]; + } + // Set control setpoint vector(s) matrix::Vector c[ActuatorEffectiveness::MAX_NUM_MATRICES]; c[0](0) = _torque_sp(0); @@ -418,10 +429,17 @@ ControlAllocator::Run() c[0](5) = _thrust_sp(2); if (_num_control_allocation > 1) { + matrix::Vector vehicle_torque_setpoint_matrix_1; + if (_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint)) { - c[1](0) = vehicle_torque_setpoint.xyz[0]; - c[1](1) = vehicle_torque_setpoint.xyz[1]; - c[1](2) = vehicle_torque_setpoint.xyz[2]; + vehicle_torque_setpoint_matrix_1(0) = vehicle_torque_setpoint.xyz[0]; + vehicle_torque_setpoint_matrix_1(1) = vehicle_torque_setpoint.xyz[1]; + vehicle_torque_setpoint_matrix_1(2) = vehicle_torque_setpoint.xyz[2]; + + // reduce outputs on servos (matrix 1) if differential thrust with motors (matrix 0) is enabled + c[1](0) = vehicle_torque_setpoint_matrix_1(0) * (1.f - fw_dthr_weight[0]); + c[1](1) = vehicle_torque_setpoint_matrix_1(1) * (1.f - fw_dthr_weight[1]); + c[1](2) = vehicle_torque_setpoint_matrix_1(2) * (1.f - fw_dthr_weight[2]); } if (_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint)) { @@ -429,6 +447,30 @@ ControlAllocator::Run() c[1](4) = vehicle_thrust_setpoint.xyz[1]; c[1](5) = vehicle_thrust_setpoint.xyz[2]; } + + const bool has_non_zero_dthr_weight = fw_dthr_weight[0] > FLT_EPSILON || fw_dthr_weight[1] > FLT_EPSILON + || fw_dthr_weight[2] > FLT_EPSILON; + + // VTOL differential thrust in FW + if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT && has_non_zero_dthr_weight) { + /* CA_FW_DTHR_SC_R etc are scales to tune response of differential thrust around each axis. + The scaling factor is applied to the (normalized) torque setpoint from the rate controller going + to the motors for rate control in fixed-wing flight using differential thrust instead of + aerodynamic control surfaces. + Set this parameter such that when the control surfaces are disabled, + the systems rate tracking is maintained as best as possible through + differential thrust. + */ + + c[0](0) = vehicle_torque_setpoint_matrix_1(0) * fw_dthr_scale[0] * fw_dthr_weight[0]; + c[0](1) = vehicle_torque_setpoint_matrix_1(1) * fw_dthr_scale[1] * fw_dthr_weight[1]; + c[0](2) = vehicle_torque_setpoint_matrix_1(2) * fw_dthr_scale[2] * fw_dthr_weight[2]; + + _actuator_effectiveness->setEnableAuxiliaryMotors(true); + + } else if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT && !has_non_zero_dthr_weight) { + _actuator_effectiveness->setEnableAuxiliaryMotors(false); + } } for (int i = 0; i < _num_control_allocation; ++i) { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 925f95a2d8..4c7dd39426 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -217,7 +217,13 @@ private: (ParamInt) _param_ca_airframe, (ParamInt) _param_ca_method, (ParamInt) _param_ca_failure_mode, - (ParamInt) _param_r_rev + (ParamInt) _param_r_rev, + (ParamFloat) _param_ca_fw_dthr_sc_r, + (ParamFloat) _param_ca_fw_dthr_sc_p, + (ParamFloat) _param_ca_fw_dthr_sc_y, + (ParamFloat) _param_ca_fw_dthr_wgt_r, + (ParamFloat) _param_ca_fw_dthr_wgt_p, + (ParamFloat) _param_ca_fw_dthr_wgt_y ) }; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 8683d7477d..480215baf7 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -40,7 +40,7 @@ parameters: If set to Automtic, the selection is based on the airframe (CA_AIRFRAME). type: enum values: - 0: Pseudo-inverse with output clipping + 0: Pseudo-inverse without any desaturation 1: Pseudo-inverse with sequential desaturation technique 2: Automatic default: 2 @@ -541,6 +541,71 @@ parameters: 1: Remove first failed motor from effectiveness default: 0 + CA_FW_DTHR_SC_R: + description: + short: Fixed-wing differential thrust roll scale + type: float + decimal: 2 + increment: 0.1 + min: 0 + max: 10 + default: 1 + + CA_FW_DTHR_SC_P: + description: + short: Fixed-wing differential thrust pitch scale + type: float + decimal: 2 + increment: 0.1 + min: 0 + max: 10 + default: 1 + + CA_FW_DTHR_SC_Y: + description: + short: Fixed-wing differential thrust yaw scale + type: float + decimal: 2 + increment: 0.1 + min: 0 + max: 10 + default: 1 + + CA_FW_DTHR_WGT_R: + description: + short: Fixed-wing differential thrust roll weight + long: | + Weight of torque allocated through differential thrust vs control surface. + type: float + decimal: 2 + increment: 0.1 + min: 0 + max: 1 + default: 0 + + CA_FW_DTHR_WGT_P: + description: + short: Pitch weight of differential thrust in fixed-wing + long: | + Weight of torque allocated through differential thrust vs control surface. + type: float + decimal: 2 + increment: 0.1 + min: 0 + max: 1 + default: 0 + + CA_FW_DTHR_WGT_Y: + description: + short: Yaw weight of differential thrust in fixed-wing + long: | + Weight of torque allocated through differential thrust vs control surface. + type: float + decimal: 2 + increment: 0.1 + min: 0 + max: 1 + default: 0 # Mixer mixer: actuator_types: diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index af95a05083..09ea8f9cb8 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -291,19 +291,6 @@ void Tailsitter::fill_actuator_outputs() _thrust_setpoint_0->xyz[2] = -_vehicle_thrust_setpoint_virtual_fw->xyz[0]; - /* allow differential thrust if enabled */ - if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { - _torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0] * _param_vt_fw_difthr_s_y.get(); - } - - if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::PITCH_BIT)) { - _torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1] * _param_vt_fw_difthr_s_p.get(); - } - - if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::ROLL_BIT)) { - _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_r.get(); - } - // for the short period after switching to FW where there is no thrust published yet from the FW controller, // keep publishing the last MC thrust to keep the motors running if (hrt_elapsed_time(&_trans_finished_ts) < 50_ms) { diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 0956e10036..b222644dde 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -379,10 +379,6 @@ void Tiltrotor::fill_actuator_outputs() collective_thrust_normalized_setpoint = _vehicle_thrust_setpoint_virtual_fw->xyz[0]; _thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint; - /* allow differential thrust if enabled */ - if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { - _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get() ; - } } else { collective_thrust_normalized_setpoint = -_vehicle_thrust_setpoint_virtual_mc->xyz[2] * _mc_throttle_weight; 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 3bcb56e498..24fbe792da 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -283,61 +283,6 @@ PARAM_DEFINE_INT32(VT_FW_QC_HMAX, 0); */ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f); -/** - * Differential thrust in forwards flight. - * - * Enable differential thrust seperately for roll, pitch, yaw in forward (fixed-wing) mode. - * The effectiveness of differential thrust around the corresponding axis can be - * tuned by setting VT_FW_DIFTHR_S_R / VT_FW_DIFTHR_S_P / VT_FW_DIFTHR_S_Y. - * - * @min 0 - * @max 7 - * @bit 0 Yaw - * @bit 1 Roll - * @bit 2 Pitch - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); - -/** - * Roll differential thrust factor in forward flight - * - * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN. - * - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.1 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_R, 1.f); - -/** - * Pitch differential thrust factor in forward flight - * - * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN. - * - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.1 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f); - -/** - * Yaw differential thrust factor in forward flight - * - * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN. - * - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.1 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f); - /** * Backtransition deceleration setpoint to pitch I gain. * diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 8f9ae9ef38..541e25db6d 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -76,12 +76,6 @@ enum VtolForwardActuationMode { ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND }; -// enum for bitmask of VT_FW_DIFTHR_EN parameter options -enum class VtFwDifthrEnBits : int32_t { - YAW_BIT = (1 << 0), - ROLL_BIT = (1 << 1), - PITCH_BIT = (1 << 2), -}; enum class QuadchuteReason { None = 0, @@ -344,10 +338,6 @@ protected: (ParamBool) _param_fw_use_airspd, (ParamFloat) _param_vt_trans_timeout, (ParamFloat) _param_mpc_xy_cruise, - (ParamInt) _param_vt_fw_difthr_en, - (ParamFloat) _param_vt_fw_difthr_s_y, - (ParamFloat) _param_vt_fw_difthr_s_p, - (ParamFloat) _param_vt_fw_difthr_s_r, (ParamFloat) _param_vt_b_dec_i, (ParamFloat) _param_vt_b_dec_mss,