Joseph Roberts / FreeIMU

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HMC58X3 MODI2C MPU6050 MS561101BA

Fork of FreeIMU by Yifei Teng

Diff: FreeIMU.cpp

Revision:
6:6b1185b32814
Parent:
3:f9b100a9aa65
Child:
8:cd43764b9623
diff -r 773bd4e605eb -r 6b1185b32814 FreeIMU.cpp
--- a/FreeIMU.cpp	Sat Nov 09 09:00:22 2013 +0000
+++ b/FreeIMU.cpp	Mon Dec 23 08:35:22 2013 +0000
@@ -54,6 +54,10 @@
     ezInt = 0.0;
     twoKp = twoKpDef;
     twoKi = twoKiDef;
+    
+    twoKiz = twoKiDef / 6.0;
+    twoKpz = twoKpDef * 8.0;
+    
     integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;
 
     update.start();
@@ -375,7 +379,7 @@
         if(twoKi > 0.0f) {
             integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
             integralFBy += twoKi * halfey * (1.0f / sampleFreq);
-            integralFBz += twoKi * halfez * (1.0f / sampleFreq);
+            integralFBz += twoKiz * halfez * (1.0f / sampleFreq);
             gx += integralFBx;  // apply integral feedback
             gy += integralFBy;
             gz += integralFBz;
@@ -388,7 +392,7 @@
         // Apply proportional feedback
         gx += twoKp * halfex;
         gy += twoKp * halfey;
-        gz += twoKp * halfez;
+        gz += twoKpz * halfez;
     }
 
     // Integrate rate of change of quaternion