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 e4212ec41d..3e96ab97a3 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -112,15 +112,19 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
 	vehicle_attitude_setpoint_s attitude_setpoint{};
 	const float yaw = Eulerf(q).psi();
 
-	/* reset yaw setpoint to current position if needed */
+	// 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());
+
+	} else {
+		attitude_setpoint.yaw_sp_move_rate = 0.f;
+	}
+
+	// Make sure not absolute heading error builds up
 	if (reset_yaw_sp) {
 		_man_yaw_sp = yaw;
 
-	} else if ((_manual_control_setpoint.throttle > -.9f)
-		   || (_param_mc_airmode.get() == 2)) {
-
-		const float yaw_rate = math::radians(_param_mpc_man_y_max.get());
-		attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate;
+	} else {
 		_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);
 	}