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

FreeIMU.h

Committer:
tyftyftyf
Date:
2013-11-02
Revision:
0:21840c01d3d7
Child:
2:5c419926dcd7

File content as of revision 0:21840c01d3d7:

/*
FreeIMU.h - A libre and easy to use orientation sensing library for Arduino
Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>

Development of this code has been supported by the Department of Computer Science,
Universita' degli Studi di Torino, Italy within the Piemonte Project
http://www.piemonte.di.unito.it/


This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#ifndef FreeIMU_h
#define FreeIMU_h

// Uncomment the appropriated version of FreeIMU you are using
//#define FREEIMU_v01
//#define FREEIMU_v02
//#define FREEIMU_v03
//#define FREEIMU_v035
//#define FREEIMU_v035_MS
//#define FREEIMU_v035_BMP
#define FREEIMU_v04

// 3rd party boards. Please consider donating or buying a FreeIMU board to support this library development.
//#define SEN_10121 //IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345 SEN-10121 http://www.sparkfun.com/products/10121
//#define SEN_10736 //9 Degrees of Freedom - Razor IMU SEN-10736 http://www.sparkfun.com/products/10736
//#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.

// *** No configuration needed below this line ***


#define FREEIMU_LIB_VERSION "20121122"

#define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"

#if F_CPU == 16000000L
  #define FREEIMU_FREQ "16 MHz"
#elif F_CPU == 8000000L
  #define FREEIMU_FREQ "8 MHz"
#endif


// board IDs

#if defined(FREEIMU_v04)
  #define FREEIMU_ID "FreeIMU v0.4"
#endif


#define HAS_MPU6050() (defined(FREEIMU_v04) || defined(GEN_MPU6050))

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



//#include <Wire.h>

#include "mbed.h"
#include "calibration.h"
/*
#ifndef CALIBRATION_H
#include <EEPROM.h>
#endif

#define FREEIMU_EEPROM_BASE 0x0A
#define FREEIMU_EEPROM_SIGNATURE 0x19
*/
//#if FREEIMU_VER <= 3

#if HAS_MPU6050()
 // #include <Wire.h>
  #include "I2Cdev.h"
  #include "MPU6050.h"
  #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS

#endif

#include <HMC58X3.h>
#include <MS561101BA.h>


#define FIMU_BARO_ADDR MS561101BA_ADDR_CSB_LOW


#define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW
#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 * 0.5f) // 2 * proportional gain
#define twoKiDef  (2.0f * 0.1f) // 2 * integral gain

#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif

class FreeIMU
{
  public:
    FreeIMU();
    void init();
    void init(bool fastmode);
    
    void init(int accgyro_addr, bool fastmode);
    
    #ifndef CALIBRATION_H
    void calLoad();
    #endif
    void zeroGyro();
    void getRawValues(int16_t * raw_values);
    void getValues(float * values);
    void getQ(float * q);
    void getEuler(float * angles);
    void getYawPitchRoll(float * ypr);
    void getEulerRad(float * angles);
    void getYawPitchRollRad(float * ypr);
    void gravityCompensateAcc(float * acc, float * q);
    float getRawPressure();
    
    float getBaroAlt();
    float getBaroAlt(float sea_press);
    
    // we make them public so that users can interact directly with device classes

      MPU6050 accgyro;
      MS561101BA baro;
      HMC58X3 magn;
    
    int* raw_acc, raw_gyro, raw_magn;
    // calibration parameters
    int16_t gyro_off_x, gyro_off_y, gyro_off_z;
    int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
    float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
    
  private:

    void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);

    //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
    float iq0, iq1, iq2, iq3;
    float exInt, eyInt, ezInt;  // scaled integral error
    volatile float twoKp;      // 2 * proportional gain (Kp)
    volatile float twoKi;      // 2 * integral gain (Ki)
    volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
    volatile float integralFBx,  integralFBy, integralFBz;
    Timer update;
    int dt_us;
    //unsigned long lastUpdate, now; // sample period expressed in milliseconds
    float sampleFreq; // half the sample period expressed in seconds
    
    void getQ_simple(float* q);
};

float invSqrt(float number);
void arr3_rad_to_deg(float * arr);



#endif // FreeIMU_h