dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

VTOL: standard VTOL and Tiltrotor manual MC throttle during transition 

This sets the throttle of the Multicopter actuators to the throttle stick position
while in Stabilized, Acro or Manual mode.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2020-11-19 19:53:14 +01:00 committed by Lorenz Meier
parent 802c68a62e
commit 706f806943
2 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/vtol_att_control/standard.cpp
View File

@ -267,6 +267,11 @@ void Standard::update_transition_state()
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
if (!_v_control_mode->flag_control_climb_rate_enabled) {
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d); q_sp.copyTo(_v_att_sp->q_d);
@ -287,6 +292,11 @@ void Standard::update_transition_state()
_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
} }
// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
if (!_v_control_mode->flag_control_climb_rate_enabled) {
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d); q_sp.copyTo(_v_att_sp->q_d);

15
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -322,6 +322,11 @@ void Tiltrotor::update_transition_state()
_thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; _thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
if (!_v_control_mode->flag_control_climb_rate_enabled) {
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) { } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely // the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = _params_tiltrotor.tilt_transition + _tilt_control = _params_tiltrotor.tilt_transition +
@ -341,6 +346,11 @@ void Tiltrotor::update_transition_state()
_thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; _thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
if (!_v_control_mode->flag_control_climb_rate_enabled) {
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
// turn on all MC motors // turn on all MC motors
set_all_motor_state(motor_state::ENABLED); set_all_motor_state(motor_state::ENABLED);
@ -376,6 +386,11 @@ void Tiltrotor::update_transition_state()
// slowly ramp up throttle to avoid step inputs // slowly ramp up throttle to avoid step inputs
_mc_throttle_weight = (time_since_trans_start - 1.0f) / 1.0f; _mc_throttle_weight = (time_since_trans_start - 1.0f) / 1.0f;
} }
// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
if (!_v_control_mode->flag_control_climb_rate_enabled) {
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}
} }
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));