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:
- 1:794e9cdbc2a0
- Parent:
- 0:21840c01d3d7
- Child:
- 2:5c419926dcd7
--- a/FreeIMU.cpp Sat Nov 02 17:25:51 2013 +0000 +++ b/FreeIMU.cpp Tue Nov 05 11:31:39 2013 +0000 @@ -40,12 +40,9 @@ I2C i2c(I2C_SDA,I2C_SCL); -FreeIMU::FreeIMU() { +FreeIMU::FreeIMU() : accgyro(MPU6050(i2c)), magn(HMC58X3(i2c)), baro(MS561101BA(i2c)){ i2c.frequency(400000); - accgyro = MPU6050(i2c); // I2C - magn = HMC58X3(i2c); - baro = MS561101BA(i2c); // initialize quaternion q0 = 1.0f; @@ -59,7 +56,6 @@ twoKi = twoKiDef; integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; - update.start(); dt_us=0; /* @@ -110,43 +106,26 @@ void FreeIMU::init(int accgyro_addr, bool fastmode) { - wait_ms(5); - /* - // disable internal pullups of the ATMEGA which Wire enable by default - #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) - // deactivate internal pull-ups for twi - // as per note from atmega8 manual pg167 - cbi(PORTC, 4); - cbi(PORTC, 5); - #else - // deactivate internal pull-ups for twi - // as per note from atmega128 manual pg204 - cbi(PORTD, 0); - cbi(PORTD, 1); - #endif - */ - - /* - if(fastmode) { // switch to 400KHz I2C - eheheh - TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c - } -*/ - //accgyro = MPU6050(false, accgyro_addr); + wait_ms(10); + accgyro = MPU6050(0x68); accgyro.initialize(); accgyro.setI2CMasterModeEnabled(0); accgyro.setI2CBypassEnabled(1); - accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); - wait_ms(5); + accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_1000); + accgyro.setDLPFMode(1); + accgyro.setRate(0); + wait_ms(10); // init HMC5843 magn.init(false); // Don't set mode yet, we'll do that later on. + magn.setGain(0); // Calibrate HMC using self test, not recommended to change the gain after calibration. - magn.calibrate(1); // Use gain 1=default, valid 0-7, 7 not recommended. + magn.calibrate(0, 8); // Use gain 1=default, valid 0-7, 7 not recommended. // Single mode conversion was used in calibration, now set continuous mode magn.setMode(0); - wait_ms(10); + wait_ms(20); magn.setDOR(6); baro.init(FIMU_BARO_ADDR); @@ -195,19 +174,6 @@ q[3] = q3; } } -/* -#ifndef CALIBRATION_H - -static uint8_t location; // assuming ordered reads - -void eeprom_read_var(uint8_t size, byte * var) { - for(uint8_t i = 0; i<size; i++) { - var[i] = EEPROM.read(location + i); - } - location += size; -} -*/ - /** * Populates raw_values with the raw_values from the sensors @@ -245,7 +211,7 @@ values[i] = (float) accgyroval[i]; } else { - values[i] = ((float) accgyroval[i]) / 16.4f; // NOTE: this depends on the sensitivity chosen + values[i] = ((float) accgyroval[i]) / 32.8f; // NOTE: this depends on the sensitivity chosen } }