From f887ad6ebf2bda262c1eb9b9c75c891c49c7fc46 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 2 Feb 2023 16:57:07 +0100 Subject: [PATCH] mc_att_control: allow commanding a yaw rate with zero throttle Still avoiding to build up absolute yaw error in that case. --- src/modules/mc_att_control/mc_att_control_main.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) 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 3e96ab97a3..29ee08248c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -112,12 +112,11 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, vehicle_attitude_setpoint_s attitude_setpoint{}; const float yaw = Eulerf(q).psi(); - // Avoid giving yaw rate setpoint with arming stick gesture - if ((_manual_control_setpoint.throttle > -.9f) || (_param_mc_airmode.get() == 2)) { - attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_mpc_man_y_max.get()); + attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_mpc_man_y_max.get()); - } else { - attitude_setpoint.yaw_sp_move_rate = 0.f; + // Avoid accumulating absolute yaw error with arming stick gesture in case heading_good_for_control stays true + if ((_manual_control_setpoint.throttle < -.9f) && (_param_mc_airmode.get() != 2)) { + reset_yaw_sp = true; } // Make sure not absolute heading error builds up