Revision 2:5c419926dcd7, committed 2013-11-05

Comitter:

tyftyftyf

Date:

Tue Nov 05 13:38:07 2013 +0000

Parent:

1:794e9cdbc2a0

Child:

3:f9b100a9aa65

Commit message:

Sensor fusion algorithm cold start

Changed in this revision

--- a/FreeIMU.cpp	Tue Nov 05 11:31:39 2013 +0000
+++ b/FreeIMU.cpp	Tue Nov 05 13:38:07 2013 +0000
@@ -150,9 +150,9 @@
   
   float xh = values[6]*cos(pitch)+values[7]*sin(roll)*sin(pitch)-values[8]*cos(roll)*sin(pitch);
   float yh = values[7]*cos(roll)+values[8]*sin(roll);
-  float yaw = atan2(yh, xh);
+  float yaw = -atan2(yh, xh);
   
-  float rollOver2 = roll * 0.5f;
+  float rollOver2 = (roll + M_PI) * 0.5f;
   float sinRollOver2 = (float)sin(rollOver2);
   float cosRollOver2 = (float)cos(rollOver2);
   float pitchOver2 = pitch * 0.5f;
@@ -162,10 +162,10 @@
   float sinYawOver2 = (float)sin(yawOver2);
   float cosYawOver2 = (float)cos(yawOver2);
 
-  q0 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
-  q1 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
-  q2 = - cosYawOver2 * cosPitchOver2 * cosRollOver2 - sinYawOver2 * sinPitchOver2 * sinRollOver2;
-  q3 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
+  q0 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
+  q1 = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
+  q2 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
+  q3 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
   
   if (q!=NULL){
           q[0] = q0;

--- a/FreeIMU.h	Tue Nov 05 11:31:39 2013 +0000
+++ b/FreeIMU.h	Tue Nov 05 13:38:07 2013 +0000
@@ -136,6 +136,8 @@
     float getBaroAlt();
     float getBaroAlt(float sea_press);
     
+    void getQ_simple(float* q);
+    
     // we make them public so that users can interact directly with device classes
 
       MPU6050 accgyro;
@@ -163,8 +165,7 @@
     int dt_us;
     //unsigned long lastUpdate, now; // sample period expressed in milliseconds
     float sampleFreq; // half the sample period expressed in seconds
-    
-    void getQ_simple(float* q);
+
 };
 
 float invSqrt(float number);