Diff: FreeIMU.cpp

Revision:

3:f9b100a9aa65

Parent:

2:5c419926dcd7

Child:

6:6b1185b32814

--- a/FreeIMU.cpp	Tue Nov 05 13:38:07 2013 +0000
+++ b/FreeIMU.cpp	Sat Nov 09 08:53:25 2013 +0000
@@ -25,169 +25,201 @@
 //#include <inttypes.h>
 //#include <stdint.h>
 //#define DEBUG
+#include "MODI2C.h"
 #include "FreeIMU.h"
+#include "rtos.h"
+
 #define     M_PI 3.1415926535897932384626433832795
 
 #ifdef DEBUG
- #define DEBUG_PRINT(x) Serial.println(x)
- #else
- #define DEBUG_PRINT(x)
- #endif
+#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()
+{
 
-FreeIMU::FreeIMU() : accgyro(MPU6050(i2c)), magn(HMC58X3(i2c)), baro(MS561101BA(i2c)){
-
-   i2c.frequency(400000);
+    // 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;
 
-  // 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
+    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() {
+void FreeIMU::init()
+{
 
-  init(FIMU_ACCGYRO_ADDR, false);
+    init(FIMU_ACCGYRO_ADDR, false);
 
 }
 
-void FreeIMU::init(bool fastmode) {
- 
-  init(FIMU_ACCGYRO_ADDR, fastmode);
- 
+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) {
+void FreeIMU::init(int accgyro_addr, bool fastmode)
+{
+    accgyro = new MPU6050();
+    Thread::wait(10);
+    baro = new MS561101BA();
+    magn = new HMC58X3();
 
-  wait_ms(10);
+    Thread::wait(10);
+
+    printf("AccGyro init start\r\n");
+
+    printf("DeviceId = %d\r\n",accgyro->getDeviceID());
 
-  accgyro = MPU6050(0x68);
-  accgyro.initialize();
-  accgyro.setI2CMasterModeEnabled(0);
-  accgyro.setI2CBypassEnabled(1);
-  accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
-  accgyro.setDLPFMode(1);
-  accgyro.setRate(0);
-  wait_ms(10);
+    accgyro->initialize();
+    printf("AccGyro setting\r\n");
+    accgyro->setI2CMasterModeEnabled(0);
+    accgyro->setI2CBypassEnabled(1);
+    accgyro->setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
+    accgyro->setDLPFMode(0);
+    accgyro->setRate(0);
+    Thread::wait(20);
 
+    accgyro->start_sampling();
+
+    printf("AccGyro init fin\r\n");
+    Thread::wait(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(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(20);
-  magn.setDOR(6);
-  
-  baro.init(FIMU_BARO_ADDR);
-  
-  // zero gyro
-  zeroGyro();
-  
-  #ifndef CALIBRATION_H
-  // load calibration from eeprom
-  calLoad();
-  #endif
-  getQ_simple(NULL);
+    // 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(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);
+
+    Thread::wait(30);
+
+    magn->setDOR(6);
+
+    Thread::wait(30);
+
+    printf("Magn init fin\r\n");
+
+    magn->start_sampling();
+
+    Thread::wait(30);
+
+    baro->init(FIMU_BARO_ADDR);
+
+    printf("Baro init fin\r\n");
+
+    // zero gyro
+    zeroGyro();
+
+#ifndef CALIBRATION_H
+    // load calibration from eeprom
+    calLoad();
+#endif
+
+    Thread::wait(30);
+
+    getQ_simple(NULL);
+
+    baro->start_sampling(MS561101BA_OSR_4096);
 }
 
 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 + M_PI) * 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);
+    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);
 
-  q0 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
-  q1 = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
-  q2 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
-  q3 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
-  
-  if (q!=NULL){
-          q[0] = q0;
-          q[1] = q1;
-          q[2] = q2;
-          q[3] = q3;
-  }
+    float rollOver2 = (roll + M_PI) * 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 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
+    q1 = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
+    q2 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
+    q3 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
+
+    if (q!=NULL) {
+        q[0] = q0;
+        q[1] = q1;
+        q[2] = q2;
+        q[3] = q3;
+    }
 }
 
 /**
  * Populates raw_values with the raw_values from the sensors
 */
-void FreeIMU::getRawValues(int16_t * raw_values) {
+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]);
-    
+    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);
+    temp = baro->rawTemperature();
     raw_values[9] = temp;
-    press = baro.rawPressure(MS561101BA_OSR_4096);
+    press = baro->rawPressure();
     raw_values[10] = press;
 }
 
@@ -195,220 +227,216 @@
 /**
  * Populates values with calibrated readings from the sensors
 */
-void FreeIMU::getValues(float * values) {  
+void FreeIMU::getValues(float * values)
+{
 
 // MPU6050
     int16_t accgyroval[6];
-    accgyro.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
-    
+    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]) / 32.8f; // NOTE: this depends on the sensitivity chosen
-      }
+        if(i < 3) {
+            values[i] = (float) accgyroval[i];
+        } else {
+            values[i] = ((float) accgyroval[i]) / 32.8f; // 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?
+
+
+#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;
+void FreeIMU::zeroGyro()
+{
+    const int totSamples = 8;
+    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];
+        Thread::wait(2);
+    }
+
+    gyro_off_x = tmpOffsets[0] / totSamples;
+    gyro_off_y = tmpOffsets[1] / totSamples;
+    gyro_off_z = tmpOffsets[2] / totSamples;
 }
 
+extern bool magn_valid;
 
 /**
  * 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) {
+void  FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, bool _magn_valid)
+{
+
+    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;
 
-  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;
 
-  // 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);
-  }
+    // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
+    if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f) && _magn_valid) {
+        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;
 
-  // 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);
-  }
+        // 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));
 
-  // 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;
+        // 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);
+
+        magn_valid = false;
     }
 
-    // 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;
+    // 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
+void FreeIMU::getQ(float * q)
+{
+    float val[9];
+    getValues(val);
 
-    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]);
+    //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
 
-  if (q!=NULL){
-      q[0] = q0;
-      q[1] = q1;
-      q[2] = q2;
-      q[3] = q3;
-  }
+    AHRSupdate(val[3] * M_PI/180.0, val[4] * M_PI/180.0, val[5] * M_PI/180.0, val[0], val[1], val[2], val[6], val[7], val[8], magn_valid);
+
+    if (q!=NULL) {
+        q[0] = q0;
+        q[1] = q1;
+        q[2] = q2;
+        q[3] = q3;
+    }
 }
 
 
@@ -417,21 +445,24 @@
 /**
  * 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;
+float FreeIMU::getBaroAlt(float sea_press)
+{
+    float temp = baro->getTemperature();
+    float press = baro->getPressure();
+    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::getBaroAlt()
+{
+    return getBaroAlt(def_sea_press);
 }
 
-float FreeIMU::getRawPressure() {
-  return baro.getPressure(MS561101BA_OSR_4096);
+float FreeIMU::getRawPressure()
+{
+    return baro->getPressure();
 }
 
 
@@ -440,47 +471,50 @@
  * @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];
+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 
+ * 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
+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 
+ * 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);
+void FreeIMU::getEuler(float * angles)
+{
+    getEulerRad(angles);
+    arr3_rad_to_deg(angles);
 }
 
 
@@ -488,24 +522,25 @@
  * 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));
+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));
 }
 
 
@@ -513,25 +548,27 @@
  * 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);
+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;
+void arr3_rad_to_deg(float * arr)
+{
+    arr[0] *= 180/M_PI;
+    arr[1] *= 180/M_PI;
+    arr[2] *= 180/M_PI;
 }
 
 
@@ -539,18 +576,19 @@
  * 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;
+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;
+    x = number * 0.5F;
+    y = number;
+    i = * ( long * ) &y;
+    i = 0x5f375a86 - ( i >> 1 );
+    y = * ( float * ) &i;
+    y = y * ( f - ( x * y * y ) );
+    return y;
 }