Diff: FreeIMU.cpp

Revision:

6:6b1185b32814

Parent:

3:f9b100a9aa65

Child:

8:cd43764b9623

--- 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