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