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

Revision:
18:3f4803a943d3
Parent:
13:21b275eeeda2
Child:
20:a2edd3ced80e
--- a/FreeIMU.h	Wed Mar 07 04:13:28 2018 +0000
+++ b/FreeIMU.h	Wed Mar 28 20:26:02 2018 +0000
@@ -24,8 +24,10 @@
 #ifndef FreeIMU_h
 #define FreeIMU_h
 
+#ifndef I2C_SDA
 #define I2C_SDA p28
 #define I2C_SCL p27
+#endif
 
 // Uncomment the appropriated version of FreeIMU you are using
 //#define FREEIMU_v01
@@ -42,7 +44,8 @@
 //#define SEN_10724 //9 Degrees of Freedom - Sensor Stick SEN-10724 http://www.sparkfun.com/products/10724
 //#define SEN_10183 //9 Degrees of Freedom - Sensor Stick  SEN-10183 http://www.sparkfun.com/products/10183
 //#define ARDUIMU_v3 //  DIYDrones ArduIMU+ V3 http://store.diydrones.com/ArduIMU_V3_p/kt-arduimu-30.htm or https://www.sparkfun.com/products/11055
-#define GEN_MPU6050 // Generic MPU6050 breakout board. Compatible with GY-521, SEN-11028 and other MPU6050 wich have the MPU6050 AD0 pin connected to GND.
+//#define GEN_MPU6050 // Generic MPU6050 breakout board. Compatible with GY-521, SEN-11028 and other MPU6050 wich have the MPU6050 AD0 pin connected to GND.
+#define GEN_MPU9250
 
 // *** No configuration needed below this line ***
 
@@ -66,6 +69,7 @@
 
 
 #define HAS_MPU6050() (defined(FREEIMU_v04) || defined(GEN_MPU6050))
+#define HAS_MPU9250() (defined(FREEIMU_v04) || defined(GEN_MPU9250))
 
 #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
 #define HAS_AXIS_ALIGNED() (defined(FREEIMU_v01) || defined(FREEIMU_v02) || defined(FREEIMU_v03) || defined(FREEIMU_v035) || defined(FREEIMU_v035_MS) || defined(FREEIMU_v035_BMP) || defined(FREEIMU_v04) || defined(SEN_10121) || defined(SEN_10736))
@@ -91,10 +95,17 @@
   #include "I2Cdev.h"
   #include "MPU6050.h"
   #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS
-
+  #include <HMC58X3.h>
 #endif
 
-#include <HMC58X3.h>
+#if HAS_MPU9250()
+ // #include <Wire.h>
+  #include "I2Cdev.h"
+  #include "MPU6050.h"
+  #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS
+#endif
+
+
 #include <MS561101BA.h>
 
 
@@ -105,9 +116,130 @@
 #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND
 // HMC5843 address is fixed so don't bother to define it
 
-
-#define twoKpDef  (2.0f * 3.0f) // 2 * proportional gain
-#define twoKiDef  (2.0f * 0.4f) // 2 * integral gain
+#if defined(DFROBOT) 
+    #define twoKpDef  (2.0f * 0.5f)
+    #define twoKiDef  (2.0f * 0.00002f)
+    #define betaDef  0.1f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif defined(FREEIMU_v04)
+    #define twoKpDef  (2.0f * 0.75f)    //works with and without mag enabled
+    #define twoKiDef  (2.0f * 0.1625f)
+    #define betaDef  0.085f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif defined(GEN_MPU6050)
+    #define twoKpDef  (2.0f * 0.5f)
+    #define twoKiDef  (2.0f * 0.25f)
+    #define betaDef   0.2f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif defined(GEN_MPU9150) 
+    #define twoKpDef  (2.0f * 0.75f)
+    #define twoKiDef  (2.0f * 0.1f) 
+    #define betaDef   0.01f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif defined(Altimu10)
+    //#define twoKpDef  (2.0f * 1.01f)
+    //#define twoKiDef  (2.0f * 0.00002f)   
+    #define twoKpDef  (2.0f * 2.75f)
+    #define twoKiDef  (2.0f * 0.1625f)
+    #define betaDef  2.0f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif defined(GEN_MPU9250) || defined(MPU9250_5611) || defined(MPU9250_5637)
+    #define twoKpDef  (2.0f * 1.75f) // was 0.95
+    #define twoKiDef  (2.0f * 0.05f) // was 0.05    
+    #define betaDef   0.515f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif defined(APM_2_5)
+    #define twoKpDef  (2.0f * 0.5f)
+    #define twoKiDef  (2.0f * 0.25f)
+    #define betaDef   0.015f    //was 0.015
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif defined(Microduino)
+    #define twoKpDef  (2.0f * 1.75f)    //works with and without mag enabled, 1.75
+    #define twoKiDef  (2.0f * 0.0075f)  //.1625f
+    #define betaDef  0.015f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif (defined(ST_LSM9DS0) || defined(LSM9DS0_MS5637))
+    //Madgwick's implementation of Mayhony's AHRS algorithm (option 0)
+    #define twoKpDef  (2.0f * 1.75f)    //works with and without mag enabled
+    #define twoKiDef  (2.0f * 0.025f)
+    //Implementation of Madgwick's IMU and AHRS algorithms (option 1)
+    #define betaDef  0.15f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423, was 1.2(2/16/17)
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.2f;   // was 1.2 and 0.02, was 1.2 (2/16/17)
+    const float Ki_YAW = 0.02f;
+#elif defined(ST_LSM9DS1) || defined(ST_LSM9DS1_MS5611)
+    //Madgwick's implementation of Mayhony's AHRS algorithm
+    #define twoKpDef  (2.0f * 1.75f)    //works with and without mag enabled
+    #define twoKiDef  (2.0f * 0.025f)
+    //Implementation of Madgwick's IMU and AHRS algorithms
+    #define betaDef  0.15f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.2f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.02f;
+#elif defined(CurieImu) || defined(CurieIMU_Mag)
+    #define twoKpDef  (2.0f * 1.25f)
+    #define twoKiDef  (2.0f * 0.1f)
+    #define betaDef   0.07f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#elif defined(PropShield)
+    #define twoKpDef  (2.0f * 4.25f) // was 0.95, 3.25
+    #define twoKiDef  (2.0f * 0.25) // was 0.05 , 0.25
+    #define betaDef   0.35
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#else
+    #define twoKpDef  (2.0f * 0.5f)
+    #define twoKiDef  (2.0f * 0.1f)
+    #define betaDef  0.1f
+    //Used for DCM filter
+    const float Kp_ROLLPITCH = 1.2f;  //was .3423
+    const float Ki_ROLLPITCH = 0.0234f;
+    const float Kp_YAW = 1.75f;   // was 1.2 and 0.02
+    const float Ki_YAW = 0.002f;
+#endif 
 
 #ifndef cbi
 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
@@ -147,7 +279,10 @@
 
       MPU6050 *accgyro;
       MS561101BA *baro;
+
+#if HAS_MPU6050()
       HMC58X3 *magn;
+#endif
     
     int* raw_acc, raw_gyro, raw_magn;
     // calibration parameters