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 f04ae65be1..bc47083cfb 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -375,6 +375,8 @@ private:
 
 	void vel_sp_slewrate();
 
+	void wrap_yaw_speed(float yaw_speed);
+
 	void update_velocity_derivative();
 
 	void do_control();
@@ -1618,18 +1620,8 @@ MulticopterPositionControl::control_offboard()
 			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
 
 		} else if (_pos_sp_triplet.current.yawspeed_valid) {
-			float yaw_target = wrap_pi(_att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * _dt);
-			float yaw_offs = wrap_pi(yaw_target - _yaw);
-			const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max;
-			const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get();
 
-			// If the yaw offset became too big for the system to track stop
-			// shifting it, only allow if it would make the offset smaller again.
-			if (fabsf(yaw_offs) < yaw_offset_max ||
-			    (_pos_sp_triplet.current.yawspeed > 0 && yaw_offs < 0) ||
-			    (_pos_sp_triplet.current.yawspeed < 0 && yaw_offs > 0)) {
-				_att_sp.yaw_body = yaw_target;
-			}
+			wrap_yaw_speed(_pos_sp_triplet.current.yawspeed);
 		}
 
 	} else {
@@ -1860,21 +1852,8 @@ void MulticopterPositionControl::control_auto()
 				yaw_speed = _traj_wp_avoidance.point_0[trajectory_waypoint_s::YAW_SPEED];
 			}
 
-			/* we want to know the real constraint, and global overrides manual */
-			const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
-						   _params.global_yaw_max;
-			const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
+			wrap_yaw_speed(yaw_speed);
 
-			float yaw_target = _wrap_pi(_att_sp.yaw_body + yaw_speed * _dt);
-			float yaw_offs = _wrap_pi(yaw_target - _yaw);
-
-			// If the yaw offset became too big for the system to track stop
-			// 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;
-			}
 
 		} else {
 
@@ -2326,6 +2305,25 @@ void MulticopterPositionControl::control_auto()
 	}
 }
 
+void
+MulticopterPositionControl::wrap_yaw_speed(float yaw_speed)
+{
+	/* we want to know the real constraint, and global overrides manual */
+	const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max;
+	const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get();
+
+	float yaw_target = _wrap_pi(_att_sp.yaw_body + yaw_speed * _dt);
+	float yaw_offs = _wrap_pi(yaw_target - _yaw);
+
+	// If the yaw offset became too big for the system to track stop
+	// shifting it, only allow if it would make the offset smaller again.
+	if (fabsf(yaw_offs) < yaw_offset_max ||
+	    (yaw_speed > 0 && yaw_offs < 0) ||
+	    (yaw_speed < 0 && yaw_offs > 0)) {
+		_att_sp.yaw_body = yaw_target;
+	}
+}
+
 void
 MulticopterPositionControl::update_velocity_derivative()
 {
@@ -2869,7 +2867,6 @@ MulticopterPositionControl::generate_attitude_setpoint()
 
 		/* we want to know the real constraint, and global overrides manual */
 		const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max;
-		const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get();
 
 		_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
 
@@ -2878,16 +2875,7 @@ MulticopterPositionControl::generate_attitude_setpoint()
 			_att_sp.yaw_sp_move_rate = _traj_wp_avoidance.point_0[trajectory_waypoint_s::YAW_SPEED];
 		}
 
-		float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * _dt);
-		float yaw_offs = _wrap_pi(yaw_target - _yaw);
-
-		// If the yaw offset became too big for the system to track stop
-		// 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;
-		}
+		wrap_yaw_speed(_att_sp.yaw_sp_move_rate);
 	}
 
 	/* control throttle directly if no climb rate controller is active */