Joseph Roberts / FreeIMU

10DOF FreeIMU port for FreeIMU v4 board and GY-86. This library was modified extensively to specifically suit the Mbed platform. Used threads and interrupts to achieve async mode.

Dependencies:   HMC58X3 MODI2C MPU6050 MS561101BA

Fork of FreeIMU by Yifei Teng

Files at this revision

API Documentation at this revision

Revision 6:6b1185b32814, committed 2013-12-23

Comitter:
tyftyftyf
Date:
Mon Dec 23 08:35:22 2013 +0000
Parent:
4:773bd4e605eb
Child:
7:248255b21c2e
Commit message:
Bug fixes

Changed in this revision

FreeIMU.cpp Show annotated file Show diff for this revision Revisions of this file
FreeIMU.h Show annotated file Show diff for this revision Revisions of this file
MODI2C.lib Show diff for this revision Revisions of this file
MPU6050.lib Show annotated file Show diff for this revision Revisions of this file 
--- 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

--- a/FreeIMU.h	Sat Nov 09 09:00:22 2013 +0000
+++ b/FreeIMU.h	Mon Dec 23 08:35:22 2013 +0000
@@ -106,8 +106,8 @@
 // HMC5843 address is fixed so don't bother to define it
 
 
-#define twoKpDef  (2.0f * 7.5f) // 2 * proportional gain
-#define twoKiDef  (2.0f * 0.25f) // 2 * integral gain
+#define twoKpDef  (2.0f * 1.0f) // 2 * proportional gain
+#define twoKiDef  (2.0f * 0.6f) // 2 * integral gain
 
 #ifndef cbi
 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
@@ -162,6 +162,7 @@
     float exInt, eyInt, ezInt;  // scaled integral error
     volatile float twoKp;      // 2 * proportional gain (Kp)
     volatile float twoKi;      // 2 * integral gain (Ki)
+    volatile float twoKiz, twoKpz;
     volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
     volatile float integralFBx,  integralFBy, integralFBz;
     Timer update;

--- a/MODI2C.lib	Sat Nov 09 09:00:22 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/tyftyftyf/code/MODI2C/#eed116eb680a

--- a/MPU6050.lib	Sat Nov 09 09:00:22 2013 +0000
+++ b/MPU6050.lib	Mon Dec 23 08:35:22 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/tyftyftyf/code/MPU6050/#d879deb55ae1
+http://mbed.org/users/tyftyftyf/code/MPU6050/#11cc1b413f49