From fd78164ea26e084e4628b30cf3b94b34092b8ab5 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Thu, 19 May 2022 15:31:13 -0700 Subject: [PATCH] enable optional differential thrust for pitch and roll in fw mode --- src/modules/vtol_att_control/tailsitter.cpp | 39 +++++++++++++----- src/modules/vtol_att_control/tailsitter.h | 6 ++- .../vtol_att_control_params.c | 40 +++++++++++++++++++ 3 files changed, 75 insertions(+), 10 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 4d677a26b0..630b61131c 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -336,17 +336,23 @@ void Tailsitter::fill_actuator_outputs() // FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation _thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE]; - /* allow differential thrust if enabled */ + /* allow differential thrust for yaw 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() ; } - mc_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_0->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; + if (_param_vt_fw_difthr_r.get()) { + // enable differential thrust for fixed-wing roll control + mc_out[actuator_controls_s::INDEX_YAW] = -fw_in[actuator_controls_s::INDEX_ROLL]; + _torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_ROLL]; + } - mc_out[actuator_controls_s::INDEX_YAW] = -fw_in[actuator_controls_s::INDEX_ROLL]; - _torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_ROLL]; + if (_param_vt_fw_difthr_p.get()) { + // enable differential thrust for fixed-wing pitch control + mc_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; + _torque_setpoint_0->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; + } } else { _torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL]; @@ -370,10 +376,25 @@ void Tailsitter::fill_actuator_outputs() fw_out[actuator_controls_s::INDEX_PITCH] = 0; } else { - fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; - fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; - _torque_setpoint_1->xyz[2] = -fw_in[actuator_controls_s::INDEX_ROLL]; + if (_param_vt_elev_lock.get()) { + // lock the elevator always + fw_out[actuator_controls_s::INDEX_PITCH] = 0.0f; + _torque_setpoint_1->xyz[1] = 0.0f; + + } else { + fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; + _torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; + } + + if (_param_vt_ail_fw_lock.get() && _vtol_schedule.flight_mode != vtol_mode::MC_MODE) { + // lock the ailerons in transition and fixed-wing + fw_out[actuator_controls_s::INDEX_ROLL] = 0.0f; + _torque_setpoint_1->xyz[2] = 0.0f; + + } else { + fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; + _torque_setpoint_1->xyz[2] = -fw_in[actuator_controls_s::INDEX_ROLL]; + } } _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 36dd05b39a..0d4836e89e 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -81,7 +81,11 @@ private: void parameters_update() override; DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType, - (ParamFloat) _param_fw_psp_off + (ParamFloat) _param_fw_psp_off, + (ParamInt) _param_vt_fw_difthr_p, + (ParamInt) _param_vt_fw_difthr_r, + (ParamInt) _param_vt_elev_lock, + (ParamInt) _param_vt_ail_fw_lock ) 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 035426ac07..e498fb067c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -75,6 +75,22 @@ PARAM_DEFINE_INT32(VT_TYPE, 0); */ PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 1); +/** + * Lock elevator action always + * + * @boolean + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_ELEV_LOCK, 1); + +/** + * Lock ailerons in transition and fixed-wing modes + * + * @boolean + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_AIL_FW_LOCK, 0); + /** * Duration of a front transition * @@ -297,6 +313,30 @@ PARAM_DEFINE_INT32(VT_MOT_ID, 0); */ PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); +/** + * Differential thrust in forwards flight for pitch. + * + * Set to 1 to enable differential thrust in fixed-wing flight. + * + * @min 0 + * @max 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_DIFTHR_P, 1); + +/** + * Differential thrust in forwards flight for roll. + * + * Set to 1 to enable differential thrust in fixed-wing flight. + * + * @min 0 + * @max 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_DIFTHR_R, 1); + /** * Differential thrust scaling factor *