diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 9a36343d1b..2a64593ef0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -358,7 +358,7 @@ MulticopterAttitudeControl::control_attitude() { _v_att_sp_sub.update(&_v_att_sp); - // reinitialize the setpoint while not armed to make sure no value from the last flight is still kept + // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept if (!_v_control_mode.flag_armed) { Quatf().copyTo(_v_att_sp.q_d); Vector3f().copyTo(_v_att_sp.thrust_body); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 7aae48165e..618b6ebb53 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -48,6 +48,9 @@ */ #include "vtol_att_control_main.h" #include +#include + +using namespace matrix; namespace VTOL_att_control { @@ -512,6 +515,14 @@ void VtolAttitudeControl::task_main() _vtol_type->fill_actuator_outputs(); } + // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept + if (!_v_control_mode.flag_armed) { + Quatf().copyTo(_mc_virtual_att_sp.q_d); + Vector3f().copyTo(_mc_virtual_att_sp.thrust_body); + Quatf().copyTo(_v_att_sp.q_d); + Vector3f().copyTo(_v_att_sp.thrust_body); + } + /* Only publish if the proper mode(s) are enabled */ if (_v_control_mode.flag_control_attitude_enabled || _v_control_mode.flag_control_rates_enabled ||