|
|
|
@ -264,6 +264,9 @@ void Standard::update_transition_state()
@@ -264,6 +264,9 @@ 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; |
|
|
|
|
|
|
|
|
|
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); |
|
|
|
|
|
|
|
|
@ -279,6 +282,7 @@ void Standard::update_transition_state()
@@ -279,6 +282,7 @@ void Standard::update_transition_state()
|
|
|
|
|
|
|
|
|
|
// maintain FW_PSP_OFF
|
|
|
|
|
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; |
|
|
|
|
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_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); |
|
|
|
|
|
|
|
|
|