diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 9bb7b56d49..22fb7daf0a 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1975,10 +1975,10 @@ MulticopterPositionControl::task_main()
 				float yaw_offs = _wrap_pi(yaw_target - _yaw);
 
 				// If the yaw offset became too big for the system to track stop
-				// shifting it
-
-				// XXX this needs inspection - probably requires a clamp, not an if
-				if (fabsf(yaw_offs) < yaw_offset_max) {
+				// shifting it, only allow if it would make the offset smaller again.
+				if (fabsf(yaw_offs) < yaw_offset_max ||
+						(_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) ||
+						(_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) {
 					_att_sp.yaw_body = yaw_target;
 				}
 			}