From 5aef810e7c8771f633a8425693fb069eb27f27fc Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Sun, 24 Jan 2016 01:50:34 -0500
Subject: [PATCH] Fixed euler angles.

---
 matrix/Euler.hpp  | 28 ++++++++++++++++------------
 test/attitude.cpp | 45 ++++++++++++++++++++++++++++++++-------------
 2 files changed, 48 insertions(+), 25 deletions(-)

diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp
index 89a188eab4..23c970b375 100644
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@@ -48,20 +48,24 @@ public:
 
     Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
     {
-        theta() = (Type)asin(-dcm(2,0));
+        Type phi_val = Type(atan2(dcm(2,1), dcm(2,2)));
+        Type theta_val = Type(asin(-dcm(2,0)));
+        Type psi_val = Type(atan2(dcm(1,0), dcm(0,0)));
+        Type pi = Type(M_PI);
 
-        if (fabs(theta() - (Type)M_PI_2) < 1.0e-3) {
-            phi() = (Type)0.0;
-            psi() = (Type)atan2(dcm(0,1), dcm(1,1));
-            psi() = (Type)atan2(dcm(1,2), dcm(0,2));
-        } else if ((Type)fabs(theta() + (Type)M_PI_2) < (Type)1.0e-3) {
-            phi() = (Type)0.0;
-            psi() = (Type)atan2(-dcm(1,2), -dcm(0,2));
-
-        } else {
-            phi() = (Type)atan2(dcm(2,1), dcm(2,2));
-            psi() = (Type)atan2(dcm(1,0), dcm(0,0));
+        if (fabs(theta_val - pi/2) < 1.0e-3) {
+            phi_val = Type(0.0);
+            psi_val = Type(atan2(dcm(1,2), dcm(0,2)));
+        } else if (Type(fabs(theta_val + pi/2) < Type(1.0e-3))) {
+            phi_val = Type(0.0);
+            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;
     }
 
     Euler(const Quaternion<Type> & q) :
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 71d26b86a1..6206eb1713 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -91,27 +91,46 @@ int main()
     double rad2deg = 180.0/M_PI;
 
     // euler dcm round trip check
-    for (int roll=-90; roll<90; roll+=1) {
-        for (int pitch=-90; pitch<90; pitch+=1) {
-            for (int yaw=0; yaw<360; yaw+=1) {
+    for (int roll=-90; roll<=90; roll+=90) {
+        for (int pitch=-90; pitch<=90; pitch+=90) {
+            for (int yaw=1; yaw<=360; yaw+=90) {
                 // note if theta = pi/2, then roll is set to zero
-                if (pitch == 90 || pitch == -90) {
-                    roll = 0;
+                int roll_expected = roll;
+                int yaw_expected = yaw;
+                if (pitch == 90) {
+                    roll_expected = 0;
+                    yaw_expected = yaw - roll;
+                } else if (pitch == -90) {
+                    roll_expected = 0;
+                    yaw_expected = yaw + roll;
                 }
-                printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
-                Euler<double> euler(deg2rad*double(roll),
-                                    deg2rad*double(pitch),
-                                    deg2rad*double(yaw));
-                Dcm<double> dcm_from_euler(euler);
-                Euler<double> euler_out(dcm_from_euler);
-                TEST(isEqual(rad2deg*euler, rad2deg*euler_out));
+                if (yaw_expected < 0) yaw_expected += 360;
+                if (yaw_expected > 360) yaw_expected -= 360;
 
+                printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
+                Euler<double> euler_expected(
+                    deg2rad*double(roll_expected),
+                    deg2rad*double(pitch),
+                    deg2rad*double(yaw_expected));
+                Euler<double> euler(
+                    deg2rad*double(roll),
+                    deg2rad*double(pitch),
+                    deg2rad*double(yaw));
+                Dcm<double> dcm_from_euler(euler);
+                dcm_from_euler.print();
+                Euler<double> euler_out(dcm_from_euler);
+                TEST(isEqual(rad2deg*euler_expected, rad2deg*euler_out));
+
+                Eulerf eulerf_expected(
+                    float(deg2rad)*float(roll_expected),
+                    float(deg2rad)*float(pitch),
+                    float(deg2rad)*float(yaw_expected));
                 Eulerf eulerf(float(deg2rad)*float(roll),
                               float(deg2rad)*float(pitch),
                               float(deg2rad)*float(yaw));
                 Dcm<float> dcm_from_eulerf(eulerf);
                 Euler<float> euler_outf(dcm_from_eulerf);
-                TEST(isEqual(float(rad2deg)*eulerf,
+                TEST(isEqual(float(rad2deg)*eulerf_expected,
                              float(rad2deg)*euler_outf));
             }
         }