Joseph Roberts
/
FreeIMU_external_magnetometer
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: HMC58X31 MODI2C MPU6050 MS561101BA
Fork of
FreeIMU
by 
Yifei Teng
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
--- 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