Diff: AHRS.cpp

Revision:

7:3e74239e3778

Parent:

6:5665d427bceb

Child:

8:928673148b55

--- a/AHRS.cpp	Wed Jan 20 02:34:07 2016 +0000
+++ b/AHRS.cpp	Wed Jan 20 02:36:32 2016 +0000
@@ -329,25 +329,6 @@
     q[2] = q3 * norm;
     q[3] = q4 * norm;
 
-    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-
-    float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
-    float Yh = my*cos(roll)+mz*sin(roll);
-
-    float yawmag = atan2(Yh,Xh)+PI;
-    //////test.printf("Xh= %f Yh= %f ",Xh,Yh);
-    //////test.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
-    //test.printf(",%f",yawmag*180.0f/PI);
-
-
-
-    pitch *= 180.0f / PI;
-    yaw   *= 180.0f / PI;
-    yaw   += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
-    roll  *= 180.0f / PI;
-
 }
 
 
@@ -439,24 +420,6 @@
     q[2] = q3 * norm;
     q[3] = q4 * norm;
 
-    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-
-    Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
-    Yh = my*cos(roll)+mz*sin(roll);
-
-    yawmag = atan2(Yh,Xh)+PI;
-    ////////////test.printf("Xh= %f Yh= %f ",Xh,Yh);
-    //////test.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
-    //test.printf(",%f",yawmag*180.0f/PI);
-
-
-
-    pitch *= 180.0f / PI;
-    yaw   *= 180.0f / PI;
-    yaw   += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
-    roll  *= 180.0f / PI;
 }
 
 void AHRS::TimeStart()