From 048cca7bc40d3274d9298c96a27f19bcd4304622 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 12 Jun 2019 19:59:29 +0200 Subject: [PATCH] vtol_att_control: apply multicopter takeoff hotfix also for vtol (#12250) Please see reference: https://github.com/PX4/Firmware/issues/12171 --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- .../vtol_att_control/vtol_att_control_main.cpp | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) 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 ||