diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5422a01898..3681c2b17c 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -172,11 +172,6 @@ void Standard::update_transition_state() VtolType::update_transition_state(); - const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); - float roll_body = attitude_setpoint_euler.phi(); - float pitch_body = attitude_setpoint_euler.theta(); - float yaw_body = attitude_setpoint_euler.psi(); - // we get attitude setpoint from a multirotor flighttask if climbrate 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) { @@ -186,7 +181,6 @@ void Standard::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); } else { // we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active) @@ -198,6 +192,16 @@ void Standard::update_transition_state() _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; } + + const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + + if (_v_control_mode->flag_control_climb_rate_enabled) { + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); + } + if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) { if (_param_vt_psher_slew.get() <= FLT_EPSILON) { // just set the final target throttle value diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 9a05cf1034..7fb52b4688 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -203,11 +203,6 @@ void Tiltrotor::update_transition_state() const hrt_abstime now = hrt_absolute_time(); - const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); - float roll_body = attitude_setpoint_euler.phi(); - float pitch_body = attitude_setpoint_euler.theta(); - float yaw_body = attitude_setpoint_euler.psi(); - // 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) { @@ -217,7 +212,6 @@ void Tiltrotor::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else { @@ -231,6 +225,15 @@ void Tiltrotor::update_transition_state() } + const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + + if (_v_control_mode->flag_control_climb_rate_enabled) { + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); + } + if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { // for the first part of the transition all rotors are enabled