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 18:3f4803a943d3, committed 2018-03-28

Comitter:
tyftyftyf
Date:
Wed Mar 28 20:26:02 2018 +0000
Parent:
17:faefcafc8822
Child:
19:de9da9b46a5e
Commit message:
wip

Changed in this revision

AK8963.lib Show annotated file Show diff for this revision Revisions of this file
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
HMC58X3.lib Show annotated file Show diff for this revision Revisions of this file
MPU6050.lib Show diff for this revision Revisions of this file
MPU9250.lib Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AK8963.lib	Wed Mar 28 20:26:02 2018 +0000
@@ -0,0 +1,1 @@
+AK8963#ec59a31e784b

--- a/FreeIMU.cpp	Wed Mar 07 04:13:28 2018 +0000
+++ b/FreeIMU.cpp	Wed Mar 28 20:26:02 2018 +0000
@@ -25,8 +25,9 @@
 //#include <inttypes.h>
 //#include <stdint.h>
 //#define DEBUG
-#include "MODI2C.h"
+
 #include "FreeIMU.h"
+#include "mbed.h"
 #include "rtos.h"
 
 #define     M_PI 3.1415926535897932384626433832795
@@ -109,7 +110,10 @@
     accgyro = new MPU6050();
     Thread::wait(10);
     baro = new MS561101BA();
+
+#if HAS_MPU6050()
     magn = new HMC58X3();
+#endif
 
     Thread::wait(10);
 
@@ -123,8 +127,9 @@
 
     accgyro->start_sampling();
 
-    Thread::wait(10);
+    Thread::wait(20);
 
+#if HAS_MPU6050()
     // init HMC5843
     magn->init(false); // Don't set mode yet, we'll do that later on.
     magn->setGain(0);
@@ -134,6 +139,8 @@
     magn->setDOR(6);
     Thread::wait(30);
     magn->start_sampling();
+#endif
+    
     Thread::wait(30);
     baro->init(FIMU_BARO_ADDR);
 
@@ -193,8 +200,14 @@
 void FreeIMU::getRawValues(int16_t * raw_values)
 {
 
+#if HAS_MPU6050()
     accgyro->getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
     magn->getValues(&raw_values[6], &raw_values[7], &raw_values[8]);
+#endif
+
+#if HAS_MPU9250()
+    accgyro->getMotion9(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5], &raw_values[6], &raw_values[7], &raw_values[8]);
+#endif
 
     int temp, press;
     //TODO: possible loss of precision
@@ -210,17 +223,27 @@
 */
 void FreeIMU::getValues(float * values)
 {
-
-// MPU6050
+#if HAS_MPU6050()
+    // MPU6050
     int16_t accgyroval[6];
     accgyro->getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
+#endif
+
+#if HAS_MPU9250()
+    // MPU6250
+    int16_t accgyroval[9];
+    accgyro->getMotion9(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5], &accgyroval[6], &accgyroval[7], &accgyroval[8]);
+    values[6] = accgyroval[6];
+    values[7] = accgyroval[7];
+    values[8] = accgyroval[8];
+#endif
 
     // remove offsets from the gyroscope
     accgyroval[3] = accgyroval[3] - gyro_off_x;
     accgyroval[4] = accgyroval[4] - gyro_off_y;
     accgyroval[5] = accgyroval[5] - gyro_off_z;
 
-    for(int i = 0; i<6; i++) {
+    for (int i = 0; i < 6; i++) {
         if(i < 3) {
             values[i] = (float) accgyroval[i];
         } else {
@@ -228,15 +251,16 @@
         }
     }
 
-
-
 #warning Accelerometer calibration active: have you calibrated your device?
     // remove offsets and scale accelerometer (calibration)
     values[0] = (values[0] - acc_off_x) / acc_scale_x;
     values[1] = (values[1] - acc_off_y) / acc_scale_y;
     values[2] = (values[2] - acc_off_z) / acc_scale_z;
 
+#if HAS_MPU6050()
     magn->getValues(&values[6]);
+#endif
+
     // calibration
 #warning Magnetometer calibration active: have you calibrated your device?
     values[6] = (values[6] - magn_off_x) / magn_scale_x;
@@ -268,7 +292,14 @@
     gyro_off_z = tmpOffsets[2] / totSamples;
 }
 
+#if HAS_MPU6050()
 extern volatile bool magn_valid;
+#endif
+
+#if HAS_MPU9250()
+const bool magn_valid = true;
+#endif
+
 
 /**
  * Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
@@ -326,7 +357,9 @@
         halfey = (mz * halfwx - mx * halfwz);
         halfez = (mx * halfwy - my * halfwx);
 
+#if HAS_MPU6050()
         magn_valid = false;
+#endif
     }
 
     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)

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

--- a/HMC58X3.lib	Wed Mar 07 04:13:28 2018 +0000
+++ b/HMC58X3.lib	Wed Mar 28 20:26:02 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/tyftyftyf/code/HMC58X3/#02c679492d35
+http://mbed.org/users/tyftyftyf/code/HMC58X3/#5279fb447af1

--- a/MPU6050.lib	Wed Mar 07 04:13:28 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/tyftyftyf/code/MPU6050/#9549be34fa7f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.lib	Wed Mar 28 20:26:02 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/tyftyftyf/code/MPU9250/#a74f2d622b54