fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

enable optional differential thrust for pitch and roll in fw mode

Browse Source
This commit is contained in:
Thomas Stastny 2022-05-19 15:31:13 -07:00
parent cbac0e8b8c
commit fd78164ea2
3 changed files with 75 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -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;

6
src/modules/vtol_att_control/tailsitter.h
View File

@ -81,7 +81,11 @@ private:
void parameters_update() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
(ParamInt<px4::params::VT_FW_DIFTHR_P>) _param_vt_fw_difthr_p,
(ParamInt<px4::params::VT_FW_DIFTHR_R>) _param_vt_fw_difthr_r,
(ParamInt<px4::params::VT_ELEV_LOCK>) _param_vt_elev_lock,
(ParamInt<px4::params::VT_AIL_FW_LOCK>) _param_vt_ail_fw_lock
)

40
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -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
*