diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert index c87491c413..45c5d7ba73 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert @@ -14,7 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert} 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_SC 0.3 +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_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter index d72ea64da2..1b9bb77115 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter @@ -70,7 +70,7 @@ param set-default MPC_XY_VEL_D_ACC 0.1 param set-default NAV_ACC_RAD 5 param set-default VT_FW_DIFTHR_EN 1 -param set-default VT_FW_DIFTHR_SC 0.5 +param set-default VT_FW_DIFTHR_S_Y 0.5 param set-default VT_F_TRANS_DUR 1.5 param set-default VT_F_TRANS_THR 0.7 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 c883b1f2bb..9ba9822367 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 @@ -22,7 +22,7 @@ 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_SC 0.3 +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/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 0bb35fbf58..e0c4b4d6ad 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -250,5 +250,14 @@ bool param_modify_on_import(bson_node_t node) } } + // 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y + { + if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) { + strcpy(node->name, "VT_FW_DIFTHR_S_Y"); + PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y"); + return true; + } + } + return false; } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 2d6ef23f46..fd47faadef 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -343,9 +343,22 @@ void Tailsitter::fill_actuator_outputs() _thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE]; /* allow differential thrust if enabled */ - if (_param_vt_fw_difthr_en.get()) { - mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ; - _torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ; + if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { + float yaw_control = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get(); + mc_out[actuator_controls_s::INDEX_ROLL] = yaw_control; + _torque_setpoint_0->xyz[0] = yaw_control; + } + + if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::PITCH_BIT)) { + float pitch_control = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get(); + mc_out[actuator_controls_s::INDEX_PITCH] = pitch_control; + _torque_setpoint_0->xyz[1] = pitch_control; + } + + if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::ROLL_BIT)) { + float roll_control = -fw_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get(); + mc_out[actuator_controls_s::INDEX_YAW] = roll_control; + _torque_setpoint_0->xyz[2] = roll_control; } } else { diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3d2c2e2aa3..ab20579b79 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -481,9 +481,9 @@ void Tiltrotor::fill_actuator_outputs() _thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE]; /* allow differential thrust if enabled */ - if (_param_vt_fw_difthr_en.get()) { - mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ; - _torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ; + if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { + mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; + _torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; } } else { 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 2fefc5c5bd..a6e408fc9f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -254,27 +254,63 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f); /** * Differential thrust in forwards flight. * - * Set to 1 to enable differential thrust in fixed-wing 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 1 - * @decimal 0 + * @max 7 + * @bit 0 Yaw + * @bit 1 Roll + * @bit 2 Pitch * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); /** - * Differential thrust scaling factor + * Roll differential thrust factor in forward flight * - * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight. + * Maps the roll control output in forward flight to the actuator differential thrust output. + * + * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN. * * @min 0.0 - * @max 1.0 + * @max 2.0 * @decimal 2 * @increment 0.1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f); +PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_R, 1.f); + +/** + * Pitch differential thrust factor in forward flight + * + * Maps the pitch control output in forward flight to the actuator differential thrust output. + * + * 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 + * + * Maps the yaw control output in forward flight to the actuator differential thrust output. + * + * 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 feedforward gain. diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 326373e760..8bc1a27b80 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -77,6 +77,13 @@ 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), +}; + class VtolAttitudeControl; class VtolType : public ModuleParams @@ -239,8 +246,10 @@ protected: (ParamBool) _param_fw_arsp_mode, (ParamFloat) _param_vt_trans_timeout, (ParamFloat) _param_mpc_xy_cruise, - (ParamBool) _param_vt_fw_difthr_en, - (ParamFloat) _param_vt_fw_difthr_sc, + (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_ff, (ParamFloat) _param_vt_b_dec_i, (ParamFloat) _param_vt_b_dec_mss,