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);
}
Committer:
tyftyftyf
Date:
Sat Nov 02 17:25:51 2013 +0000
Revision:
0:21840c01d3d7
Child:
2:5c419926dcd7
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tyftyftyf 0:21840c01d3d7 1 /*
tyftyftyf 0:21840c01d3d7 2 FreeIMU.h - A libre and easy to use orientation sensing library for Arduino
tyftyftyf 0:21840c01d3d7 3 Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>
tyftyftyf 0:21840c01d3d7 4
tyftyftyf 0:21840c01d3d7 5 Development of this code has been supported by the Department of Computer Science,
tyftyftyf 0:21840c01d3d7 6 Universita' degli Studi di Torino, Italy within the Piemonte Project
tyftyftyf 0:21840c01d3d7 7 http://www.piemonte.di.unito.it/
tyftyftyf 0:21840c01d3d7 8
tyftyftyf 0:21840c01d3d7 9
tyftyftyf 0:21840c01d3d7 10 This program is free software: you can redistribute it and/or modify
tyftyftyf 0:21840c01d3d7 11 it under the terms of the version 3 GNU General Public License as
tyftyftyf 0:21840c01d3d7 12 published by the Free Software Foundation.
tyftyftyf 0:21840c01d3d7 13
tyftyftyf 0:21840c01d3d7 14 This program is distributed in the hope that it will be useful,
tyftyftyf 0:21840c01d3d7 15 but WITHOUT ANY WARRANTY; without even the implied warranty of
tyftyftyf 0:21840c01d3d7 16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
tyftyftyf 0:21840c01d3d7 17 GNU General Public License for more details.
tyftyftyf 0:21840c01d3d7 18
tyftyftyf 0:21840c01d3d7 19 You should have received a copy of the GNU General Public License
tyftyftyf 0:21840c01d3d7 20 along with this program. If not, see <http://www.gnu.org/licenses/>.
tyftyftyf 0:21840c01d3d7 21
tyftyftyf 0:21840c01d3d7 22 */
tyftyftyf 0:21840c01d3d7 23
tyftyftyf 0:21840c01d3d7 24 #ifndef FreeIMU_h
tyftyftyf 0:21840c01d3d7 25 #define FreeIMU_h
tyftyftyf 0:21840c01d3d7 26
tyftyftyf 0:21840c01d3d7 27 // Uncomment the appropriated version of FreeIMU you are using
tyftyftyf 0:21840c01d3d7 28 //#define FREEIMU_v01
tyftyftyf 0:21840c01d3d7 29 //#define FREEIMU_v02
tyftyftyf 0:21840c01d3d7 30 //#define FREEIMU_v03
tyftyftyf 0:21840c01d3d7 31 //#define FREEIMU_v035
tyftyftyf 0:21840c01d3d7 32 //#define FREEIMU_v035_MS
tyftyftyf 0:21840c01d3d7 33 //#define FREEIMU_v035_BMP
tyftyftyf 0:21840c01d3d7 34 #define FREEIMU_v04
tyftyftyf 0:21840c01d3d7 35
tyftyftyf 0:21840c01d3d7 36 // 3rd party boards. Please consider donating or buying a FreeIMU board to support this library development.
tyftyftyf 0:21840c01d3d7 37 //#define SEN_10121 //IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345 SEN-10121 http://www.sparkfun.com/products/10121
tyftyftyf 0:21840c01d3d7 38 //#define SEN_10736 //9 Degrees of Freedom - Razor IMU SEN-10736 http://www.sparkfun.com/products/10736
tyftyftyf 0:21840c01d3d7 39 //#define SEN_10724 //9 Degrees of Freedom - Sensor Stick SEN-10724 http://www.sparkfun.com/products/10724
tyftyftyf 0:21840c01d3d7 40 //#define SEN_10183 //9 Degrees of Freedom - Sensor Stick SEN-10183 http://www.sparkfun.com/products/10183
tyftyftyf 0:21840c01d3d7 41 //#define ARDUIMU_v3 // DIYDrones ArduIMU+ V3 http://store.diydrones.com/ArduIMU_V3_p/kt-arduimu-30.htm or https://www.sparkfun.com/products/11055
tyftyftyf 0:21840c01d3d7 42 #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.
tyftyftyf 0:21840c01d3d7 43
tyftyftyf 0:21840c01d3d7 44 // *** No configuration needed below this line ***
tyftyftyf 0:21840c01d3d7 45
tyftyftyf 0:21840c01d3d7 46
tyftyftyf 0:21840c01d3d7 47 #define FREEIMU_LIB_VERSION "20121122"
tyftyftyf 0:21840c01d3d7 48
tyftyftyf 0:21840c01d3d7 49 #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
tyftyftyf 0:21840c01d3d7 50
tyftyftyf 0:21840c01d3d7 51 #if F_CPU == 16000000L
tyftyftyf 0:21840c01d3d7 52 #define FREEIMU_FREQ "16 MHz"
tyftyftyf 0:21840c01d3d7 53 #elif F_CPU == 8000000L
tyftyftyf 0:21840c01d3d7 54 #define FREEIMU_FREQ "8 MHz"
tyftyftyf 0:21840c01d3d7 55 #endif
tyftyftyf 0:21840c01d3d7 56
tyftyftyf 0:21840c01d3d7 57
tyftyftyf 0:21840c01d3d7 58 // board IDs
tyftyftyf 0:21840c01d3d7 59
tyftyftyf 0:21840c01d3d7 60 #if defined(FREEIMU_v04)
tyftyftyf 0:21840c01d3d7 61 #define FREEIMU_ID "FreeIMU v0.4"
tyftyftyf 0:21840c01d3d7 62 #endif
tyftyftyf 0:21840c01d3d7 63
tyftyftyf 0:21840c01d3d7 64
tyftyftyf 0:21840c01d3d7 65 #define HAS_MPU6050() (defined(FREEIMU_v04) || defined(GEN_MPU6050))
tyftyftyf 0:21840c01d3d7 66
tyftyftyf 0:21840c01d3d7 67 #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
tyftyftyf 0:21840c01d3d7 68 #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))
tyftyftyf 0:21840c01d3d7 69
tyftyftyf 0:21840c01d3d7 70
tyftyftyf 0:21840c01d3d7 71
tyftyftyf 0:21840c01d3d7 72 //#include <Wire.h>
tyftyftyf 0:21840c01d3d7 73
tyftyftyf 0:21840c01d3d7 74 #include "mbed.h"
tyftyftyf 0:21840c01d3d7 75 #include "calibration.h"
tyftyftyf 0:21840c01d3d7 76 /*
tyftyftyf 0:21840c01d3d7 77 #ifndef CALIBRATION_H
tyftyftyf 0:21840c01d3d7 78 #include <EEPROM.h>
tyftyftyf 0:21840c01d3d7 79 #endif
tyftyftyf 0:21840c01d3d7 80
tyftyftyf 0:21840c01d3d7 81 #define FREEIMU_EEPROM_BASE 0x0A
tyftyftyf 0:21840c01d3d7 82 #define FREEIMU_EEPROM_SIGNATURE 0x19
tyftyftyf 0:21840c01d3d7 83 */
tyftyftyf 0:21840c01d3d7 84 //#if FREEIMU_VER <= 3
tyftyftyf 0:21840c01d3d7 85
tyftyftyf 0:21840c01d3d7 86 #if HAS_MPU6050()
tyftyftyf 0:21840c01d3d7 87 // #include <Wire.h>
tyftyftyf 0:21840c01d3d7 88 #include "I2Cdev.h"
tyftyftyf 0:21840c01d3d7 89 #include "MPU6050.h"
tyftyftyf 0:21840c01d3d7 90 #define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS
tyftyftyf 0:21840c01d3d7 91
tyftyftyf 0:21840c01d3d7 92 #endif
tyftyftyf 0:21840c01d3d7 93
tyftyftyf 0:21840c01d3d7 94 #include <HMC58X3.h>
tyftyftyf 0:21840c01d3d7 95 #include <MS561101BA.h>
tyftyftyf 0:21840c01d3d7 96
tyftyftyf 0:21840c01d3d7 97
tyftyftyf 0:21840c01d3d7 98 #define FIMU_BARO_ADDR MS561101BA_ADDR_CSB_LOW
tyftyftyf 0:21840c01d3d7 99
tyftyftyf 0:21840c01d3d7 100
tyftyftyf 0:21840c01d3d7 101 #define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW
tyftyftyf 0:21840c01d3d7 102 #define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND
tyftyftyf 0:21840c01d3d7 103 // HMC5843 address is fixed so don't bother to define it
tyftyftyf 0:21840c01d3d7 104
tyftyftyf 0:21840c01d3d7 105
tyftyftyf 0:21840c01d3d7 106 #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
tyftyftyf 0:21840c01d3d7 107 #define twoKiDef (2.0f * 0.1f) // 2 * integral gain
tyftyftyf 0:21840c01d3d7 108
tyftyftyf 0:21840c01d3d7 109 #ifndef cbi
tyftyftyf 0:21840c01d3d7 110 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
tyftyftyf 0:21840c01d3d7 111 #endif
tyftyftyf 0:21840c01d3d7 112
tyftyftyf 0:21840c01d3d7 113 class FreeIMU
tyftyftyf 0:21840c01d3d7 114 {
tyftyftyf 0:21840c01d3d7 115 public:
tyftyftyf 0:21840c01d3d7 116 FreeIMU();
tyftyftyf 0:21840c01d3d7 117 void init();
tyftyftyf 0:21840c01d3d7 118 void init(bool fastmode);
tyftyftyf 0:21840c01d3d7 119
tyftyftyf 0:21840c01d3d7 120 void init(int accgyro_addr, bool fastmode);
tyftyftyf 0:21840c01d3d7 121
tyftyftyf 0:21840c01d3d7 122 #ifndef CALIBRATION_H
tyftyftyf 0:21840c01d3d7 123 void calLoad();
tyftyftyf 0:21840c01d3d7 124 #endif
tyftyftyf 0:21840c01d3d7 125 void zeroGyro();
tyftyftyf 0:21840c01d3d7 126 void getRawValues(int16_t * raw_values);
tyftyftyf 0:21840c01d3d7 127 void getValues(float * values);
tyftyftyf 0:21840c01d3d7 128 void getQ(float * q);
tyftyftyf 0:21840c01d3d7 129 void getEuler(float * angles);
tyftyftyf 0:21840c01d3d7 130 void getYawPitchRoll(float * ypr);
tyftyftyf 0:21840c01d3d7 131 void getEulerRad(float * angles);
tyftyftyf 0:21840c01d3d7 132 void getYawPitchRollRad(float * ypr);
tyftyftyf 0:21840c01d3d7 133 void gravityCompensateAcc(float * acc, float * q);
tyftyftyf 0:21840c01d3d7 134 float getRawPressure();
tyftyftyf 0:21840c01d3d7 135
tyftyftyf 0:21840c01d3d7 136 float getBaroAlt();
tyftyftyf 0:21840c01d3d7 137 float getBaroAlt(float sea_press);
tyftyftyf 0:21840c01d3d7 138
tyftyftyf 0:21840c01d3d7 139 // we make them public so that users can interact directly with device classes
tyftyftyf 0:21840c01d3d7 140
tyftyftyf 0:21840c01d3d7 141 MPU6050 accgyro;
tyftyftyf 0:21840c01d3d7 142 MS561101BA baro;
tyftyftyf 0:21840c01d3d7 143 HMC58X3 magn;
tyftyftyf 0:21840c01d3d7 144
tyftyftyf 0:21840c01d3d7 145 int* raw_acc, raw_gyro, raw_magn;
tyftyftyf 0:21840c01d3d7 146 // calibration parameters
tyftyftyf 0:21840c01d3d7 147 int16_t gyro_off_x, gyro_off_y, gyro_off_z;
tyftyftyf 0:21840c01d3d7 148 int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
tyftyftyf 0:21840c01d3d7 149 float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
tyftyftyf 0:21840c01d3d7 150
tyftyftyf 0:21840c01d3d7 151 private:
tyftyftyf 0:21840c01d3d7 152
tyftyftyf 0:21840c01d3d7 153 void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
tyftyftyf 0:21840c01d3d7 154
tyftyftyf 0:21840c01d3d7 155 //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
tyftyftyf 0:21840c01d3d7 156 float iq0, iq1, iq2, iq3;
tyftyftyf 0:21840c01d3d7 157 float exInt, eyInt, ezInt; // scaled integral error
tyftyftyf 0:21840c01d3d7 158 volatile float twoKp; // 2 * proportional gain (Kp)
tyftyftyf 0:21840c01d3d7 159 volatile float twoKi; // 2 * integral gain (Ki)
tyftyftyf 0:21840c01d3d7 160 volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
tyftyftyf 0:21840c01d3d7 161 volatile float integralFBx, integralFBy, integralFBz;
tyftyftyf 0:21840c01d3d7 162 Timer update;
tyftyftyf 0:21840c01d3d7 163 int dt_us;
tyftyftyf 0:21840c01d3d7 164 //unsigned long lastUpdate, now; // sample period expressed in milliseconds
tyftyftyf 0:21840c01d3d7 165 float sampleFreq; // half the sample period expressed in seconds
tyftyftyf 0:21840c01d3d7 166
tyftyftyf 0:21840c01d3d7 167 void getQ_simple(float* q);
tyftyftyf 0:21840c01d3d7 168 };
tyftyftyf 0:21840c01d3d7 169
tyftyftyf 0:21840c01d3d7 170 float invSqrt(float number);
tyftyftyf 0:21840c01d3d7 171 void arr3_rad_to_deg(float * arr);
tyftyftyf 0:21840c01d3d7 172
tyftyftyf 0:21840c01d3d7 173
tyftyftyf 0:21840c01d3d7 174
tyftyftyf 0:21840c01d3d7 175 #endif // FreeIMU_h
tyftyftyf 0:21840c01d3d7 176