From 1c776f16ec8321245e4fa8afd6ffd5003a3ef0bb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 22 May 2019 12:44:47 +0100 Subject: [PATCH] mc_att_control: fix applying not updated thrust setpoint --- src/modules/mc_att_control/mc_att_control_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) 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 b12bdaed6e..295ae87296 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -521,6 +521,12 @@ MulticopterAttitudeControl::control_attitude() { vehicle_attitude_setpoint_poll(); + // reinitialize the setpoint while not armed to make sure no value from the last flight is still kept + if (!_v_control_mode.flag_armed) { + Quatf().copyTo(_v_att_sp.q_d); + Vector3f().copyTo(_v_att_sp.thrust_body); + } + // physical thrust axis is the negative of body z axis _thrust_sp = -_v_att_sp.thrust_body[2];