diff --git a/src/QGC.cc b/src/QGC.cc
index 812099ab6a0e0ffd2a76bfa96f2c758d9b787159..dcfa7e2d52a6064334d6bd2938a047b46889de4e 100644
--- a/src/QGC.cc
+++ b/src/QGC.cc
@@ -61,14 +61,14 @@ double limitAngleToPMPId(double angle)
         {
             while (angle < -M_PI)
             {
-                angle += M_PI;
+                angle += 2.0f * M_PI;
             }
         }
         else if (angle > M_PI)
         {
             while (angle > M_PI)
             {
-                angle -= M_PI;
+                angle -= 2.0f * M_PI;
             }
         }
     }