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);
}

Diff: FreeIMU.cpp

Revision:
27:d1042e848b07
Parent:
24:dc802ce22e75
Child:
28:de24fce0509a
--- a/FreeIMU.cpp	Wed Mar 28 22:17:32 2018 +0000
+++ b/FreeIMU.cpp	Thu Mar 29 22:07:11 2018 +0000
@@ -120,9 +120,6 @@
     accgyro->initialize();
     accgyro->setI2CMasterModeEnabled(0);
     accgyro->setI2CBypassEnabled(1);
-    accgyro->setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
-    accgyro->setDLPFMode(0);
-    accgyro->setRate(0);
     Thread::wait(20);
     
 #if HAS_MPU9250()
@@ -131,6 +128,12 @@
     Thread::wait(50);
 #endif
 
+    accgyro->setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
+    // both accelerometer and gyroscope will output at 1khz
+    accgyro->setDLPFMode(1);
+    accgyro->setRate(0);
+    Thread::wait(20);
+
     accgyro->start_sampling();
 
     Thread::wait(20);
@@ -303,7 +306,8 @@
 #endif
 
 #if HAS_MPU9250()
-const bool magn_valid = true;
+// need to automatically turn this to true at 100Hz. This is done within the MPU9250 lib.
+extern volatile bool magn_valid_9250;
 #endif
 
 
@@ -337,7 +341,7 @@
     q3q3 = q3 * q3;
 
     // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
-    if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f) && _magn_valid) {
+    if ((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f) && _magn_valid) {
         float hx, hy, bx, bz;
         float halfwx, halfwy, halfwz;
 
@@ -365,11 +369,13 @@
 
 #if HAS_MPU6050()
         magn_valid = false;
+#elif HAS_MPU9250()
+        magn_valid_9250 = false;
 #endif
     }
 
     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-    if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
+    if ((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
         float halfvx, halfvy, halfvz;
 
         // Normalise accelerometer measurement
@@ -390,7 +396,7 @@
     }
 
     // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
-    if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
+    if (halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
         // Compute and apply integral feedback if enabled
         if(twoKi > 0.0f) {
             integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
@@ -446,10 +452,16 @@
     dt_us=update.read_us();
     sampleFreq = 1.0 / ((dt_us) / 1000000.0);
     update.reset();
-// lastUpdate = now;
+    // lastUpdate = now;
     // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
 
-    AHRSupdate(val[3] * M_PI/180.0, val[4] * M_PI/180.0, val[5] * M_PI/180.0, val[0], val[1], val[2], val[6], val[7], val[8], magn_valid);
+    bool _magn_valid;
+#if HAS_MPU6050()
+    _magn_valid = magn_valid;
+#elif HAS_MPU9250()
+    _magn_valid = magn_valid_9250;
+#endif
+    AHRSupdate(val[3] * M_PI/180.0, val[4] * M_PI/180.0, val[5] * M_PI/180.0, val[0], val[1], val[2], val[6], val[7], val[8], _magn_valid);
 
     if (q!=NULL) {
         q[0] = q0;