Diff: SensorFusion.cpp

Revision:

26:8f3e4e1a3acc

Parent:

25:abb0f208e6a9

Child:

31:d65576185bdf

--- a/SensorFusion.cpp	Fri Apr 17 12:08:00 2015 +0000
+++ b/SensorFusion.cpp	Tue Apr 21 12:27:24 2015 +0000
@@ -50,20 +50,6 @@
 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);
-}
-
 void SensorFusion::sensorUpdate(Vector3 gyro_degrees)
 {
     Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
@@ -74,7 +60,7 @@
                    gyro_reading.x,    gyro_reading.y,    gyro_reading.z,
                    magneto_reading.x, magneto_reading.y, magneto_reading.z);
 
-    Vector3 const fused = eulerAngles(q);
+    Vector3 const fused(q.getEulerAngles());
 
     delegate->sensorTick(fused, accel_reading, magneto_reading, gyro_degrees, q);
 }