Diff: SensorFusion.cpp

Revision:

17:e9d42864c8a1

Parent:

16:3e2468d4f4c1

Child:

19:9e9753b87cfe

--- a/SensorFusion.cpp	Fri Mar 20 10:30:01 2015 +0000
+++ b/SensorFusion.cpp	Thu Mar 26 15:43:49 2015 +0000
@@ -43,6 +43,20 @@
 static float const deg_to_radian =  0.0174532925f;
 static float const radian_to_deg = 57.2957795131f;
 
+        
+Vector3 SensorFusion::eulerAngles(Quaternion const &q) const {
+    float const q0 = q.w;
+    float const q1 = q.v.x;
+    float const q2 = q.v.y;
+    float const q3 = q.v.z;
+    
+    float const roll = asin(2*(q0*q2-q3*q1));
+    float const pitch = atan2(2*(q0*q1+q2*q3), 1 - 2*(q1*q1+q2*q2));
+    float const yaw = atan2(2*(q0*q3+q1*q2), 1 - 2*(q2*q2+q3*q3));
+    
+    return Vector3(pitch, roll, yaw) * radian_to_deg;
+}
+
 void SensorFusion::sensorUpdate(Vector3 gyro_degrees)
 {
     Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
@@ -53,7 +67,7 @@
                    gyro_reading.x,    gyro_reading.y,    gyro_reading.z,
                    magneto_reading.x, magneto_reading.y, magneto_reading.z);
 
-    Vector3 const fused = q.getEulerAngles() * radian_to_deg;
+    Vector3 const fused = eulerAngles(q);
 
     sensorTick(fused, accel_reading, magneto_reading, gyro_degrees, q);
 }