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

vtol: take fixed wing attitude setpoint during transition if altitude is

Browse Source 
not controlled

- required as there is no flightask running if altitude is not controlled

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2021-04-26 14:33:17 +03:00 committed by Silvan Fuhrer
parent 435e5515df
commit 8dd76050e0
2 changed files with 21 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -230,8 +230,16 @@ void Standard::update_transition_state()
VtolType::update_transition_state();
// copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
if (_v_control_mode->flag_control_climb_rate_enabled) {
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
} else {
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
if (_params_standard.pusher_ramp_dt <= 0.0f) {
@ -263,13 +271,6 @@ void Standard::update_transition_state()
// ramp up FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
_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));
q_sp.copyTo(_v_att_sp->q_d);
@ -283,18 +284,11 @@ void Standard::update_transition_state()
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
if (_v_control_mode->flag_control_climb_rate_enabled) {
// control backtransition deceleration using pitch.
_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));
q_sp.copyTo(_v_att_sp->q_d);

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

@ -289,10 +289,18 @@ void Tiltrotor::update_transition_state()
{
VtolType::update_transition_state();
// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
if (_v_control_mode->flag_control_climb_rate_enabled) {
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
} else {
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
_thrust_transition = _fw_virtual_att_sp->thrust_body[0];
}
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
@ -339,13 +347,6 @@ void Tiltrotor::update_transition_state()
_mc_yaw_weight = _mc_roll_weight;
}
_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) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
_tilt_control = _params_tiltrotor.tilt_transition +
@ -359,22 +360,12 @@ void Tiltrotor::update_transition_state()
int ramp_down_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) *
(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
_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) {
// turn on all MC motors
set_all_motor_state(motor_state::ENABLED);
// set idle speed for rotary wing mode
if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc();
@ -405,11 +396,6 @@ void Tiltrotor::update_transition_state()
// slowly ramp up throttle to avoid step inputs
_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));