Revision 0:21840c01d3d7, committed 2013-11-02

Comitter:

tyftyftyf

Date:

Sat Nov 02 17:25:51 2013 +0000

Child:

1:794e9cdbc2a0

Commit message:

Initial commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FreeIMU.cpp	Sat Nov 02 17:25:51 2013 +0000
@@ -0,0 +1,590 @@
+/*
+FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino
+Copyright (C) 2011-2012 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/>.
+
+02/20/2013 - Modified by Aloïs Wolff for MBED with MPU6050 only (wolffalois@gmail.com)
+*/
+
+//#include <inttypes.h>
+//#include <stdint.h>
+//#define DEBUG
+#include "FreeIMU.h"
+#define     M_PI 3.1415926535897932384626433832795
+
+#ifdef DEBUG
+ #define DEBUG_PRINT(x) Serial.println(x)
+ #else
+ #define DEBUG_PRINT(x)
+ #endif
+// #include "WireUtils.h"
+//#include "DebugUtils.h"
+
+//#include "vector_math.h"
+
+I2C i2c(I2C_SDA,I2C_SCL);
+
+FreeIMU::FreeIMU() {
+
+   i2c.frequency(400000);
+   accgyro = MPU6050(i2c); // I2C
+   magn = HMC58X3(i2c);
+   baro = MS561101BA(i2c);
+
+  // initialize quaternion
+  q0 = 1.0f;
+  q1 = 0.0f;
+  q2 = 0.0f;
+  q3 = 0.0f;
+  exInt = 0.0;
+  eyInt = 0.0;
+  ezInt = 0.0;
+  twoKp = twoKpDef;
+  twoKi = twoKiDef;
+  integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;
+  
+  
+  update.start();
+  dt_us=0;
+  /*
+  lastUpdate = 0;
+  now = 0;
+  */
+  #ifndef CALIBRATION_H
+  // initialize scale factors to neutral values
+  acc_scale_x = 1;
+  acc_scale_y = 1;
+  acc_scale_z = 1;
+  magn_scale_x = 1;
+  magn_scale_y = 1;
+  magn_scale_z = 1;
+  #else
+  // get values from global variables of same name defined in calibration.h
+  acc_off_x = ::acc_off_x;
+  acc_off_y = ::acc_off_y;
+  acc_off_z = ::acc_off_z;
+  acc_scale_x = ::acc_scale_x;
+  acc_scale_y = ::acc_scale_y;
+  acc_scale_z = ::acc_scale_z;
+  magn_off_x = ::magn_off_x;
+  magn_off_y = ::magn_off_y;
+  magn_off_z = ::magn_off_z;
+  magn_scale_x = ::magn_scale_x;
+  magn_scale_y = ::magn_scale_y;
+  magn_scale_z = ::magn_scale_z;
+  #endif
+}
+
+void FreeIMU::init() {
+
+  init(FIMU_ACCGYRO_ADDR, false);
+
+}
+
+void FreeIMU::init(bool fastmode) {
+ 
+  init(FIMU_ACCGYRO_ADDR, fastmode);
+ 
+}
+
+
+/**
+ * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
+*/
+
+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);
+  accgyro = MPU6050(0x68);
+  accgyro.initialize();
+  accgyro.setI2CMasterModeEnabled(0);
+  accgyro.setI2CBypassEnabled(1);
+  accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
+  wait_ms(5);
+
+
+  // init HMC5843
+  magn.init(false); // Don't set mode yet, we'll do that later on.
+  // 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.
+  // Single mode conversion was used in calibration, now set continuous mode
+  magn.setMode(0);
+  wait_ms(10);
+  magn.setDOR(6);
+  
+  baro.init(FIMU_BARO_ADDR);
+  
+  // zero gyro
+  zeroGyro();
+  
+  #ifndef CALIBRATION_H
+  // load calibration from eeprom
+  calLoad();
+  #endif
+  getQ_simple(NULL);
+}
+
+void FreeIMU::getQ_simple(float* q)
+{
+  float values[9];
+  getValues(values);
+  
+  float pitch = atan2(values[0], sqrt(values[1]*values[1]+values[2]*values[2]));
+  float roll = -atan2(values[1], sqrt(values[0]*values[0]+values[2]*values[2]));
+  
+  float xh = values[6]*cos(pitch)+values[7]*sin(roll)*sin(pitch)-values[8]*cos(roll)*sin(pitch);
+  float yh = values[7]*cos(roll)+values[8]*sin(roll);
+  float yaw = atan2(yh, xh);
+  
+  float rollOver2 = roll * 0.5f;
+  float sinRollOver2 = (float)sin(rollOver2);
+  float cosRollOver2 = (float)cos(rollOver2);
+  float pitchOver2 = pitch * 0.5f;
+  float sinPitchOver2 = (float)sin(pitchOver2);
+  float cosPitchOver2 = (float)cos(pitchOver2);
+  float yawOver2 = yaw * 0.5f;
+  float sinYawOver2 = (float)sin(yawOver2);
+  float cosYawOver2 = (float)cos(yawOver2);
+
+  q0 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
+  q1 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
+  q2 = - cosYawOver2 * cosPitchOver2 * cosRollOver2 - sinYawOver2 * sinPitchOver2 * sinRollOver2;
+  q3 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
+  
+  if (q!=NULL){
+          q[0] = q0;
+          q[1] = q1;
+          q[2] = q2;
+          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
+*/
+void FreeIMU::getRawValues(int16_t * raw_values) {
+
+    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]);
+    
+    int temp, press;
+    //TODO: possible loss of precision
+    temp = baro.rawTemperature(MS561101BA_OSR_4096);
+    raw_values[9] = temp;
+    press = baro.rawPressure(MS561101BA_OSR_4096);
+    raw_values[10] = press;
+}
+
+
+/**
+ * Populates values with calibrated readings from the sensors
+*/
+void FreeIMU::getValues(float * values) {  
+
+// MPU6050
+    int16_t accgyroval[6];
+    accgyro.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
+    
+    // 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++) {
+      if(i < 3) {
+        values[i] = (float) accgyroval[i];
+      }
+      else {
+        values[i] = ((float) accgyroval[i]) / 16.4f; // NOTE: this depends on the sensitivity chosen
+      }
+    }
+
+  
+  
+  #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;
+  
+  magn.getValues(&values[6]);
+    // calibration 
+    #warning Magnetometer calibration active: have you calibrated your device?
+    values[6] = (values[6] - magn_off_x) / magn_scale_x;
+    values[7] = (values[7] - magn_off_y) / magn_scale_y;
+    values[8] = (values[8] - magn_off_z) / magn_scale_z;
+ 
+}
+
+
+/**
+ * Computes gyro offsets
+*/
+void FreeIMU::zeroGyro() {
+  const int totSamples = 3;
+  int16_t raw[11];
+  float tmpOffsets[] = {0,0,0};
+  
+  for (int i = 0; i < totSamples; i++){
+    getRawValues(raw);
+    tmpOffsets[0] += raw[3];
+    tmpOffsets[1] += raw[4];
+    tmpOffsets[2] += raw[5];
+  }
+  
+  gyro_off_x = tmpOffsets[0] / totSamples;
+  gyro_off_y = tmpOffsets[1] / totSamples;
+  gyro_off_z = tmpOffsets[2] / totSamples;
+}
+
+
+/**
+ * Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
+ * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
+ * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
+ * axis only.
+ * 
+ * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+*/
+
+void  FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+
+  float recipNorm;
+  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+  float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
+  float qa, qb, qc;
+
+  // Auxiliary variables to avoid repeated arithmetic
+  q0q0 = q0 * q0;
+  q0q1 = q0 * q1;
+  q0q2 = q0 * q2;
+  q0q3 = q0 * q3;
+  q1q1 = q1 * q1;
+  q1q2 = q1 * q2;
+  q1q3 = q1 * q3;
+  q2q2 = q2 * q2;
+  q2q3 = q2 * q3;
+  q3q3 = q3 * q3;
+  
+  // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
+  if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
+    float hx, hy, bx, bz;
+    float halfwx, halfwy, halfwz;
+    
+    // Normalise magnetometer measurement
+    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    mx *= recipNorm;
+    my *= recipNorm;
+    mz *= recipNorm;
+    
+    // Reference direction of Earth's magnetic field
+    hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+    hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+    bx = sqrt(hx * hx + hy * hy);
+    bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
+    
+    // Estimated direction of magnetic field
+    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+    
+    // Error is sum of cross product between estimated direction and measured direction of field vectors
+    halfex = (my * halfwz - mz * halfwy);
+    halfey = (mz * halfwx - mx * halfwz);
+    halfez = (mx * halfwy - my * halfwx);
+  }
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+  if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
+    float halfvx, halfvy, halfvz;
+    
+    // Normalise accelerometer measurement
+    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+    
+    // Estimated direction of gravity
+    halfvx = q1q3 - q0q2;
+    halfvy = q0q1 + q2q3;
+    halfvz = q0q0 - 0.5f + q3q3;
+  
+    // Error is sum of cross product between estimated direction and measured direction of field vectors
+    halfex += (ay * halfvz - az * halfvy);
+    halfey += (az * halfvx - ax * halfvz);
+    halfez += (ax * halfvy - ay * halfvx);
+  }
+
+  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
+  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
+    // Compute and apply integral feedback if enabled
+    if(twoKi > 0.0f) {
+      integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
+      integralFBy += twoKi * halfey * (1.0f / sampleFreq);
+      integralFBz += twoKi * halfez * (1.0f / sampleFreq);
+      gx += integralFBx;  // apply integral feedback
+      gy += integralFBy;
+      gz += integralFBz;
+    }
+    else {
+      integralFBx = 0.0f; // prevent integral windup
+      integralFBy = 0.0f;
+      integralFBz = 0.0f;
+    }
+
+    // Apply proportional feedback
+    gx += twoKp * halfex;
+    gy += twoKp * halfey;
+    gz += twoKp * halfez;
+  }
+  
+  // Integrate rate of change of quaternion
+  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
+  gy *= (0.5f * (1.0f / sampleFreq));
+  gz *= (0.5f * (1.0f / sampleFreq));
+  qa = q0;
+  qb = q1;
+  qc = q2;
+  q0 += (-qb * gx - qc * gy - q3 * gz);
+  q1 += (qa * gx + qc * gz - q3 * gy);
+  q2 += (qa * gy - qb * gz + q3 * gx);
+  q3 += (qa * gz + qb * gy - qc * gx);
+  
+  // Normalise quaternion
+  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+}
+
+
+/**
+ * Populates array q with a quaternion representing the IMU orientation with respect to the Earth
+ * 
+ * @param q the quaternion to populate
+*/
+void FreeIMU::getQ(float * q) {
+  float val[9];
+  getValues(val);
+  
+  DEBUG_PRINT(val[3] * M_PI/180);
+  DEBUG_PRINT(val[4] * M_PI/180);
+  DEBUG_PRINT(val[5] * M_PI/180);
+  DEBUG_PRINT(val[0]);
+  DEBUG_PRINT(val[1]);
+  DEBUG_PRINT(val[2]);
+  DEBUG_PRINT(val[6]);
+  DEBUG_PRINT(val[7]);
+  DEBUG_PRINT(val[8]);
+  
+  //now = micros();
+  dt_us=update.read_us();
+  sampleFreq = 1.0 / ((dt_us) / 1000000.0);
+  update.reset();
+ // lastUpdate = now;
+  // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
+
+    AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], val[6], val[7], val[8]);
+
+  if (q!=NULL){
+      q[0] = q0;
+      q[1] = q1;
+      q[2] = q2;
+      q[3] = q3;
+  }
+}
+
+
+const float def_sea_press = 1013.25;
+
+/**
+ * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure
+*/
+float FreeIMU::getBaroAlt(float sea_press) {
+  float temp = baro.getTemperature(MS561101BA_OSR_4096);
+  float press = baro.getPressure(MS561101BA_OSR_4096);
+  return ((pow((float)(sea_press / press), 1.0f/5.257f) - 1.0f) * (temp + 273.15f)) / 0.0065f;
+}
+
+/**
+ * Returns an altitude estimate from baromether readings only using a default sea level pressure
+*/
+float FreeIMU::getBaroAlt() {
+  return getBaroAlt(def_sea_press);
+}
+
+float FreeIMU::getRawPressure() {
+  return baro.getPressure(MS561101BA_OSR_4096);
+}
+
+
+/**
+ * Compensates the accelerometer readings in the 3D vector acc expressed in the sensor frame for gravity
+ * @param acc the accelerometer readings to compensate for gravity
+ * @param q the quaternion orientation of the sensor board with respect to the world
+*/
+void FreeIMU::gravityCompensateAcc(float * acc, float * q) {
+  float g[3];
+  
+  // get expected direction of gravity in the sensor frame
+  g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
+  g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
+  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
+  
+  // compensate accelerometer readings with the expected direction of gravity
+  acc[0] = acc[0] - g[0];
+  acc[1] = acc[1] - g[1];
+  acc[2] = acc[2] - g[2];
+}
+
+
+/**
+ * Returns the Euler angles in radians defined in the Aerospace sequence.
+ * See Sebastian O.H. Madwick report "An efficient orientation filter for 
+ * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
+ * 
+ * @param angles three floats array which will be populated by the Euler angles in radians
+*/
+void FreeIMU::getEulerRad(float * angles) {
+  float q[4]; // quaternion
+  getQ(q);
+  angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
+  angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
+  angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
+}
+
+
+/**
+ * Returns the Euler angles in degrees defined with the Aerospace sequence.
+ * See Sebastian O.H. Madwick report "An efficient orientation filter for 
+ * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
+ * 
+ * @param angles three floats array which will be populated by the Euler angles in degrees
+*/
+void FreeIMU::getEuler(float * angles) {
+  getEulerRad(angles);
+  arr3_rad_to_deg(angles);
+}
+
+
+/**
+ * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
+ * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
+ * and the Earth ground plane and the IMU Y axis.
+ * 
+ * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
+ * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
+ * 
+ * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians
+*/
+void FreeIMU::getYawPitchRollRad(float * ypr) {
+  float q[4]; // quaternion
+  float gx, gy, gz; // estimated gravity direction
+  getQ(q);
+  
+  gx = 2 * (q[1]*q[3] - q[0]*q[2]);
+  gy = 2 * (q[0]*q[1] + q[2]*q[3]);
+  gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
+  
+  ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
+  ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
+  ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
+}
+
+
+/**
+ * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between
+ * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
+ * and the Earth ground plane and the IMU Y axis.
+ * 
+ * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
+ * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see FreeIMU::getEuler
+ * 
+ * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees
+*/
+void FreeIMU::getYawPitchRoll(float * ypr) {
+  getYawPitchRollRad(ypr);
+  arr3_rad_to_deg(ypr);
+}
+
+
+/**
+ * Converts a 3 elements array arr of angles expressed in radians into degrees
+*/
+void arr3_rad_to_deg(float * arr) {
+  arr[0] *= 180/M_PI;
+  arr[1] *= 180/M_PI;
+  arr[2] *= 180/M_PI;
+}
+
+
+/**
+ * Fast inverse square root implementation
+ * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root
+*/
+float invSqrt(float number) {
+  volatile long i;
+  volatile float x, y;
+  volatile const float f = 1.5F;
+
+  x = number * 0.5F;
+  y = number;
+  i = * ( long * ) &y;
+  i = 0x5f375a86 - ( i >> 1 );
+  y = * ( float * ) &i;
+  y = y * ( f - ( x * y * y ) );
+  return y;
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FreeIMU.h	Sat Nov 02 17:25:51 2013 +0000
@@ -0,0 +1,176 @@
+/*
+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
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC58X3.lib	Sat Nov 02 17:25:51 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tyftyftyf/code/HMC58X3/#c5ac16c88514

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Sat Nov 02 17:25:51 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tyftyftyf/code/MPU6050/#95e74f827c08

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MS561101BA.lib	Sat Nov 02 17:25:51 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tyftyftyf/code/MS561101BA/#8545a1d1d1e4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/calibration.h	Sat Nov 02 17:25:51 2013 +0000
@@ -0,0 +1,22 @@
+
+/**
+ * FreeIMU calibration header. Automatically generated by FreeIMU_GUI.
+ * Do not edit manually unless you know what you are doing.
+*/
+
+
+#define CALIBRATION_H
+
+const int acc_off_x = 799;
+const int acc_off_y = 143;
+const int acc_off_z = -2724;
+const float acc_scale_x = 15256.909937;
+const float acc_scale_y = 17366.956857;
+const float acc_scale_z = 18168.225188;
+
+const int magn_off_x = 10;
+const int magn_off_y = -45;
+const int magn_off_z = -161;
+const float magn_scale_x = 353.204566;
+const float magn_scale_y = 478.401406;
+const float magn_scale_z = 491.092701;