Forum Rule: Always post complete source code & details to reproduce any issue!

# Thread: quaternian is changing very very slowly

1. ## 9DOF : Quaternian is changing very very slowly with Teensy 3.1

Hi everyone,

I have an issue that I don't even know where to begin. I'm using sparkfun's 9DOF LSM9DS0 IMU Breakout. when I run one of the examples see below.
the arithmetic for the quaternian calculations for yaw pitch and roll run much slower then the simpler calculations for yaw pitch and roll. Here it is some of the data log.

Any ideas as why I see this issue with Teensy and not with arduino pro 3.3 8mhz?
Thank you!

Pitch, Roll: -8.16, -6.76
ax = -156.01 ay = -131.84 az = 977.60 mg
gx = 1.35 gy = 0.87 gz = -9.33 deg/s
mx = -16.30 my = 59.51 mz = 427.98 mG
Yaw, Pitch, Roll: 63.36, 4.84, 35.34
q0 = 0.72 qx = 0.20 qy = 0.23 qz = 0.62
deltat = 0.0035
Pitch, Roll: -8.09, -6.83
ax = -156.19 ay = -134.09 az = 981.87 mg
gx = 1.49 gy = 1.31 gz = -8.50 deg/s
mx = -14.22 my = 61.10 mz = 439.21 mG
Yaw, Pitch, Roll: 63.67, 4.89, 36.13
q0 = 0.72 qx = 0.20 qy = 0.24 qz = 0.62
deltat = 0.0035
Pitch, Roll: -8.13, -6.86
ax = -156.01 ay = -134.03 az = 978.45 mg
gx = 2.00 gy = 0.37 gz = -9.23 deg/s
mx = -15.99 my = 60.24 mz = 424.32 mG
Yaw, Pitch, Roll: 63.21, 5.96, 33.09
q0 = 0.73 qx = 0.18 qy = 0.23 qz = 0.62
deltat = 0.0035
Pitch, Roll: -7.96, -6.71
ax = -155.27 ay = -133.24 az = 988.83 mg
gx = 1.29 gy = 0.96 gz = -9.45 deg/s
mx = -17.15 my = 61.34 mz = 417.48 mG
Yaw, Pitch, Roll: 63.41, 6.12, 33.93
q0 = 0.72 qx = 0.18 qy = 0.23 qz = 0.62
deltat = 0.0035
Pitch, Roll: -8.01, -6.84
ax = -155.33 ay = -134.89 az = 984.07 mg
gx = 1.91 gy = 0.68 gz = -9.32 deg/s
mx = -18.86 my = 61.65 mz = 432.56 mG
Yaw, Pitch, Roll: 63.70, 6.14, 34.62
q0 = 0.72 qx = 0.19 qy = 0.24 qz = 0.63
deltat = 0.0035
Pitch, Roll: -8.14, -6.91
ax = -157.23 ay = -135.80 az = 981.32 mg
gx = 1.20 gy = 1.05 gz = -8.36 deg/s
mx = -18.13 my = 62.93 mz = 407.71 mG
Yaw, Pitch, Roll: 63.91, 6.32, 35.46
q0 = 0.72 qx = 0.19 qy = 0.24 qz = 0.63
deltat = 0.0035
Pitch, Roll: -8.04, -6.90
ax = -155.76 ay = -135.80 az = 983.03 mg
gx = 1.38 gy = 1.11 gz = -9.19 deg/s
mx = -18.55 my = 63.72 mz = 421.94 mG
Yaw, Pitch, Roll: 63.48, 7.09, 32.43
q0 = 0.73 qx = 0.17 qy = 0.23 qz = 0.63
deltat = 0.0035

Code:
```/*****************************************************************
LSM9DS0_AHRS.ino
SFE_LSM9DS0 Library AHRS Data Fusion Example Code
Jim Lindblom @ SparkFun Electronics
Original Creation Date: February 18, 2014
https://github.com/sparkfun/LSM9DS0_Breakout

Modified by Kris Winer, April 4, 2014

The LSM9DS0 is a versatile 9DOF sensor. It has a built-in
accelerometer, gyroscope, and magnetometer. Very cool! Plus it
functions over either SPI or I2C.

This Arduino sketch utilizes Jim Lindblom's SFE_LSM9DS0 library to generate the basic sensor data
for use in two sensor fusion algorithms becoming increasingly popular with DIY quadcopter and robotics engineers.

Like the original LSM9SD0_simple.ino sketch, it'll demo the following:
* How to create a LSM9DS0 object, using a constructor (global
variables section).
* How to use the begin() function of the LSM9DS0 class.
* How to read the gyroscope, accelerometer, and magnetometer
gx, gy, gz, ax, ay, az, mx, my, and mz variables.
* How to calculate actual acceleration, rotation speed, magnetic
field strength using the calcAccel(), calcGyro() and calcMag()
functions.

In addition, the sketch will demo:
* How to check for data updates using interrupts
* How to display output at a rate different from the sensor data update and fusion filter update rates
* How to specify the accelerometer anti-aliasing (low-pass) filter rate
* How to use the data from the LSM9DS0 to fuse the sensor data into a quaternion representation of the sensor frame
orientation relative to a fixed Earth frame providing absolute orientation information for subsequent use.
* An example of how to use the quaternion data to generate standard aircraft orientation data in the form of
Tait-Bryan angles representing the sensor yaw, pitch, and roll angles suitable for any vehicle stablization control application.

Hardware setup: This library supports communicating with the
LSM9DS0 over either I2C or SPI. If you're using I2C, these are
the only connections that need to be made:
LSM9DS0 --------- Arduino
SCL ---------- SCL (A5 on older 'Duinos')
SDA ---------- SDA (A4 on older 'Duinos')
VDD ------------- 3.3V
GND ------------- GND
DRDYG-------------4   (gyro data ready interrupt, can be any digital pin)
INTX1-------------3   (accelerometer data ready interrupt, can be any digital pin)
INTX2-------------2   (magnetometer data ready interrupt, can be any digital pin)
(CSG, CSXM, SDOG, and SDOXM should all be pulled high jumpers on
the breakout board will do this for you.)

If you're using SPI, here is an example hardware setup:
LSM9DS0 --------- Arduino
CSG -------------- 9
CSXM ------------- 10
SDOG ------------- 12
SDOXM ------------ 12 (tied to SDOG)
SCL -------------- 13
SDA -------------- 11
VDD -------------- 3.3V
GND -------------- GND

The LSM9DS0 has a maximum voltage of 3.6V. Make sure you power it
off the 3.3V rail! And either use level shifters between SCL
and SDA or just use a 3.3V Arduino Pro.

In addition, this sketch uses a Nokia 5110 48 x 84 pixel display which requires
digital pins 5 - 9 described below. If using SPI you might need to press one of the A0 - A3 pins
into service as a digital input instead.

Development environment specifics:
IDE: Arduino 1.0.5
Hardware Platform: Arduino Pro 3.3V/8MHz
LSM9DS0 Breakout Version: 1.0

This code is beerware. If you see me (or any other SparkFun

Distributed as-is; no warranty is given.
*****************************************************************/

// The SFE_LSM9DS0 requires both the SPI and Wire libraries.
// Unfortunately, you'll need to include both in the Arduino
// sketch, before including the SFE_LSM9DS0 library.
#include <SPI.h> // Included for SFE_LSM9DS0 library
#include <Wire.h>
#include <SFE_LSM9DS0.h>
//#include "Arduino.h"

// Using NOKIA 5110 monochrome 84 x 48 pixel display
// pin 9 - Serial clock out (SCLK)
// pin 8 - Serial data out (DIN)
// pin 7 - Data/Command select (D/C)
// pin 5 - LCD chip select (CS)
// pin 6 - LCD reset (RST)

///////////////////////
// Example I2C Setup //
///////////////////////
// Comment out this section if you're using SPI
// SDO_XM and SDO_G are both grounded, so our addresses are:
#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
// Create an instance of the LSM9DS0 library called `dof` the
// parameters for this constructor are:
LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);

///////////////////////
// Example SPI Setup //
///////////////////////
/* // Uncomment this section if you're using SPI
#define LSM9DS0_CSG  9  // CSG connected to Arduino pin 9
#define LSM9DS0_CSXM 10 // CSXM connected to Arduino pin 10
LSM9DS0 dof(MODE_SPI, LSM9DS0_CSG, LSM9DS0_CSXM);
*/

///////////////////////////////
// Interrupt Pin Definitions //
///////////////////////////////
const byte INT1XM = 2; // INT1XM tells us when accel data is ready
const byte INT2XM = 3; // INT2XM tells us when mag data is ready
const byte DRDYG  = 4; // DRDYG  tells us when gyro data is ready

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
#define GyroMeasError PI * (40.0f / 180.0f)       // gyroscope measurement error in rads/s (shown as 3 deg/s)
#define GyroMeasDrift PI * (0.0f / 180.0f)      // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)
// There is a tradeoff in the beta parameter between accuracy and response speed.
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
#define beta sqrt(3.0f / 4.0f) * GyroMeasError   // compute beta
#define 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

uint16_t count = 0;  // used to control display output rate
uint16_t delt_t = 0; // used to control display output rate
float deltat = 0.0f;        // integration interval for both filter schemes
uint16_t lastUpdate = 0;    // used to calculate integration interval
uint16_t now = 0;           // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
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

void setup()
{
Serial.begin(38400); // Start serial at 38400 bps

// Set up interrupt pins as inputs:
pinMode(INT1XM, INPUT);
pinMode(INT2XM, INPUT);
pinMode(DRDYG,  INPUT);

display.begin(); // Initialize the display
display.setContrast(58); // Set the contrast
display.setRotation(0); //  0 or 2) width = width, 1 or 3) width = height, swapped etc.

// Start device display with ID of sensor
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0,0);  display.print("LSM9DS0");
display.setTextSize(1);
display.setCursor(0, 20); display.print("9 DOF sensor");
display.setCursor(5, 30); display.print("data fusion");
display.setCursor(20, 40); display.print("AHRS");
display.display();
delay(2000);

// Set up for data display
display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
display.clearDisplay();   // clears the screen and buffer
display.display();

// begin() returns a 16-bit value which includes both the gyro
// and accelerometers WHO_AM_I response. You can check this to
// make sure communication was successful.
uint16_t status = dof.begin();

Serial.print("LSM9DS0 WHO_AM_I's returned: 0x");
Serial.println(status, HEX);
Serial.println("Should be 0x49D4");
Serial.println();
display.setCursor(0,0); display.print("I AM");
display.setCursor(0,10); display.print(status, HEX);
display.setCursor(0,30); display.print("I Should Be");
display.setCursor(0,40); display.print(0x49D4, HEX);
display.display();
delay(2000);

// Set data output ranges; choose lowest ranges for maximum resolution
// Accelerometer scale can be: A_SCALE_2G, A_SCALE_4G, A_SCALE_6G, A_SCALE_8G, or A_SCALE_16G
dof.setAccelScale(dof.A_SCALE_2G);
// Gyro scale can be:  G_SCALE__245, G_SCALE__500, or G_SCALE__2000DPS
dof.setGyroScale(dof.G_SCALE_245DPS);
// Magnetometer scale can be: M_SCALE_2GS, M_SCALE_4GS, M_SCALE_8GS, M_SCALE_12GS
dof.setMagScale(dof.M_SCALE_2GS);

// Set output data rates
// Accelerometer output data rate (ODR) can be: A_ODR_3125 (3.225 Hz), A_ODR_625 (6.25 Hz), A_ODR_125 (12.5 Hz), A_ODR_25, A_ODR_50,
//                                              A_ODR_100,  A_ODR_200, A_ODR_400, A_ODR_800, A_ODR_1600 (1600 Hz)
dof.setAccelODR(dof.A_ODR_100); // Set accelerometer update rate at 100 Hz
// Accelerometer anti-aliasing filter rate can be 50, 194, 362, or 763 Hz
// Anti-aliasing acts like a low-pass filter allowing oversampling of accelerometer and rejection of high-frequency spurious noise.
// Strategy here is to effectively oversample accelerometer at 100 Hz and use a 50 Hz anti-aliasing (low-pass) filter frequency
// to get a smooth ~150 Hz filter update rate
dof.setAccelABW(dof.A_ABW_50); // Choose lowest filter setting for low noise

// Gyro output data rates can be: 95 Hz (bandwidth 12.5 or 25 Hz), 190 Hz (bandwidth 12.5, 25, 50, or 70 Hz)
//                                 380 Hz (bandwidth 20, 25, 50, 100 Hz), or 760 Hz (bandwidth 30, 35, 50, 100 Hz)
dof.setGyroODR(dof.G_ODR_190_BW_125);  // Set gyro update rate to 190 Hz with the smallest bandwidth for low noise

// Magnetometer output data rate can be: 3.125 (ODR_3125), 6.25 (ODR_625), 12.5 (ODR_125), 25, 50, or 100 Hz
dof.setMagODR(dof.M_ODR_125); // Set magnetometer to update every 80 ms
}

void loop()
{
gx = dof.calcGyro(dof.gx);   // Convert to degrees per seconds
gy = dof.calcGyro(dof.gy);
gz = dof.calcGyro(dof.gz);
}

ax = dof.calcAccel(dof.ax);   // Convert to g's
ay = dof.calcAccel(dof.ay);
az = dof.calcAccel(dof.az);
}

mx = dof.calcMag(dof.mx);     // Convert to Gauss
my = dof.calcMag(dof.my);
mz = dof.calcMag(dof.mz);
}

now = micros();
deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = now;
// Sensors x- and y-axes are aligned but magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!
// This is ok by aircraft orientation standards!
// Pass gyro rate as rad/s
MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
//MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);

// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = millis() - count;
if (delt_t > 500) { // update LCD once per half-second independent of read rate

// Print the heading and orientation for fun!
printOrientation(ax, ay, az);

// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination),
// looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, to get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw   *= 180.0f / PI - 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
roll  *= 180.0f / PI;

Serial.print("ax = "); Serial.print((int)1000*ax);
Serial.print(" ay = "); Serial.print((int)1000*ay);
Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
Serial.print("gx = "); Serial.print( gx, 2);
Serial.print(" gy = "); Serial.print( gy, 2);
Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
Serial.print("mx = "); Serial.print( (int)1000*mx, 2);
Serial.print(" my = "); Serial.print( (int)1000*my, 2);
Serial.print(" mz = "); Serial.print( (int)1000*mz, 2); Serial.println(" mG");

Serial.print("Yaw, Pitch, Roll: ");
Serial.print(yaw, 2);
Serial.print(", ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);

Serial.print("q0 = "); Serial.print(q[0]);
Serial.print(" qx = "); Serial.print(q[1]);
Serial.print(" qy = "); Serial.print(q[2]);
Serial.print(" qz = "); Serial.println(q[3]);

Serial.print("deltat = "); Serial.println(deltat, 4);

display.clearDisplay();

display.setCursor(0, 0); display.print(" x   y   z  ");

display.setCursor(0,  8); display.print((int)(1000*ax));
display.setCursor(24, 8); display.print((int)(1000*ay));
display.setCursor(48, 8); display.print((int)(1000*az));
display.setCursor(72, 8); display.print("mg");

display.setCursor(0,  16); display.print((int)(gx));
display.setCursor(24, 16); display.print((int)(gy));
display.setCursor(48, 16); display.print((int)(gz));
display.setCursor(66, 16); display.print("o/s");

display.setCursor(0,  24); display.print((int)(1000*mx));
display.setCursor(24, 24); display.print((int)(1000*my));
display.setCursor(48, 24); display.print((int)(1000*mz));
display.setCursor(72, 24); display.print("mG");

display.setCursor(0,  32); display.print((int)(yaw));
display.setCursor(24, 32); display.print((int)(pitch));
display.setCursor(48, 32); display.print((int)(roll));
display.setCursor(66, 32); display.print("ypr");

// With ODR settings of 400 Hz, 380 Hz, and 25 Hz for the accelerometer, gyro, and magnetometer, respectively,
// the filter is updating at a ~125 Hz rate using the Madgwick scheme and ~165 Hz using the Mahony scheme
// even though the display refreshes at only 2 Hz.
// The filter update rate can be increased by reducing the rate of data reading. The optimal implementation is
// one which balances the competing rates so they are about the same; that is, the filter updates the sensor orientation
// at about the same rate the data is changing. Of course, other implementations are possible. One might consider
// updating the filter at twice the average new data rate to allow for finite filter convergence times.
// The filter update rate is determined mostly by the mathematical steps in the respective algorithms,
// the processor speed (8 MHz for the 3.3V Pro Mini), and the sensor ODRs, especially the magnetometer ODR:
// smaller ODRs for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
// filter update rates of ~110 and ~135 Hz for the Madgwick and Mahony schemes, respectively.
// This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
// With low ODR settings of 100 Hz, 95 Hz, and 6.25 Hz for the accelerometer, gyro, and magnetometer, respectively,
// the filter is updating at a ~150 Hz rate using the Madgwick scheme and ~200 Hz using the Mahony scheme.
// These filter update rates should be fast enough to maintain accurate platform orientation for
// stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
// produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
// The 3.3 V 8 MHz Pro Mini is doing pretty well!
display.setCursor(0, 40); display.print("rt: "); display.print((1/deltat)); display.print(" Hz");

display.display();
count = millis();
}
}

// magnetic field.
// It only works if the sensor is flat (z-axis normal to Earth).
// See: http://www.ngdc.noaa.gov/geomag/declination.shtml
{
if (hy > 0)
{
heading = 90 - (atan(hx / hy) * (180 / PI));
}
else if (hy < 0)
{
heading = - (atan(hx / hy) * (180 / PI));
}
else // hy = 0
{
if (hx < 0) heading = 180;
}

}

// Another fun function that does calculations based on the
// acclerometer data. This function will print your LSM9DS0's
// orientation -- it's roll and pitch angles.
void printOrientation(float x, float y, float z)
{
// float pitch, roll;

pitch = atan2(x, sqrt(y * y) + (z * z));
roll = atan2(y, sqrt(x * x) + (z * z));
pitch *= 180.0 / PI;
roll *= 180.0 / PI;

Serial.print("Pitch, Roll: ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);
}

// 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;

}```

2. I have seen the same behavior with two of my Teensies. The sketch runs well and the LSM9DS0 reports sensible accelerometer, gyro, and magnetometer values that respond as expected when the sensor orientation is changed. For some reason, though, the quaternion calculation is not being updated properly; its values change very slowly such that the calculated yaw, pitch, and roll values do not keep up with the changing orientation as they do when the microcontroller is a 3.3 V 8 MHz Pro Mini Arduino. Is it possible that the math in the Madgwick or Mahony algorithms is handled differently on the Teensy? It seems like pretty generic arithmetic with a square root thrown in. It is a very curious result and hard to understand.

3. Given both the AVR processor (such as the Arduino Pro) and the Teensy 3.x do software emulation of floating point, and the Teensy is a 32-bit processor, and the clock speed is 12 times faster, you would expect the Teensy to run much faster.

In glancing at the code, I see calls to the various math functions (sqrt, atan,and atan2). These are done in double precision rather the single precision that most of the rest of the calculations are done in. If you do stuff in double precision, it takes more time to calculate the excess mantissa bits that are then thrown away (and the conversion between single precision and double precision adds to the cost). Try changing the calls to sqrtf, atanf, and atan2f, and see if it speeds things up. On the AVR processors, this is not an issue, since the 'double' type is mapped to 'float', so you are using the single precision versions of the library any way.

The other thought might be the processor is more optimized to 32-bit integers over 16-bit integers, and changing the int16_t types to int32_t might help (but you don't want to use int32_t on the AVR, so there you might want to have a type guarded by #ifdef to select the optimal type). However, compared to the floating point which predominates the calculations, I think this is a second order effect. I would check out the math functions first.

4. Is this the Sparkfun sensor?

https://www.sparkfun.com/products/12636

5. Yes, Paul.
I'm currently trying to see if MichaelMeissner suggestions make a difference.

Thank you all for your kind replies.

6. Ok, I have tried different approaches to what was suggested here.
It doesn't seem to quite work yet, and for some reason my new yaw quaternion is now giving me a negative number
see output here
Pitch, Roll: -0.67, 6.47
ax = -12.94 ay = 114.75 az = 999.39 mg
gx = 2.18 gy = 0.40 gz = -9.93 deg/s
mx = 285.10 my = 126.28 mz = 105.22 mG
Yaw, Pitch, Roll: -16.88, 0.73, 6.48
q0 = 0.98 qx = 0.06 qy = -0.00 qz = -0.19
deltat = 0.0024

code used below
Code:
```/*****************************************************************
LSM9DS0_AHRS.ino
SFE_LSM9DS0 Library AHRS Data Fusion Example Code
Jim Lindblom @ SparkFun Electronics
Original Creation Date: February 18, 2014
https://github.com/sparkfun/LSM9DS0_Breakout

Modified by Kris Winer, April 4, 2014

The LSM9DS0 is a versatile 9DOF sensor. It has a built-in
accelerometer, gyroscope, and magnetometer. Very cool! Plus it
functions over either SPI or I2C.

This Arduino sketch utilizes Jim Lindblom's SFE_LSM9DS0 library to generate the basic sensor data
for use in two sensor fusion algorithms becoming increasingly popular with DIY quadcopter and robotics engineers.

Like the original LSM9SD0_simple.ino sketch, it'll demo the following:
* How to create a LSM9DS0 object, using a constructor (global
variables section).
* How to use the begin() function of the LSM9DS0 class.
* How to read the gyroscope, accelerometer, and magnetometer
gx, gy, gz, ax, ay, az, mx, my, and mz variables.
* How to calculate actual acceleration, rotation speed, magnetic
field strength using the calcAccel(), calcGyro() and calcMag()
functions.

In addition, the sketch will demo:
* How to check for data updates using interrupts
* How to display output at a rate different from the sensor data update and fusion filter update rates
* How to specify the accelerometer anti-aliasing (low-pass) filter rate
* How to use the data from the LSM9DS0 to fuse the sensor data into a quaternion representation of the sensor frame
orientation relative to a fixed Earth frame providing absolute orientation information for subsequent use.
* An example of how to use the quaternion data to generate standard aircraft orientation data in the form of
Tait-Bryan angles representing the sensor yaw, pitch, and roll angles suitable for any vehicle stablization control application.

Hardware setup: This library supports communicating with the
LSM9DS0 over either I2C or SPI. If you're using I2C, these are
the only connections that need to be made:
LSM9DS0 --------- Arduino
SCL ---------- SCL (A5 on older 'Duinos')
SDA ---------- SDA (A4 on older 'Duinos')
VDD ------------- 3.3V
GND ------------- GND
DRDYG-------------4   (gyro data ready interrupt, can be any digital pin)
INTX1-------------3   (accelerometer data ready interrupt, can be any digital pin)
INTX2-------------2   (magnetometer data ready interrupt, can be any digital pin)
(CSG, CSXM, SDOG, and SDOXM should all be pulled high jumpers on
the breakout board will do this for you.)

If you're using SPI, here is an example hardware setup:
LSM9DS0 --------- Arduino
CSG -------------- 9
CSXM ------------- 10
SDOG ------------- 12
SDOXM ------------ 12 (tied to SDOG)
SCL -------------- 13
SDA -------------- 11
VDD -------------- 3.3V
GND -------------- GND

The LSM9DS0 has a maximum voltage of 3.6V. Make sure you power it
off the 3.3V rail! And either use level shifters between SCL
and SDA or just use a 3.3V Arduino Pro.

In addition, this sketch uses a Nokia 5110 48 x 84 pixel display which requires
digital pins 5 - 9 described below. If using SPI you might need to press one of the A0 - A3 pins
into service as a digital input instead.

Development environment specifics:
IDE: Arduino 1.0.5
Hardware Platform: Arduino Pro 3.3V/8MHz
LSM9DS0 Breakout Version: 1.0

This code is beerware. If you see me (or any other SparkFun

Distributed as-is; no warranty is given.
*****************************************************************/

// The SFE_LSM9DS0 requires both the SPI and Wire libraries.
// Unfortunately, you'll need to include both in the Arduino
// sketch, before including the SFE_LSM9DS0 library.
#include <SPI.h> // Included for SFE_LSM9DS0 library
#include <Wire.h>
#include <SFE_LSM9DS0.h>
//#include "Arduino.h"

// Using NOKIA 5110 monochrome 84 x 48 pixel display
// pin 9 - Serial clock out (SCLK)
// pin 8 - Serial data out (DIN)
// pin 7 - Data/Command select (D/C)
// pin 5 - LCD chip select (CS)
// pin 6 - LCD reset (RST)

///////////////////////
// Example I2C Setup //
///////////////////////
// Comment out this section if you're using SPI
// SDO_XM and SDO_G are both grounded, so our addresses are:
#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
// Create an instance of the LSM9DS0 library called `dof` the
// parameters for this constructor are:
LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);

///////////////////////
// Example SPI Setup //
///////////////////////
/* // Uncomment this section if you're using SPI
#define LSM9DS0_CSG  9  // CSG connected to Arduino pin 9
#define LSM9DS0_CSXM 10 // CSXM connected to Arduino pin 10
LSM9DS0 dof(MODE_SPI, LSM9DS0_CSG, LSM9DS0_CSXM);
*/

///////////////////////////////
// Interrupt Pin Definitions //
///////////////////////////////
const byte INT1XM = 2; // INT1XM tells us when accel data is ready
const byte INT2XM = 3; // INT2XM tells us when mag data is ready
const byte DRDYG  = 4; // DRDYG  tells us when gyro data is ready

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
#define GyroMeasError PI * (40.0f / 180.0f)       // gyroscope measurement error in rads/s (shown as 3 deg/s)
#define GyroMeasDrift PI * (0.0f / 180.0f)      // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)
// There is a tradeoff in the beta parameter between accuracy and response speed.
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
#define beta sqrtf(3.0f / 4.0f) * GyroMeasError   // compute beta
#define zeta sqrtf(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

uint32_t count = 0;  // used to control display output rate
uint32_t delt_t = 0; // used to control display output rate
float deltat = 0.0f;        // integration interval for both filter schemes
uint32_t lastUpdate = 0;    // used to calculate integration interval
uint32_t now = 0;           // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
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

void setup()
{
Serial.begin(38400); // Start serial at 38400 bps

// Set up interrupt pins as inputs:
pinMode(INT1XM, INPUT);
pinMode(INT2XM, INPUT);
pinMode(DRDYG,  INPUT);

display.begin(); // Initialize the display
display.setContrast(58); // Set the contrast
display.setRotation(0); //  0 or 2) width = width, 1 or 3) width = height, swapped etc.

// Start device display with ID of sensor
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0,0);  display.print("LSM9DS0");
display.setTextSize(1);
display.setCursor(0, 20); display.print("9 DOF sensor");
display.setCursor(5, 30); display.print("data fusion");
display.setCursor(20, 40); display.print("AHRS");
display.display();
delay(2000);

// Set up for data display
display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
display.clearDisplay();   // clears the screen and buffer
display.display();

// begin() returns a 16-bit value which includes both the gyro
// and accelerometers WHO_AM_I response. You can check this to
// make sure communication was successful.
uint16_t status = dof.begin();

Serial.print("LSM9DS0 WHO_AM_I's returned: 0x");
Serial.println(status, HEX);
Serial.println("Should be 0x49D4");
Serial.println();
display.setCursor(0,0); display.print("I AM");
display.setCursor(0,10); display.print(status, HEX);
display.setCursor(0,30); display.print("I Should Be");
display.setCursor(0,40); display.print(0x49D4, HEX);
display.display();
delay(2000);

// Set data output ranges; choose lowest ranges for maximum resolution
// Accelerometer scale can be: A_SCALE_2G, A_SCALE_4G, A_SCALE_6G, A_SCALE_8G, or A_SCALE_16G
dof.setAccelScale(dof.A_SCALE_2G);
// Gyro scale can be:  G_SCALE__245, G_SCALE__500, or G_SCALE__2000DPS
dof.setGyroScale(dof.G_SCALE_245DPS);
// Magnetometer scale can be: M_SCALE_2GS, M_SCALE_4GS, M_SCALE_8GS, M_SCALE_12GS
dof.setMagScale(dof.M_SCALE_2GS);

// Set output data rates
// Accelerometer output data rate (ODR) can be: A_ODR_3125 (3.225 Hz), A_ODR_625 (6.25 Hz), A_ODR_125 (12.5 Hz), A_ODR_25, A_ODR_50,
//                                              A_ODR_100,  A_ODR_200, A_ODR_400, A_ODR_800, A_ODR_1600 (1600 Hz)
dof.setAccelODR(dof.A_ODR_100); // Set accelerometer update rate at 100 Hz
// Accelerometer anti-aliasing filter rate can be 50, 194, 362, or 763 Hz
// Anti-aliasing acts like a low-pass filter allowing oversampling of accelerometer and rejection of high-frequency spurious noise.
// Strategy here is to effectively oversample accelerometer at 100 Hz and use a 50 Hz anti-aliasing (low-pass) filter frequency
// to get a smooth ~150 Hz filter update rate
dof.setAccelABW(dof.A_ABW_50); // Choose lowest filter setting for low noise

// Gyro output data rates can be: 95 Hz (bandwidth 12.5 or 25 Hz), 190 Hz (bandwidth 12.5, 25, 50, or 70 Hz)
//                                 380 Hz (bandwidth 20, 25, 50, 100 Hz), or 760 Hz (bandwidth 30, 35, 50, 100 Hz)
dof.setGyroODR(dof.G_ODR_190_BW_125);  // Set gyro update rate to 190 Hz with the smallest bandwidth for low noise

// Magnetometer output data rate can be: 3.125 (ODR_3125), 6.25 (ODR_625), 12.5 (ODR_125), 25, 50, or 100 Hz
dof.setMagODR(dof.M_ODR_125); // Set magnetometer to update every 80 ms
}

void loop()
{
gx = dof.calcGyro(dof.gx);   // Convert to degrees per seconds
gy = dof.calcGyro(dof.gy);
gz = dof.calcGyro(dof.gz);
}

ax = dof.calcAccel(dof.ax);   // Convert to g's
ay = dof.calcAccel(dof.ay);
az = dof.calcAccel(dof.az);
}

mx = dof.calcMag(dof.mx);     // Convert to Gauss
my = dof.calcMag(dof.my);
mz = dof.calcMag(dof.mz);
}

now = micros();
deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = now;
// Sensors x- and y-axes are aligned but magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!
// This is ok by aircraft orientation standards!
// Pass gyro rate as rad/s
MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
//MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);

// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = millis() - count;
if (delt_t > 500) { // update LCD once per half-second independent of read rate

// Print the heading and orientation for fun!
printOrientation(ax, ay, az);

// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination),
// looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, to get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
yaw   = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll  = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw   *= 180.0f / PI - 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
roll  *= 180.0f / PI;

Serial.print("ax = "); Serial.print((int)1000*ax);
Serial.print(" ay = "); Serial.print((int)1000*ay);
Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
Serial.print("gx = "); Serial.print( gx, 2);
Serial.print(" gy = "); Serial.print( gy, 2);
Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
Serial.print("mx = "); Serial.print( (int)1000*mx, 2);
Serial.print(" my = "); Serial.print( (int)1000*my, 2);
Serial.print(" mz = "); Serial.print( (int)1000*mz, 2); Serial.println(" mG");

Serial.print("Yaw, Pitch, Roll: ");
Serial.print(yaw, 2);
Serial.print(", ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);

Serial.print("q0 = "); Serial.print(q[0]);
Serial.print(" qx = "); Serial.print(q[1]);
Serial.print(" qy = "); Serial.print(q[2]);
Serial.print(" qz = "); Serial.println(q[3]);

Serial.print("deltat = "); Serial.println(deltat, 4);

display.clearDisplay();

display.setCursor(0, 0); display.print(" x   y   z  ");

display.setCursor(0,  8); display.print((int)(1000*ax));
display.setCursor(24, 8); display.print((int)(1000*ay));
display.setCursor(48, 8); display.print((int)(1000*az));
display.setCursor(72, 8); display.print("mg");

display.setCursor(0,  16); display.print((int)(gx));
display.setCursor(24, 16); display.print((int)(gy));
display.setCursor(48, 16); display.print((int)(gz));
display.setCursor(66, 16); display.print("o/s");

display.setCursor(0,  24); display.print((int)(1000*mx));
display.setCursor(24, 24); display.print((int)(1000*my));
display.setCursor(48, 24); display.print((int)(1000*mz));
display.setCursor(72, 24); display.print("mG");

display.setCursor(0,  32); display.print((int)(yaw));
display.setCursor(24, 32); display.print((int)(pitch));
display.setCursor(48, 32); display.print((int)(roll));
display.setCursor(66, 32); display.print("ypr");

// With ODR settings of 400 Hz, 380 Hz, and 25 Hz for the accelerometer, gyro, and magnetometer, respectively,
// the filter is updating at a ~125 Hz rate using the Madgwick scheme and ~165 Hz using the Mahony scheme
// even though the display refreshes at only 2 Hz.
// The filter update rate can be increased by reducing the rate of data reading. The optimal implementation is
// one which balances the competing rates so they are about the same; that is, the filter updates the sensor orientation
// at about the same rate the data is changing. Of course, other implementations are possible. One might consider
// updating the filter at twice the average new data rate to allow for finite filter convergence times.
// The filter update rate is determined mostly by the mathematical steps in the respective algorithms,
// the processor speed (8 MHz for the 3.3V Pro Mini), and the sensor ODRs, especially the magnetometer ODR:
// smaller ODRs for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
// filter update rates of ~110 and ~135 Hz for the Madgwick and Mahony schemes, respectively.
// This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
// With low ODR settings of 100 Hz, 95 Hz, and 6.25 Hz for the accelerometer, gyro, and magnetometer, respectively,
// the filter is updating at a ~150 Hz rate using the Madgwick scheme and ~200 Hz using the Mahony scheme.
// These filter update rates should be fast enough to maintain accurate platform orientation for
// stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
// produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
// The 3.3 V 8 MHz Pro Mini is doing pretty well!
display.setCursor(0, 40); display.print("rt: "); display.print((1/deltat)); display.print(" Hz");

display.display();
count = millis();
}
}

// magnetic field.
// It only works if the sensor is flat (z-axis normal to Earth).
// See: http://www.ngdc.noaa.gov/geomag/declination.shtml
{
if (hy > 0)
{
heading = 90 - (atanf(hx / hy) * (180 / PI));
}
else if (hy < 0)
{
heading = - (atanf(hx / hy) * (180 / PI));
}
else // hy = 0
{
if (hx < 0) heading = 180;
}

}

// Another fun function that does calculations based on the
// acclerometer data. This function will print your LSM9DS0's
// orientation -- it's roll and pitch angles.
void printOrientation(float x, float y, float z)
{
// float pitch, roll;

pitch = atan2f(x, sqrtf(y * y) + (z * z));
roll = atan2f(y, sqrtf(x * x) + (z * z));
pitch *= 180.0 / PI;
roll *= 180.0 / PI;

Serial.print("Pitch, Roll: ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);
}

// 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 = sqrtf(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 = sqrtf(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 = sqrtf(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 = sqrtf(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 = sqrtf(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 = sqrtf(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 = sqrtf(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 = sqrtf((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 = sqrtf(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;

}```

7. I ordered one of these from Sparkfun...

8. Thank you for looking into this Paul!

_Guillermo

9. NorDevX also has their own breakout board version which is \$28, about the same price, but it comes in an 2 x 8 pin arrangement instead of the 1 x 13 arrangement of the sparkfun board. Either one works well. It is a very easy to use device.

I replaced all sqrt and tan2 functions with sqrtf and tan2f, and I still have a very slow and non-responsive quaternion update.

Guillermo, I noticed that now your pitch and roll are about the same as the simpler ones. Does the quaternion respond when you change the device orientation. You should see nearly instantaneous tracking of the sensor motion by the yaw, pitch, and roll output from the sensor fusion algorithms. For example, when you rotate the sensor while laying flat on a table about the z-axis, you should see the yaw change right away. I don't.

Michael, are you suggesting that I change all the floats in the quaternion algorithm to doubles? Otherwise, there aren't really any uint16_t types in the quaternion algorithm proper.

I did notice that when I changed to sqrtf and tan2f, the sensor fusion update rate went from ~300 Hz to ~75 Hz. Not quite what I expected. Nevermind, I forgot about the delay I had added, the sensor fusion update runs at ~600 - 2000 Hz when the Teensy is set to 48 MHz. The instantaneous rate difference depends on whether the magnetometer is read (12.5 Hz read rate).

10. Originally Posted by onehorse
Michael, are you suggesting that I change all the floats in the quaternion algorithm to doubles? Otherwise, there aren't really any uint16_t types in the quaternion algorithm proper.
I made 2 suggestions. 1) Use the float version of the math functions instead of the double versions. You did this and it didn't seem to help, and seems to have slowed things down. And 2) change the int16_t's to int32_t's. This is because on a 32-bit processor doing stuff in 16-bit means you need to do the appropriate sign/zero-extend between operations. I see this in supporting 64-bit PowerPC where in a few benchmarks using int, the sign extension becomes a problem. But in most programs, this is not a significant issue.

It may make an interesting experiment to try changing all floats to double. Perhaps the Arm single precision libraries have some slow downs that aren't present in the double libraries.

The other thing to try is put Serial.println's in the middle of the loop, and see if the Teensy is executing a loop more times than the Arduino.

11. Onehorse- Yes I see a rapid change on the Yaw, Pitch, Roll it take as a couple seconds to get stable/catch up though. I'm running Teensy at 96Mhz, should I slow it down?

12. This sounds like progress. What else did you do besides change to sqrtf and tan2f? Did you change the float to double in the Madgwick and Mahony algorithms, or did you just change the int16_t to int32_t as Michael suggested?

13. I did change the int16_t to int32_t as Michael suggested that is what I've notice to make more of a difference.

14. I am flabbergasted! I changed the few uint16_t variables to uint32_t and nothing else and now IT WORKS!

I really do not understand this.

15. are you getting the same values as what you would get with the arduino pro 3.3 8mhz?

16. Yes. I just checked by using the exact same sketch with the 3.3 V 8 MHz Pro Mini and it is working as I expect and produces the exact same values. I call this problem solved, although I really do not understand why the change to uint32_t for a few non-central variables did the trick. The sensor fusion update rate for the Pro Mini is still at 125 Hz for the Madgwick scheme and between 700 and 3000 Hz for the same scheme using the Teensy 3.1 overclocked at 96 MHz. I suppose the average of 700 and 3000 is 1800, about the twelve times faster that I would have expected. Of course, this is overkill since I have the accelerometer and gyro data sample rates set to 200 Hz, and the magnetometer sample update rate at 100 Hz. Dropping th Teensy down to 24 or 48 Hz is a better match to these ODRs. Thanks for all the help everyone!

17. Great! This is a great news, now I can move on and figure out how to implement the rest of my project

18. Wait, what about the negative value on Yaw?
Pitch, Roll: 1.40, 10.29
ax = 29.11 ay = 187.26 az = 1001.04 mg
gx = 3.66 gy = -4.54 gz = -12.51 deg/s
mx = 217.22 my = 191.16 mz = 93.93 mG
Yaw, Pitch, Roll: -30.78, -1.82, 10.51
q0 = -0.93 qx = -0.08 qy = 0.05 qz = 0.34
deltat = 0.0013

19. Originally Posted by onehorse
I am flabbergasted! I changed the few uint16_t variables to uint32_t and nothing else and now IT WORKS!

I really do not understand this.
Now, as I said, I was expecting it to improve just a few %. However, in looking at the code, while the important calculations are all in floating point, the 16-bit variables are used to control how often things are being printed. I suspect the Teensy is just running rings around the Arduino, and it is sampling too fast (or spending too much time printing, as I didn't look at the code in great detail).

I suspect this code is what is responsible for the problem.

Code:
```    // Serial print and/or display at 0.5 s rate independent of data rates
delt_t = millis() - count;
if (delt_t > 500) { // update LCD once per half-second independent of read rate```

20. First of all, your gyro values are way off for a sensor at rest. You might need to calibrate (offset) these values to be sure you have zeroes when the device is at rest. I thought the LSM9DS0 did this automatically. My two show zero or maybe 1 dgree per second at rest without calibration. Verify that your gyros behave rationally, or recalibrate with the appropriate offset.

The same is true of the accelerometer values, although these seem fine. You should verify that you read + 1000 mG for each axis in the appropriate orientation.

And you have to calibrate any required offset for the magnetometer. For example, the z-axis magnetometer should read closer to 400 mG most places on Earth. Look up the expected values of the magnetic field wherever you are for a reference. But you need to calibrate the magnetic sensor yourself. Just slowly rotate the sensor about each of the three axes, note the minimum and maximum values of the field on each axis, and then apply an offset to center the average for each axis to zero. You can do more elaborate calibrations but this is usually enough. Lastly, you need to put in your specific magnetic declination to set the absolute reference frame to true North..

Now to your question. The Yaw, Pitch, and Roll are Euler angle representations of the orientation of the sensor at any given moment. The roll and pitch are relative to the horizontal axes, so they will exhibit + and - values. The yaw is relative to a fixed vertical plane; that which is coincident with the magnetic (or true, if you take account of the magnetic declination) North vector. Therefore, if the sensor x-axis is on one side of this vertical plane, it will read positive yaw, and on the other it will read negative yaw. The Euler representation has some problems for large angle variations since the tan2 and sin functions on which the angles are based have singularities. The yaw (and maybe roll) can go from -180 to +180, or somehing like that. The value is ambiguous at +/- 180. The quaternion representation has none of these problems and is absolute and unambiguous. The quaternion of a properly calibrated sensor is 1,0,0,0 when the sensor x-axis is pointing North and the x and y axes are parallel to the ground. For your project, yaw, roll, and pitch might not be appropriate if the motions you want to track are very large in angle. In this case, you have to figure out the best transformation of the underlying good quaternion data to a more physically meaningful set of variables, keeping in mind some of the pitfalls you might encounter with singularities and ambiguities, etc.

21. I don't quite see how the evaluation and testing against a time marker could affect only the quaternion update rate, since the sensor data was being properly updated. In any case, I will run a few experiments testing your hypothesis tomorrow and report on my findings...

22. Originally Posted by onehorse
I call this problem solved, although I really do not understand why the change to uint32_t for a few non-central variables did the trick.
Could you post the fixed code?

At some point in the future (after I get this sensor), I'll investigate further. I'd like to get to the bottom of what really happened, and if there's a clean fix, submit a pull request to Sparkfun so it'll be fixed for everyone else in the future.

23. Yes, I will post the code tonight when I get home. Note I simply replaced the few uint16_t with uint32_t. Otherwise it is the same as that above in the first message.

24. ## LSM9DS0 sketch that works for Arduino and Teensy 3.1

Code:
```/*****************************************************************
LSM9DS0_AHRS.ino
SFE_LSM9DS0 Library AHRS Data Fusion Example Code
Jim Lindblom @ SparkFun Electronics
Original Creation Date: February 18, 2014
https://github.com/sparkfun/LSM9DS0_Breakout

Modified by Kris Winer, April 4, 2014

The LSM9DS0 is a versatile 9DOF sensor. It has a built-in
accelerometer, gyroscope, and magnetometer. Very cool! Plus it
functions over either SPI or I2C.

This Arduino sketch utilizes Jim Lindblom's SFE_LSM9DS0 library to generate the basic sensor data
for use in two sensor fusion algorithms becoming increasingly popular with DIY quadcopter and robotics engineers.

Like the original LSM9SD0_simple.ino sketch, it'll demo the following:
* How to create a LSM9DS0 object, using a constructor (global
variables section).
* How to use the begin() function of the LSM9DS0 class.
* How to read the gyroscope, accelerometer, and magnetometer
gx, gy, gz, ax, ay, az, mx, my, and mz variables.
* How to calculate actual acceleration, rotation speed, magnetic
field strength using the calcAccel(), calcGyro() and calcMag()
functions.

In addition, the sketch will demo:
* How to check for data updates using interrupts
* How to display output at a rate different from the sensor data update and fusion filter update rates
* How to specify the accelerometer anti-aliasing (low-pass) filter rate
* How to use the data from the LSM9DS0 to fuse the sensor data into a quaternion representation of the sensor frame
orientation relative to a fixed Earth frame providing absolute orientation information for subsequent use.
* An example of how to use the quaternion data to generate standard aircraft orientation data in the form of
Tait-Bryan angles representing the sensor yaw, pitch, and roll angles suitable for any vehicle stablization control application.

Hardware setup: This library supports communicating with the
LSM9DS0 over either I2C or SPI. If you're using I2C, these are
the only connections that need to be made:
LSM9DS0 --------- Arduino
SCL ---------- SCL (A5 on older 'Duinos')
SDA ---------- SDA (A4 on older 'Duinos')
VDD ------------- 3.3V
GND ------------- GND
DRDYG-------------4   (gyro data ready interrupt, can be any digital pin)
INTX1-------------3   (accelerometer data ready interrupt, can be any digital pin)
INTX2-------------2   (magnetometer data ready interrupt, can be any digital pin)
(CSG, CSXM, SDOG, and SDOXM should all be pulled high jumpers on
the breakout board will do this for you.)

If you're using SPI, here is an example hardware setup:
LSM9DS0 --------- Arduino
CSG -------------- 9
CSXM ------------- 10
SDOG ------------- 12
SDOXM ------------ 12 (tied to SDOG)
SCL -------------- 13
SDA -------------- 11
VDD -------------- 3.3V
GND -------------- GND

The LSM9DS0 has a maximum voltage of 3.6V. Make sure you power it
off the 3.3V rail! And either use level shifters between SCL
and SDA or just use a 3.3V Arduino Pro.

In addition, this sketch uses a Nokia 5110 48 x 84 pixel display which requires
digital pins 5 - 9 described below. If using SPI you might need to press one of the A0 - A3 pins
into service as a digital input instead.

Development environment specifics:
IDE: Arduino 1.0.5
Hardware Platform: Arduino Pro 3.3V/8MHz
LSM9DS0 Breakout Version: 1.0

This code is beerware. If you see me (or any other SparkFun

Distributed as-is; no warranty is given.
*****************************************************************/

// The SFE_LSM9DS0 requires both the SPI and Wire libraries.
// Unfortunately, you'll need to include both in the Arduino
// sketch, before including the SFE_LSM9DS0 library.
#include <SPI.h> // Included for SFE_LSM9DS0 library
#include <Wire.h>
#include <SFE_LSM9DS0.h>
//#include "Arduino.h"

// Using NOKIA 5110 monochrome 84 x 48 pixel display
// pin 9 - Serial clock out (SCLK)
// pin 8 - Serial data out (DIN)
// pin 7 - Data/Command select (D/C)
// pin 5 - LCD chip select (CS)
// pin 6 - LCD reset (RST)

///////////////////////
// Example I2C Setup //
///////////////////////
// Comment out this section if you're using SPI
// SDO_XM and SDO_G are both grounded, so our addresses are:
#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
// Create an instance of the LSM9DS0 library called `dof` the
// parameters for this constructor are:
LSM9DS0 dof(MODE_I2C, LSM9DS0_G, LSM9DS0_XM);

///////////////////////
// Example SPI Setup //
///////////////////////
/* // Uncomment this section if you're using SPI
#define LSM9DS0_CSG  9  // CSG connected to Arduino pin 9
#define LSM9DS0_CSXM 10 // CSXM connected to Arduino pin 10
LSM9DS0 dof(MODE_SPI, LSM9DS0_CSG, LSM9DS0_CSXM);
*/

///////////////////////////////
// Interrupt Pin Definitions //
///////////////////////////////
const byte INT1XM = 3; // INT1XM tells us when accel data is ready
const byte INT2XM = 2; // INT2XM tells us when mag data is ready
const byte DRDYG  = 4; // DRDYG  tells us when gyro data is ready

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
#define GyroMeasError PI * (40.0f / 180.0f)       // gyroscope measurement error in rads/s (shown as 3 deg/s)
#define GyroMeasDrift PI * (0.0f / 180.0f)      // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s)
// There is a tradeoff in the beta parameter between accuracy and response speed.
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
#define beta sqrt(3.0f / 4.0f) * GyroMeasError   // compute beta
#define 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

uint32_t count = 0;  // used to control display output rate
uint32_t delt_t = 0; // used to control display output rate
float deltat = 0.0f;        // integration interval for both filter schemes
uint32_t lastUpdate = 0;    // used to calculate integration interval
uint32_t now = 0;           // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
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

void setup()
{
Serial.begin(38400); // Start serial at 38400 bps

// Set up interrupt pins as inputs:
pinMode(INT1XM, INPUT);
pinMode(INT2XM, INPUT);
pinMode(DRDYG,  INPUT);

display.begin(); // Initialize the display
display.setContrast(58); // Set the contrast
display.setRotation(0); //  0 or 2) width = width, 1 or 3) width = height, swapped etc.

// Start device display with ID of sensor
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0,0);  display.print("LSM9DS0");
display.setTextSize(1);
display.setCursor(0, 20); display.print("9 DOF sensor");
display.setCursor(5, 30); display.print("data fusion");
display.setCursor(20, 40); display.print("AHRS");
display.display();
delay(2000);

// Set up for data display
display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
display.clearDisplay();   // clears the screen and buffer
display.display();

// begin() returns a 16-bit value which includes both the gyro
// and accelerometers WHO_AM_I response. You can check this to
// make sure communication was successful.
uint32_t status = dof.begin();

Serial.print("LSM9DS0 WHO_AM_I's returned: 0x");
Serial.println(status, HEX);
Serial.println("Should be 0x49D4");
Serial.println();
display.setCursor(0,0); display.print("I AM");
display.setCursor(0,10); display.print(status, HEX);
display.setCursor(0,30); display.print("I Should Be");
display.setCursor(0,40); display.print(0x49D4, HEX);
display.display();
delay(2000);

// Set data output ranges; choose lowest ranges for maximum resolution
// Accelerometer scale can be: A_SCALE_2G, A_SCALE_4G, A_SCALE_6G, A_SCALE_8G, or A_SCALE_16G
dof.setAccelScale(dof.A_SCALE_2G);
// Gyro scale can be:  G_SCALE__245, G_SCALE__500, or G_SCALE__2000DPS
dof.setGyroScale(dof.G_SCALE_245DPS);
// Magnetometer scale can be: M_SCALE_2GS, M_SCALE_4GS, M_SCALE_8GS, M_SCALE_12GS
dof.setMagScale(dof.M_SCALE_2GS);

// Set output data rates
// Accelerometer output data rate (ODR) can be: A_ODR_3125 (3.225 Hz), A_ODR_625 (6.25 Hz), A_ODR_125 (12.5 Hz), A_ODR_25, A_ODR_50,
//                                              A_ODR_100,  A_ODR_200, A_ODR_400, A_ODR_800, A_ODR_1600 (1600 Hz)
dof.setAccelODR(dof.A_ODR_200); // Set accelerometer update rate at 100 Hz
// Accelerometer anti-aliasing filter rate can be 50, 194, 362, or 763 Hz
// Anti-aliasing acts like a low-pass filter allowing oversampling of accelerometer and rejection of high-frequency spurious noise.
// Strategy here is to effectively oversample accelerometer at 100 Hz and use a 50 Hz anti-aliasing (low-pass) filter frequency
// to get a smooth ~150 Hz filter update rate
dof.setAccelABW(dof.A_ABW_50); // Choose lowest filter setting for low noise

// Gyro output data rates can be: 95 Hz (bandwidth 12.5 or 25 Hz), 190 Hz (bandwidth 12.5, 25, 50, or 70 Hz)
//                                 380 Hz (bandwidth 20, 25, 50, 100 Hz), or 760 Hz (bandwidth 30, 35, 50, 100 Hz)
dof.setGyroODR(dof.G_ODR_190_BW_25);  // Set gyro update rate to 190 Hz with the smallest bandwidth for low noise

// Magnetometer output data rate can be: 3.125 (ODR_3125), 6.25 (ODR_625), 12.5 (ODR_125), 25, 50, or 100 Hz
dof.setMagODR(dof.M_ODR_100); // Set magnetometer to update every 80 ms
}

void loop()
{
gx = dof.calcGyro(dof.gx);   // Convert to degrees per seconds
gy = dof.calcGyro(dof.gy);
gz = dof.calcGyro(dof.gz);
}

ax = dof.calcAccel(dof.ax);   // Convert to g's
ay = dof.calcAccel(dof.ay);
az = dof.calcAccel(dof.az);
}

mx = dof.calcMag(dof.mx);     // Convert to Gauss and correct for calibration
my = dof.calcMag(dof.my);
mz = dof.calcMag(dof.mz);
}

now = micros();
deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = now;
// Sensors x- and y-axes are aligned but magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!
// This is ok by aircraft orientation standards!
// Pass gyro rate as rad/s
MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);

// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = millis() - count;
if (delt_t > 500) { // update LCD once per half-second independent of read rate

// Print the heading and orientation for fun!
printOrientation(ax, ay, az);

// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination),
// looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, to get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw   *= 180.0f / PI - 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
roll  *= 180.0f / PI;

Serial.print("ax = "); Serial.print((int)1000*ax);
Serial.print(" ay = "); Serial.print((int)1000*ay);
Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
Serial.print("gx = "); Serial.print( gx, 2);
Serial.print(" gy = "); Serial.print( gy, 2);
Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
Serial.print("mx = "); Serial.print( (int)1000*mx);
Serial.print(" my = "); Serial.print( (int)1000*my);
Serial.print(" mz = "); Serial.print( (int)1000*mz); Serial.println(" mG");

Serial.print("Yaw, Pitch, Roll: ");
Serial.print(yaw, 2);
Serial.print(", ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);

Serial.print("q0 = "); Serial.print(q[0]);
Serial.print(" qx = "); Serial.print(q[1]);
Serial.print(" qy = "); Serial.print(q[2]);
Serial.print(" qz = "); Serial.println(q[3]);

Serial.print("deltat = "); Serial.println(deltat, 4);
display.clearDisplay();

display.setCursor(0, 0); display.print(" x   y   z  ");

display.setCursor(0,  8); display.print((int)(1000*ax));
display.setCursor(24, 8); display.print((int)(1000*ay));
display.setCursor(48, 8); display.print((int)(1000*az));
display.setCursor(72, 8); display.print("mg");

display.setCursor(0,  16); display.print((int)(gx));
display.setCursor(24, 16); display.print((int)(gy));
display.setCursor(48, 16); display.print((int)(gz));
display.setCursor(66, 16); display.print("o/s");

display.setCursor(0,  24); display.print((int)(1000*mx));
display.setCursor(24, 24); display.print((int)(1000*my));
display.setCursor(48, 24); display.print((int)(1000*mz));
display.setCursor(72, 24); display.print("mG");

display.setCursor(0,  32); display.print((int)(yaw));
display.setCursor(24, 32); display.print((int)(pitch));
display.setCursor(48, 32); display.print((int)(roll));
display.setCursor(66, 32); display.print("ypr");

// With ODR settings of 400 Hz, 380 Hz, and 25 Hz for the accelerometer, gyro, and magnetometer, respectively,
// the filter is updating at a ~125 Hz rate using the Madgwick scheme and ~165 Hz using the Mahony scheme
// even though the display refreshes at only 2 Hz.
// The filter update rate can be increased by reducing the rate of data reading. The optimal implementation is
// one which balances the competing rates so they are about the same; that is, the filter updates the sensor orientation
// at about the same rate the data is changing. Of course, other implementations are possible. One might consider
// updating the filter at twice the average new data rate to allow for finite filter convergence times.
// The filter update rate is determined mostly by the mathematical steps in the respective algorithms,
// the processor speed (8 MHz for the 3.3V Pro Mini), and the sensor ODRs, especially the magnetometer ODR:
// smaller ODRs for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
// filter update rates of ~110 and ~135 Hz for the Madgwick and Mahony schemes, respectively.
// This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
// With low ODR settings of 100 Hz, 95 Hz, and 6.25 Hz for the accelerometer, gyro, and magnetometer, respectively,
// the filter is updating at a ~150 Hz rate using the Madgwick scheme and ~200 Hz using the Mahony scheme.
// These filter update rates should be fast enough to maintain accurate platform orientation for
// stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
// produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
// The 3.3 V 8 MHz Pro Mini is doing pretty well!
display.setCursor(0, 40); display.print("rt: "); display.print((1/deltat)); display.print(" Hz");

display.display();
count = millis();
}
}

// magnetic field.
// It only works if the sensor is flat (z-axis normal to Earth).
// See: http://www.ngdc.noaa.gov/geomag/declination.shtml
{
if (hy > 0)
{
heading = 90 - (atan(hx / hy) * (180 / PI));
}
else if (hy < 0)
{
heading = - (atan(hx / hy) * (180 / PI));
}
else // hy = 0
{
if (hx < 0) heading = 180;
}

}

// Another fun function that does calculations based on the
// acclerometer data. This function will print your LSM9DS0's
// orientation -- it's roll and pitch angles.
void printOrientation(float x, float y, float z)
{
// float pitch, roll;

pitch = atan2(x, sqrt(y * y) + (z * z));
roll = atan2(y, sqrt(x * x) + (z * z));
pitch *= 180.0 / PI;
roll *= 180.0 / PI;

Serial.print("Pitch, Roll: ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);
}

// 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;

}```

#### Posting Permissions

• You may not post new threads
• You may not post replies
• You may not post attachments
• You may not edit your posts
•