Fixed euler wrapping.

9 years ago · 1778ac199c
2 changed files with 2 additions and 4 deletions
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@ -61,8 +61,6 @@ public:
				@@ -61,8 +61,6 @@ public:
            psi_val = Type(atan2(-dcm(1,2), -dcm(0,2)));
        }

-        if (psi_val < 0) psi_val += 2*pi;
-
        phi() = phi_val;
        theta() = theta_val;
        psi() = psi_val;
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@ -104,8 +104,8 @@ int main()
				@@ -104,8 +104,8 @@ int main()
                    roll_expected = 0;
                    yaw_expected = yaw + roll;
                }
-                if (yaw_expected < 0) yaw_expected += 360;
-                if (yaw_expected > 360) yaw_expected -= 360;
+                if (yaw_expected < -180) yaw_expected += 360;
+                if (yaw_expected > 180) yaw_expected -= 360;

                printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
                Euler<double> euler_expected(