diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index 228d8c31baf62e8f446bb07f5b05278eea413f68..144783941a27448357325f6d9e5965f68083871b 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -133,6 +133,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
     request.ownCrazyflie.y -= setpoint[1];
     request.ownCrazyflie.z -= setpoint[2];
     float yaw = request.ownCrazyflie.yaw - setpoint[3];
+    while(yaw > PI) {yaw -= 2 * PI;}
+    while(yaw < -PI) {yaw += 2 * PI;}
+    request.ownCrazyflie.yaw = yaw;
 
     float est[9];
 
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index efe421bf435b3d57ce73e0a20028227df4cc41f1..4b712ef3cfd01c8209622e08e443b4a93b8686dc 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -174,6 +174,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
     request.ownCrazyflie.y -= setpoint[1];
     request.ownCrazyflie.z -= setpoint[2];
     float yaw = request.ownCrazyflie.yaw - setpoint[3];
+
 	
     //bag.write("Offset", ros::Time::now(), request.ownCrazyflie);