Revision 4:404c35f32ce3, committed 2014-09-04

Comitter:

janekm

Date:

Thu Sep 04 21:19:05 2014 +0000

Parent:

3:c05fbe0aef1f

Child:

5:ea541d293095

Commit message:

turning it into more of a library...

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS.c	Thu Sep 04 21:19:05 2014 +0000
@@ -0,0 +1,194 @@
+#include "AHRS.h"
+#include "math.h"
+
+static float eInt[3] = {0.0f, 0.0f, 0.0f}; 
+
+// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
+// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
+// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
+// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
+// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
+// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
+
+    void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat, float *q, float beta) {
+        float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
+        float norm;
+        float hx, hy, _2bx, _2bz;
+        float s1, s2, s3, s4;
+        float qDot1, qDot2, qDot3, qDot4;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        float _2q1mx;
+        float _2q1my;
+        float _2q1mz;
+        float _2q2mx;
+        float _4bx;
+        float _4bz;
+        float _2q1 = 2.0f * q1;
+        float _2q2 = 2.0f * q2;
+        float _2q3 = 2.0f * q3;
+        float _2q4 = 2.0f * q4;
+        float _2q1q3 = 2.0f * q1 * q3;
+        float _2q3q4 = 2.0f * q3 * q4;
+        float q1q1 = q1 * q1;
+        float q1q2 = q1 * q2;
+        float q1q3 = q1 * q3;
+        float q1q4 = q1 * q4;
+        float q2q2 = q2 * q2;
+        float q2q3 = q2 * q3;
+        float q2q4 = q2 * q4;
+        float q3q3 = q3 * q3;
+        float q3q4 = q3 * q4;
+        float q4q4 = q4 * q4;
+
+        // Normalise accelerometer measurement
+        norm = sqrt(ax * ax + ay * ay + az * az);
+        if (norm == 0.0f) return; // handle NaN
+        norm = 1.0f/norm;
+        ax *= norm;
+        ay *= norm;
+        az *= norm;
+
+        // Normalise magnetometer measurement
+        norm = sqrt(mx * mx + my * my + mz * mz);
+        if (norm == 0.0f) return; // handle NaN
+        norm = 1.0f/norm;
+        mx *= norm;
+        my *= norm;
+        mz *= norm;
+
+        // Reference direction of Earth's magnetic field
+        _2q1mx = 2.0f * q1 * mx;
+        _2q1my = 2.0f * q1 * my;
+        _2q1mz = 2.0f * q1 * mz;
+        _2q2mx = 2.0f * q2 * mx;
+        hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+        hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+        _2bx = sqrt(hx * hx + hy * hy);
+        _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+        _4bx = 2.0f * _2bx;
+        _4bz = 2.0f * _2bz;
+
+        // Gradient decent algorithm corrective step
+        s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
+        norm = 1.0f/norm;
+        s1 *= norm;
+        s2 *= norm;
+        s3 *= norm;
+        s4 *= norm;
+
+        // Compute rate of change of quaternion
+        qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
+        qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
+        qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
+        qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
+
+        // Integrate to yield quaternion
+        q1 += qDot1 * deltat;
+        q2 += qDot2 * deltat;
+        q3 += qDot3 * deltat;
+        q4 += qDot4 * deltat;
+        norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
+        norm = 1.0f/norm;
+        q[0] = q1 * norm;
+        q[1] = q2 * norm;
+        q[2] = q3 * norm;
+        q[3] = q4 * norm;
+
+}
+
+
+
+// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
+// measured ones.
+    void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat, float *q) {
+        float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
+        float norm;
+        float hx, hy, bx, bz;
+        float vx, vy, vz, wx, wy, wz;
+        float ex, ey, ez;
+        float pa, pb, pc;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        float q1q1 = q1 * q1;
+        float q1q2 = q1 * q2;
+        float q1q3 = q1 * q3;
+        float q1q4 = q1 * q4;
+        float q2q2 = q2 * q2;
+        float q2q3 = q2 * q3;
+        float q2q4 = q2 * q4;
+        float q3q3 = q3 * q3;
+        float q3q4 = q3 * q4;
+        float q4q4 = q4 * q4;
+
+        // Normalise accelerometer measurement
+        norm = sqrt(ax * ax + ay * ay + az * az);
+        if (norm == 0.0f) return; // handle NaN
+        norm = 1.0f / norm;        // use reciprocal for division
+        ax *= norm;
+        ay *= norm;
+        az *= norm;
+
+        // Normalise magnetometer measurement
+        norm = sqrt(mx * mx + my * my + mz * mz);
+        if (norm == 0.0f) return; // handle NaN
+        norm = 1.0f / norm;        // use reciprocal for division
+        mx *= norm;
+        my *= norm;
+        mz *= norm;
+
+        // Reference direction of Earth's magnetic field
+        hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
+        hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
+        bx = sqrt((hx * hx) + (hy * hy));
+        bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
+
+        // Estimated direction of gravity and magnetic field
+        vx = 2.0f * (q2q4 - q1q3);
+        vy = 2.0f * (q1q2 + q3q4);
+        vz = q1q1 - q2q2 - q3q3 + q4q4;
+        wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
+        wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
+        wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
+
+        // Error is cross product between estimated direction and measured direction of gravity
+        ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+        ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+        ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+        if (Ki > 0.0f) {
+            eInt[0] += ex;      // accumulate integral error
+            eInt[1] += ey;
+            eInt[2] += ez;
+        } else {
+            eInt[0] = 0.0f;     // prevent integral wind up
+            eInt[1] = 0.0f;
+            eInt[2] = 0.0f;
+        }
+
+        // Apply feedback terms
+        gx = gx + Kp * ex + Ki * eInt[0];
+        gy = gy + Kp * ey + Ki * eInt[1];
+        gz = gz + Kp * ez + Ki * eInt[2];
+
+        // Integrate rate of change of quaternion
+        pa = q2;
+        pb = q3;
+        pc = q4;
+        q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
+        q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
+        q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
+        q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
+
+        // Normalise quaternion
+        norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
+        norm = 1.0f / norm;
+        q[0] = q1 * norm;
+        q[1] = q2 * norm;
+        q[2] = q3 * norm;
+        q[3] = q4 * norm;
+
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS.h	Thu Sep 04 21:19:05 2014 +0000
@@ -0,0 +1,10 @@
+#ifndef __AHRS_H_
+#define __AHRS_H_
+
+#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
+#define Ki 0.0f
+
+void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat, float *q, float beta);
+void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat, float *q);
+
+#endif // __AHRS_H_
\ No newline at end of file

--- a/MPU9250.h	Thu Sep 04 20:16:36 2014 +0000
+++ b/MPU9250.h	Thu Sep 04 21:19:05 2014 +0000
@@ -190,12 +190,12 @@
 float aRes, gRes, mRes;      // scale resolutions per LSB for the sensors
 
 //Set up I2C, (SDA,SCL)
-I2C i2c(I2C_SDA, I2C_SCL);
+//I2C i2c(I2C_SDA, I2C_SCL);
 
-DigitalOut myled(LED1);
+//DigitalOut myled(LED1);
 
 // Pin definitions
-int intPin = 12;  // These can be changed, 2 and 3 are the Arduinos ext int pins
+//int intPin = 12;  // These can be changed, 2 and 3 are the Arduinos ext int pins
 
 int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
 int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
@@ -216,13 +216,10 @@
 float beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
 float GyroMeasDrift = PI * (1.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
-#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
-#define Ki 0.0f
 
 float pitch, yaw, roll;
 float deltat = 0.0f;                             // integration interval for both filter schemes
 int lastUpdate = 0, firstUpdate = 0, Now = 0;    // used to calculate integration interval                               // used to calculate integration interval
-float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};           // vector to hold quaternion
 float eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method
 
 class MPU9250
@@ -235,8 +232,8 @@
     //===================================================================================================================
 //====== Set of useful function to access acceleratio, gyroscope, and temperature data
 //===================================================================================================================
-    MPU9250(I2C *i2c)  _i2c( i2c ) {
-        break;
+    MPU9250(I2C *i2c) : _i2c( i2c ) {
+        
     }
     
     void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
@@ -619,7 +616,7 @@
 // Configure the accelerometer for self-test
         writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
         writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
-        delay(25); // Delay a while to let the device stabilize
+        wait(25); // Delay a while to let the device stabilize
 
         for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
 
@@ -642,7 +639,7 @@
 // Configure the gyro and accelerometer for normal operation
         writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
         writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
-        delay(25); // Delay a while to let the device stabilize
+        wait(25); // Delay a while to let the device stabilize
 
         // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
         selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
@@ -670,195 +667,5 @@
     }
 
 
-
-// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
-// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
-// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
-// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
-// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
-// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
-
-    void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) {
-        float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
-        float norm;
-        float hx, hy, _2bx, _2bz;
-        float s1, s2, s3, s4;
-        float qDot1, qDot2, qDot3, qDot4;
-
-        // Auxiliary variables to avoid repeated arithmetic
-        float _2q1mx;
-        float _2q1my;
-        float _2q1mz;
-        float _2q2mx;
-        float _4bx;
-        float _4bz;
-        float _2q1 = 2.0f * q1;
-        float _2q2 = 2.0f * q2;
-        float _2q3 = 2.0f * q3;
-        float _2q4 = 2.0f * q4;
-        float _2q1q3 = 2.0f * q1 * q3;
-        float _2q3q4 = 2.0f * q3 * q4;
-        float q1q1 = q1 * q1;
-        float q1q2 = q1 * q2;
-        float q1q3 = q1 * q3;
-        float q1q4 = q1 * q4;
-        float q2q2 = q2 * q2;
-        float q2q3 = q2 * q3;
-        float q2q4 = q2 * q4;
-        float q3q3 = q3 * q3;
-        float q3q4 = q3 * q4;
-        float q4q4 = q4 * q4;
-
-        // Normalise accelerometer measurement
-        norm = sqrt(ax * ax + ay * ay + az * az);
-        if (norm == 0.0f) return; // handle NaN
-        norm = 1.0f/norm;
-        ax *= norm;
-        ay *= norm;
-        az *= norm;
-
-        // Normalise magnetometer measurement
-        norm = sqrt(mx * mx + my * my + mz * mz);
-        if (norm == 0.0f) return; // handle NaN
-        norm = 1.0f/norm;
-        mx *= norm;
-        my *= norm;
-        mz *= norm;
-
-        // Reference direction of Earth's magnetic field
-        _2q1mx = 2.0f * q1 * mx;
-        _2q1my = 2.0f * q1 * my;
-        _2q1mz = 2.0f * q1 * mz;
-        _2q2mx = 2.0f * q2 * mx;
-        hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
-        hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
-        _2bx = sqrt(hx * hx + hy * hy);
-        _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
-        _4bx = 2.0f * _2bx;
-        _4bz = 2.0f * _2bz;
-
-        // Gradient decent algorithm corrective step
-        s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
-        s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
-        s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
-        s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
-        norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
-        norm = 1.0f/norm;
-        s1 *= norm;
-        s2 *= norm;
-        s3 *= norm;
-        s4 *= norm;
-
-        // Compute rate of change of quaternion
-        qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
-        qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
-        qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
-        qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
-
-        // Integrate to yield quaternion
-        q1 += qDot1 * deltat;
-        q2 += qDot2 * deltat;
-        q3 += qDot3 * deltat;
-        q4 += qDot4 * deltat;
-        norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
-        norm = 1.0f/norm;
-        q[0] = q1 * norm;
-        q[1] = q2 * norm;
-        q[2] = q3 * norm;
-        q[3] = q4 * norm;
-
-}
-
-
-
-// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
-// measured ones.
-    void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) {
-        float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
-        float norm;
-        float hx, hy, bx, bz;
-        float vx, vy, vz, wx, wy, wz;
-        float ex, ey, ez;
-        float pa, pb, pc;
-
-        // Auxiliary variables to avoid repeated arithmetic
-        float q1q1 = q1 * q1;
-        float q1q2 = q1 * q2;
-        float q1q3 = q1 * q3;
-        float q1q4 = q1 * q4;
-        float q2q2 = q2 * q2;
-        float q2q3 = q2 * q3;
-        float q2q4 = q2 * q4;
-        float q3q3 = q3 * q3;
-        float q3q4 = q3 * q4;
-        float q4q4 = q4 * q4;
-
-        // Normalise accelerometer measurement
-        norm = sqrt(ax * ax + ay * ay + az * az);
-        if (norm == 0.0f) return; // handle NaN
-        norm = 1.0f / norm;        // use reciprocal for division
-        ax *= norm;
-        ay *= norm;
-        az *= norm;
-
-        // Normalise magnetometer measurement
-        norm = sqrt(mx * mx + my * my + mz * mz);
-        if (norm == 0.0f) return; // handle NaN
-        norm = 1.0f / norm;        // use reciprocal for division
-        mx *= norm;
-        my *= norm;
-        mz *= norm;
-
-        // Reference direction of Earth's magnetic field
-        hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
-        hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
-        bx = sqrt((hx * hx) + (hy * hy));
-        bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
-
-        // Estimated direction of gravity and magnetic field
-        vx = 2.0f * (q2q4 - q1q3);
-        vy = 2.0f * (q1q2 + q3q4);
-        vz = q1q1 - q2q2 - q3q3 + q4q4;
-        wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
-        wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
-        wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
-
-        // Error is cross product between estimated direction and measured direction of gravity
-        ex = (ay * vz - az * vy) + (my * wz - mz * wy);
-        ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
-        ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
-        if (Ki > 0.0f) {
-            eInt[0] += ex;      // accumulate integral error
-            eInt[1] += ey;
-            eInt[2] += ez;
-        } else {
-            eInt[0] = 0.0f;     // prevent integral wind up
-            eInt[1] = 0.0f;
-            eInt[2] = 0.0f;
-        }
-
-        // Apply feedback terms
-        gx = gx + Kp * ex + Ki * eInt[0];
-        gy = gy + Kp * ey + Ki * eInt[1];
-        gz = gz + Kp * ez + Ki * eInt[2];
-
-        // Integrate rate of change of quaternion
-        pa = q2;
-        pb = q3;
-        pc = q4;
-        q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
-        q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
-        q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
-        q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
-
-        // Normalise quaternion
-        norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
-        norm = 1.0f / norm;
-        q[0] = q1 * norm;
-        q[1] = q2 * norm;
-        q[2] = q3 * norm;
-        q[3] = q4 * norm;
-
-    }
 };
 #endif
\ No newline at end of file

--- a/main.cpp	Thu Sep 04 20:16:36 2014 +0000
+++ b/main.cpp	Thu Sep 04 21:19:05 2014 +0000
@@ -30,7 +30,7 @@
 //F401_init84 myinit(0);
 #include "mbed.h"
 #include "MPU9250.h"
-#include "N5110.h"
+#include "AHRS.h"
 
 // Using NOKIA 5110 monochrome 84 x 48 pixel display
 // pin 9 - Serial clock out (SCLK)
@@ -41,17 +41,16 @@
 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
 
 float sum = 0;
+float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};           // vector to hold quaternion
+
 uint32_t sumCount = 0;
 char buffer[14];
-
-   MPU9250 mpu9250;
+I2C i2c(p7, p6);
+   MPU9250 mpu9250(&i2c);
    
    Timer t;
 
    Serial pc(USBTX, USBRX); // tx, rx
-
-   //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
-   N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
    
 
         
@@ -66,9 +65,6 @@
   
   t.start();        
   
-  lcd.init();
-//  lcd.setBrightness(0.05);
-  
     
   // Read the WHO_AM_I register, this is a good test of communication
   uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
@@ -78,11 +74,7 @@
   {  
     pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
     pc.printf("MPU9250 is online...\n\r");
-    lcd.clear();
-    lcd.printString("MPU9250 is", 0, 0);
     sprintf(buffer, "0x%x", whoami);
-    lcd.printString(buffer, 0, 1);
-    lcd.printString("shoud be 0x71", 0, 2);  
     wait(1);
     
     mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
@@ -117,13 +109,7 @@
    {
     pc.printf("Could not connect to MPU9250: \n\r");
     pc.printf("%#x \n",  whoami);
- 
-    lcd.clear();
-    lcd.printString("MPU9250", 0, 0);
-    lcd.printString("no connection", 0, 1);
-    sprintf(buffer, "WHO_AM_I 0x%x", whoami);
-    lcd.printString(buffer, 0, 2); 
- 
+  
     while(1) ; // Loop forever if communication doesn't happen
     }
 
@@ -176,7 +162,7 @@
     
    // Pass gyro rate as rad/s
 //  mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
-  mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+    MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz, deltat, q);
 
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - count;
@@ -237,7 +223,6 @@
 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
 //    lcd.printString(buffer, 0, 5);
     
-    myled= !myled;
     count = t.read_ms(); 
 
     if(count > 1<<21) {