Merge pull request #6064 from patrickelectric/ground_station_angle_correction

QGC: Solve problem with limitAngleToPMPId when angle is bigger than PI
7 years ago · 01c3b6b770
1 changed files with 2 additions and 2 deletions
--- 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;
            }
        }
    }