From 7f1bb556e943b04c888bbdcbee361f19c8f1c71a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 25 Apr 2022 22:37:25 -0400
Subject: [PATCH] Update
 src/modules/mc_pos_control/MulticopterPositionControl.cpp

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
---
 src/modules/mc_pos_control/MulticopterPositionControl.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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);
 			}
 		}