diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
index dae53eca71..31f74c198d 100644
--- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp
+++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
@@ -371,7 +371,7 @@ void MulticopterPositionControl::Run()
 			}
 
 			if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
-				_setpoint.yaw += vehicle_local_position.delta_heading;
+				_setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
 			}
 		}