Yifei Teng / FreeIMU Featured

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 AK8963 MS561101BA MODI2C MPU9250

Dependents:   MTQuadControl FreeIMU_serial FreeIMU_demo

Port of FreeIMU library from Arduino to Mbed

10DOF FreeIMU port for FreeIMU v4 board and GY-86. This library was modified extensively to specifically suit the Mbed platform. Maximum sampling rate of 500hz can be achieved using this library.

Improvements

Sensor fusion algorithm fast initialization

This library implements the ARHS hot start algorithm, meaning that you can get accurate readings seconds after the algorithm is started, much faster than the Arduino version, where outputs slowly converge to the correct value in about a minute.

Caching

Sensors are read at their maximum output rates. Read values are cached hence multiple consecutive queries will not cause multiple reads.

Fully async

Acc & Gyro reads are performed via timer interrupts. Magnetometer and barometer are read by RTOS thread. No interfering with main program logic.

Usage

Declare a global FreeIMU object like the one below. There should only be one FreeIMU instance existing at a time. 

#include "mbed.h"
#include "FreeIMU.h"
FreeIMU imu;

int main(){
    imu.init(true);
}

Then, anywhere in the code, you may call imu.getQ(q) to get the quarternion, where q is an array of 4 floats representing the quarternion structure.

You are recommended to call getQ frequently to keep the filter updated. However, the frequency should not exceed 500hz to avoid redundant calculation. One way to do this is by using the RtosTimer: 

void getIMUdata(void const *);     //method definition

//in main
RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)NULL);
IMUTimer.start(2);     //1 / 2ms = 500hz

//getIMUdata function
void getIMUdata(void const *dummy){
    imu.getQ(NULL);
}

Files at this revision

API Documentation at this revision

Revision 12:2ceb5950ee44, committed 2014-02-10

Comitter:
tyftyftyf
Date:
Mon Feb 10 01:13:34 2014 +0000
Parent:
11:b8f894e7d9d8
Parent:
10:4e8ac6acd336
Child:
13:21b275eeeda2
Commit message:

Changed in this revision

HMC58X3.lib Show annotated file Show diff for this revision Revisions of this file 
--- a/FreeIMU.cpp	Mon Feb 10 01:12:22 2014 +0000
+++ b/FreeIMU.cpp	Mon Feb 10 01:13:34 2014 +0000
@@ -31,16 +31,6 @@
 
 #define     M_PI 3.1415926535897932384626433832795
 
-#ifdef DEBUG
-#define DEBUG_PRINT(x) Serial.println(x)
-#else
-#define DEBUG_PRINT(x)
-#endif
-// #include "WireUtils.h"
-//#include "DebugUtils.h"
-
-//#include "vector_math.h"
-
 FreeIMU::FreeIMU()
 {
 
@@ -55,8 +45,8 @@
     twoKp = twoKpDef;
     twoKi = twoKiDef;
     
-    twoKiz = twoKiDef / 6.0;
-    twoKpz = twoKpDef * 8.0;
+    twoKiz = twoKiDef / 4.0;
+    twoKpz = twoKpDef * 6.0;
     
     integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;
 
@@ -118,12 +108,7 @@
 
     Thread::wait(10);
 
-    printf("AccGyro init start\r\n");
-
-    printf("DeviceId = %d\r\n",accgyro->getDeviceID());
-
     accgyro->initialize();
-    printf("AccGyro setting\r\n");
     accgyro->setI2CMasterModeEnabled(0);
     accgyro->setI2CBypassEnabled(1);
     accgyro->setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
@@ -133,7 +118,6 @@
 
     accgyro->start_sampling();
 
-    printf("AccGyro init fin\r\n");
     Thread::wait(10);
 
     // init HMC5843
@@ -141,25 +125,13 @@
     magn->setGain(0);
     // Calibrate HMC using self test, not recommended to change the gain after calibration.
     magn->calibrate(0, 8); // Use gain 1=default, valid 0-7, 7 not recommended.
-    // Single mode conversion was used in calibration, now set continuous mode
-    //magn.setMode(0);
-
     Thread::wait(30);
-
     magn->setDOR(6);
-
     Thread::wait(30);
-
-    printf("Magn init fin\r\n");
-
     magn->start_sampling();
-
     Thread::wait(30);
-
     baro->init(FIMU_BARO_ADDR);
 
-    printf("Baro init fin\r\n");
-
     // zero gyro
     zeroGyro();
 
@@ -274,7 +246,7 @@
 */
 void FreeIMU::zeroGyro()
 {
-    const int totSamples = 8;
+    const int totSamples = 64;
     int16_t raw[11];
     float tmpOffsets[] = {0,0,0};
 
@@ -283,7 +255,7 @@
         tmpOffsets[0] += raw[3];
         tmpOffsets[1] += raw[4];
         tmpOffsets[2] += raw[5];
-        Thread::wait(2);
+        Thread::wait(3);
     }
 
     gyro_off_x = tmpOffsets[0] / totSamples;
@@ -291,7 +263,7 @@
     gyro_off_z = tmpOffsets[2] / totSamples;
 }
 
-extern bool magn_valid;
+extern volatile bool magn_valid;
 
 /**
  * Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion

--- a/FreeIMU.h	Mon Feb 10 01:12:22 2014 +0000
+++ b/FreeIMU.h	Mon Feb 10 01:13:34 2014 +0000
@@ -106,8 +106,8 @@
 // HMC5843 address is fixed so don't bother to define it
 
 
-#define twoKpDef  (2.0f * 1.0f) // 2 * proportional gain
-#define twoKiDef  (2.0f * 0.6f) // 2 * integral gain
+#define twoKpDef  (2.0f * 3.0f) // 2 * proportional gain
+#define twoKiDef  (2.0f * 0.4f) // 2 * integral gain
 
 #ifndef cbi
 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))