Bump version to 2.4.0 in library.properties

Merge pull request #43 from dhalbert/clang-fixes
clang format
2025-08-26 15:16:15 +01:00 · 2025-08-24 18:45:47 -04:00 · 2025-08-24 18:10:35 -04:00 · 2025-08-24 17:40:46 -04:00 · 2025-08-19 15:13:26 -04:00 · 2025-03-26 11:09:18 -04:00
46 changed files with 3950 additions and 1443 deletions
--- a/.github/workflows/githubci.yml
+++ b/.github/workflows/githubci.yml
@ -0,0 +1,32 @@
+name: Arduino Library CI
+
+on: [pull_request, push, repository_dispatch]
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    
+    steps:
+    - uses: actions/setup-python@v4
+      with:
+        python-version: '3.x'
+    - uses: actions/checkout@v3
+    - uses: actions/checkout@v3
+      with:
+         repository: adafruit/ci-arduino
+         path: ci
+
+    - name: pre-install
+      run: bash ci/actions_install.sh
+
+    - name: test platforms
+      run: python3 ci/build_platform.py main_platforms
+
+    - name: clang
+      run: python3 ci/run-clang-format.py -e "ci/*" -e "bin/*" -r . 
+
+    - name: doxygen
+      env:
+        GH_REPO_TOKEN: ${{ secrets.GH_REPO_TOKEN }}
+        PRETTYNAME : "Adafruit AHRS Library"
+      run: bash ci/doxy_gen_and_deploy.sh
--- a/.gitignore
+++ b/.gitignore
@ -1 +1,3 @@
 serialconfig.txt
+.pio
+html
--- a/Adafruit_Simple_AHRS.cpp
+++ b/Adafruit_Simple_AHRS.cpp
@ -1,72 +0,0 @@
-#include "Adafruit_Simple_AHRS.h"
-
-// Create a simple AHRS from an explicit accelerometer and magnetometer sensor.
-Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafruit_Sensor* magnetometer):
-  _accel(accelerometer),
-  _mag(magnetometer)
-{}
-
-// Create a simple AHRS from a device with multiple sensors.
-Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor_Set& sensors):
-  _accel(sensors.getSensor(SENSOR_TYPE_ACCELEROMETER)),
-  _mag(sensors.getSensor(SENSOR_TYPE_MAGNETIC_FIELD))
-{}
-
-// Compute orientation based on accelerometer and magnetometer data.
-bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t* orientation) {
-  // Validate input and available sensors.
-  if (orientation == NULL || _accel == NULL || _mag == NULL) return false;
-
-  // Grab an acceleromter and magnetometer reading.
-  sensors_event_t accel_event;
-  _accel->getEvent(&accel_event);
-  sensors_event_t mag_event;
-  _mag->getEvent(&mag_event);
-
-  float const PI_F = 3.14159265F;
-
-  // roll: Rotation around the X-axis. -180 <= roll <= 180                                          
-  // a positive roll angle is defined to be a clockwise rotation about the positive X-axis          
-  //                                                                                                
-  //                    y                                                                           
-  //      roll = atan2(---)                                                                         
-  //                    z                                                                           
-  //                                                                                                
-  // where:  y, z are returned value from accelerometer sensor                                      
-  orientation->roll = (float)atan2(accel_event.acceleration.y, accel_event.acceleration.z);
-
-  // pitch: Rotation around the Y-axis. -180 <= roll <= 180                                         
-  // a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis         
-  //                                                                                                
-  //                                 -x                                                             
-  //      pitch = atan(-------------------------------)                                             
-  //                    y * sin(roll) + z * cos(roll)                                               
-  //                                                                                                
-  // where:  x, y, z are returned value from accelerometer sensor                                   
-  if (accel_event.acceleration.y * sin(orientation->roll) + accel_event.acceleration.z * cos(orientation->roll) == 0)
-    orientation->pitch = accel_event.acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
-  else
-    orientation->pitch = (float)atan(-accel_event.acceleration.x / (accel_event.acceleration.y * sin(orientation->roll) + \
-                                                                     accel_event.acceleration.z * cos(orientation->roll)));
-
-  // heading: Rotation around the Z-axis. -180 <= roll <= 180                                       
-  // a positive heading angle is defined to be a clockwise rotation about the positive Z-axis       
-  //                                                                                                
-  //                                       z * sin(roll) - y * cos(roll)                            
-  //   heading = atan2(--------------------------------------------------------------------------)  
-  //                    x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll))   
-  //                                                                                                
-  // where:  x, y, z are returned value from magnetometer sensor                                    
-  orientation->heading = (float)atan2(mag_event.magnetic.z * sin(orientation->roll) - mag_event.magnetic.y * cos(orientation->roll), \
-                                      mag_event.magnetic.x * cos(orientation->pitch) + \
-                                      mag_event.magnetic.y * sin(orientation->pitch) * sin(orientation->roll) + \
-                                      mag_event.magnetic.z * sin(orientation->pitch) * cos(orientation->roll));
-
-
-  // Convert angular data to degree 
-  orientation->roll = orientation->roll * 180 / PI_F;
-  orientation->pitch = orientation->pitch * 180 / PI_F;
-  orientation->heading = orientation->heading * 180 / PI_F;
-
-  return true;
-}
--- a/Adafruit_Simple_AHRS.h
+++ b/Adafruit_Simple_AHRS.h
@ -1,21 +0,0 @@
-#ifndef __ADAFRUIT_SIMPLE_AHRS_H__
-#define __ADAFRUIT_SIMPLE_AHRS_H__
-
-#include <Adafruit_Sensor.h>
-#include "Adafruit_Sensor_Set.h"
-
-// Simple sensor fusion AHRS using an accelerometer and magnetometer.
-class Adafruit_Simple_AHRS
-{
-public:
-  Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafruit_Sensor* magnetometer);
-  Adafruit_Simple_AHRS(Adafruit_Sensor_Set& sensors);
-  bool getOrientation(sensors_vec_t* orientation);
-
-private:
-  Adafruit_Sensor* _accel;
-  Adafruit_Sensor* _mag;
-
-};
-
-#endif
--- a/Madgwick.cpp
+++ b/Madgwick.cpp
@ -1,250 +0,0 @@
-//=============================================================================================
-// Madgwick.c
-//=============================================================================================
-//
-// Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-//
-// From the x-io website "Open-source resources available on this website are
-// provided under the GNU General Public Licence unless an alternative licence
-// is provided in source."
-//
-// Date			Author          Notes
-// 29/09/2011	SOH Madgwick    Initial release
-// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
-// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
-//
-//=============================================================================================
-
-//-------------------------------------------------------------------------------------------
-// Header files
-
-#include "Madgwick.h"
-#include <math.h>
-
-//-------------------------------------------------------------------------------------------
-// Definitions
-
-#define sampleFreqDef   512.0f          // sample frequency in Hz
-#define betaDef         0.1f            // 2 * proportional gain
-
-
-//============================================================================================
-// Functions
-
-//-------------------------------------------------------------------------------------------
-// AHRS algorithm update
-
-Madgwick::Madgwick() {
-	beta = betaDef;
-	q0 = 1.0f;
-	q1 = 0.0f;
-	q2 = 0.0f;
-	q3 = 0.0f;
-	invSampleFreq = 1.0f / sampleFreqDef;
-	anglesComputed = 0;
-}
-
-void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
-	float recipNorm;
-	float s0, s1, s2, s3;
-	float qDot1, qDot2, qDot3, qDot4;
-	float hx, hy;
-	float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
-
-	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
-	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-		updateIMU(gx, gy, gz, ax, ay, az);
-		return;
-	}
-
-	// Convert gyroscope degrees/sec to radians/sec
-	gx *= 0.0174533f;
-	gy *= 0.0174533f;
-	gz *= 0.0174533f;
-
-	// Rate of change of quaternion from gyroscope
-	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
-	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
-	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
-	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
-
-	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-		// Normalise accelerometer measurement
-		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-		ax *= recipNorm;
-		ay *= recipNorm;
-		az *= recipNorm;
-
-		// Normalise magnetometer measurement
-		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-		mx *= recipNorm;
-		my *= recipNorm;
-		mz *= recipNorm;
-
-		// Auxiliary variables to avoid repeated arithmetic
-		_2q0mx = 2.0f * q0 * mx;
-		_2q0my = 2.0f * q0 * my;
-		_2q0mz = 2.0f * q0 * mz;
-		_2q1mx = 2.0f * q1 * mx;
-		_2q0 = 2.0f * q0;
-		_2q1 = 2.0f * q1;
-		_2q2 = 2.0f * q2;
-		_2q3 = 2.0f * q3;
-		_2q0q2 = 2.0f * q0 * q2;
-		_2q2q3 = 2.0f * q2 * q3;
-		q0q0 = q0 * q0;
-		q0q1 = q0 * q1;
-		q0q2 = q0 * q2;
-		q0q3 = q0 * q3;
-		q1q1 = q1 * q1;
-		q1q2 = q1 * q2;
-		q1q3 = q1 * q3;
-		q2q2 = q2 * q2;
-		q2q3 = q2 * q3;
-		q3q3 = q3 * q3;
-
-		// Reference direction of Earth's magnetic field
-		hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
-		hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
-		_2bx = sqrtf(hx * hx + hy * hy);
-		_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
-		_4bx = 2.0f * _2bx;
-		_4bz = 2.0f * _2bz;
-
-		// Gradient decent algorithm corrective step
-		s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-		s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-		s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-		s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
-		s0 *= recipNorm;
-		s1 *= recipNorm;
-		s2 *= recipNorm;
-		s3 *= recipNorm;
-
-		// Apply feedback step
-		qDot1 -= beta * s0;
-		qDot2 -= beta * s1;
-		qDot3 -= beta * s2;
-		qDot4 -= beta * s3;
-	}
-
-	// Integrate rate of change of quaternion to yield quaternion
-	q0 += qDot1 * invSampleFreq;
-	q1 += qDot2 * invSampleFreq;
-	q2 += qDot3 * invSampleFreq;
-	q3 += qDot4 * invSampleFreq;
-
-	// Normalise quaternion
-	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-	q0 *= recipNorm;
-	q1 *= recipNorm;
-	q2 *= recipNorm;
-	q3 *= recipNorm;
-	anglesComputed = 0;
-}
-
-//-------------------------------------------------------------------------------------------
-// IMU algorithm update
-
-void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
-	float recipNorm;
-	float s0, s1, s2, s3;
-	float qDot1, qDot2, qDot3, qDot4;
-	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
-
-	// Convert gyroscope degrees/sec to radians/sec
-	gx *= 0.0174533f;
-	gy *= 0.0174533f;
-	gz *= 0.0174533f;
-
-	// Rate of change of quaternion from gyroscope
-	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
-	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
-	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
-	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
-
-	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-		// Normalise accelerometer measurement
-		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-		ax *= recipNorm;
-		ay *= recipNorm;
-		az *= recipNorm;
-
-		// Auxiliary variables to avoid repeated arithmetic
-		_2q0 = 2.0f * q0;
-		_2q1 = 2.0f * q1;
-		_2q2 = 2.0f * q2;
-		_2q3 = 2.0f * q3;
-		_4q0 = 4.0f * q0;
-		_4q1 = 4.0f * q1;
-		_4q2 = 4.0f * q2;
-		_8q1 = 8.0f * q1;
-		_8q2 = 8.0f * q2;
-		q0q0 = q0 * q0;
-		q1q1 = q1 * q1;
-		q2q2 = q2 * q2;
-		q3q3 = q3 * q3;
-
-		// Gradient decent algorithm corrective step
-		s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
-		s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
-		s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
-		s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
-		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
-		s0 *= recipNorm;
-		s1 *= recipNorm;
-		s2 *= recipNorm;
-		s3 *= recipNorm;
-
-		// Apply feedback step
-		qDot1 -= beta * s0;
-		qDot2 -= beta * s1;
-		qDot3 -= beta * s2;
-		qDot4 -= beta * s3;
-	}
-
-	// Integrate rate of change of quaternion to yield quaternion
-	q0 += qDot1 * invSampleFreq;
-	q1 += qDot2 * invSampleFreq;
-	q2 += qDot3 * invSampleFreq;
-	q3 += qDot4 * invSampleFreq;
-
-	// Normalise quaternion
-	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-	q0 *= recipNorm;
-	q1 *= recipNorm;
-	q2 *= recipNorm;
-	q3 *= recipNorm;
-	anglesComputed = 0;
-}
-
-//-------------------------------------------------------------------------------------------
-// Fast inverse square-root
-// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-
-float Madgwick::invSqrt(float x) {
-	float halfx = 0.5f * x;
-	float y = x;
-	long i = *(long*)&y;
-	i = 0x5f3759df - (i>>1);
-	y = *(float*)&i;
-	y = y * (1.5f - (halfx * y * y));
-	y = y * (1.5f - (halfx * y * y));
-	return y;
-}
-
-//-------------------------------------------------------------------------------------------
-
-void Madgwick::computeAngles()
-{
-	roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
-	pitch = asinf(-2.0f * (q1*q3 - q0*q2));
-	yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
-	anglesComputed = 1;
-}
--- a/Madgwick.h
+++ b/Madgwick.h
@ -1,79 +0,0 @@
-//=============================================================================================
-// Madgwick.h
-//=============================================================================================
-//
-// Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-//
-// From the x-io website "Open-source resources available on this website are
-// provided under the GNU General Public Licence unless an alternative licence
-// is provided in source."
-//
-// Date			Author          Notes
-// 29/09/2011	SOH Madgwick    Initial release
-// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
-//
-//=============================================================================================
-#ifndef __Madgwick_h__
-#define __Madgwick_h__
-#include <math.h>
-
-//--------------------------------------------------------------------------------------------
-// Variable declaration
-class Madgwick{
-private:
-    static float invSqrt(float x);
-    float beta;				// algorithm gain
-    float q0;
-    float q1;
-    float q2;
-    float q3;	// quaternion of sensor frame relative to auxiliary frame
-    float invSampleFreq;
-    float roll;
-    float pitch;
-    float yaw;
-    char anglesComputed;
-    void computeAngles();
-
-//-------------------------------------------------------------------------------------------
-// Function declarations
-public:
-    Madgwick(void);
-    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
-    void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
-    void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
-    //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
-    //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
-    //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
-    float getRoll() {
-        if (!anglesComputed) computeAngles();
-        return roll * 57.29578f;
-    }
-    float getPitch() {
-        if (!anglesComputed) computeAngles();
-        return pitch * 57.29578f;
-    }
-    float getYaw() {
-        if (!anglesComputed) computeAngles();
-        return yaw * 57.29578f + 180.0f;
-    }
-    float getRollRadians() {
-        if (!anglesComputed) computeAngles();
-        return roll;
-    }
-    float getPitchRadians() {
-        if (!anglesComputed) computeAngles();
-        return pitch;
-    }
-    float getYawRadians() {
-        if (!anglesComputed) computeAngles();
-        return yaw;
-    }
-	void getQuaternion(float *w, float *x, float *y, float *z) {
-       *w = q0;
-       *x = q1;
-       *y = q2;
-       *z = q3;
-   }
-};
-#endif
--- a/Mahony.cpp
+++ b/Mahony.cpp
@ -1,274 +0,0 @@
-//=============================================================================================
-// Mahony.c
-//=============================================================================================
-//
-// Madgwick's implementation of Mayhony's AHRS algorithm.
-// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-//
-// From the x-io website "Open-source resources available on this website are
-// provided under the GNU General Public Licence unless an alternative licence
-// is provided in source."
-//
-// Date			Author			Notes
-// 29/09/2011	SOH Madgwick    Initial release
-// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
-//
-// Algorithm paper:
-// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
-//
-//=============================================================================================
-
-//-------------------------------------------------------------------------------------------
-// Header files
-
-#include "Mahony.h"
-#include <math.h>
-
-//-------------------------------------------------------------------------------------------
-// Definitions
-
-#define DEFAULT_SAMPLE_FREQ	512.0f	// sample frequency in Hz
-#define twoKpDef	(2.0f * 0.5f)	// 2 * proportional gain
-#define twoKiDef	(2.0f * 0.0f)	// 2 * integral gain
-
-
-//============================================================================================
-// Functions
-
-//-------------------------------------------------------------------------------------------
-// AHRS algorithm update
-
-Mahony::Mahony()
-{
-	twoKp = twoKpDef;	// 2 * proportional gain (Kp)
-	twoKi = twoKiDef;	// 2 * integral gain (Ki)
-	q0 = 1.0f;
-	q1 = 0.0f;
-	q2 = 0.0f;
-	q3 = 0.0f;
-	integralFBx = 0.0f;
-	integralFBy = 0.0f;
-	integralFBz = 0.0f;
-	anglesComputed = 0;
-	invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
-}
-
-void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
-{
-	float recipNorm;
-	float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
-	float hx, hy, bx, bz;
-	float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
-	float halfex, halfey, halfez;
-	float qa, qb, qc;
-
-	// Use IMU algorithm if magnetometer measurement invalid
-	// (avoids NaN in magnetometer normalisation)
-	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-		updateIMU(gx, gy, gz, ax, ay, az);
-		return;
-	}
-
-	// Convert gyroscope degrees/sec to radians/sec
-	gx *= 0.0174533f;
-	gy *= 0.0174533f;
-	gz *= 0.0174533f;
-
-	// Compute feedback only if accelerometer measurement valid
-	// (avoids NaN in accelerometer normalisation)
-	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-		// Normalise accelerometer measurement
-		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-		ax *= recipNorm;
-		ay *= recipNorm;
-		az *= recipNorm;
-
-		// Normalise magnetometer measurement
-		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-		mx *= recipNorm;
-		my *= recipNorm;
-		mz *= recipNorm;
-
-		// Auxiliary variables to avoid repeated arithmetic
-		q0q0 = q0 * q0;
-		q0q1 = q0 * q1;
-		q0q2 = q0 * q2;
-		q0q3 = q0 * q3;
-		q1q1 = q1 * q1;
-		q1q2 = q1 * q2;
-		q1q3 = q1 * q3;
-		q2q2 = q2 * q2;
-		q2q3 = q2 * q3;
-		q3q3 = q3 * q3;
-
-		// Reference direction of Earth's magnetic field
-		hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
-		hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
-		bx = sqrtf(hx * hx + hy * hy);
-		bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
-
-		// Estimated direction of gravity and magnetic field
-		halfvx = q1q3 - q0q2;
-		halfvy = q0q1 + q2q3;
-		halfvz = q0q0 - 0.5f + q3q3;
-		halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
-		halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
-		halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
-
-		// Error is sum of cross product between estimated direction
-		// and measured direction of field vectors
-		halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
-		halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
-		halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
-
-		// Compute and apply integral feedback if enabled
-		if(twoKi > 0.0f) {
-			// integral error scaled by Ki
-			integralFBx += twoKi * halfex * invSampleFreq;
-			integralFBy += twoKi * halfey * invSampleFreq;
-			integralFBz += twoKi * halfez * invSampleFreq;
-			gx += integralFBx;	// apply integral feedback
-			gy += integralFBy;
-			gz += integralFBz;
-		} else {
-			integralFBx = 0.0f;	// prevent integral windup
-			integralFBy = 0.0f;
-			integralFBz = 0.0f;
-		}
-
-		// Apply proportional feedback
-		gx += twoKp * halfex;
-		gy += twoKp * halfey;
-		gz += twoKp * halfez;
-	}
-
-	// Integrate rate of change of quaternion
-	gx *= (0.5f * invSampleFreq);		// pre-multiply common factors
-	gy *= (0.5f * invSampleFreq);
-	gz *= (0.5f * invSampleFreq);
-	qa = q0;
-	qb = q1;
-	qc = q2;
-	q0 += (-qb * gx - qc * gy - q3 * gz);
-	q1 += (qa * gx + qc * gz - q3 * gy);
-	q2 += (qa * gy - qb * gz + q3 * gx);
-	q3 += (qa * gz + qb * gy - qc * gx);
-
-	// Normalise quaternion
-	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-	q0 *= recipNorm;
-	q1 *= recipNorm;
-	q2 *= recipNorm;
-	q3 *= recipNorm;
-	anglesComputed = 0;
-}
-
-//-------------------------------------------------------------------------------------------
-// IMU algorithm update
-
-void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
-{
-	float recipNorm;
-	float halfvx, halfvy, halfvz;
-	float halfex, halfey, halfez;
-	float qa, qb, qc;
-
-	// Convert gyroscope degrees/sec to radians/sec
-	gx *= 0.0174533f;
-	gy *= 0.0174533f;
-	gz *= 0.0174533f;
-
-	// Compute feedback only if accelerometer measurement valid
-	// (avoids NaN in accelerometer normalisation)
-	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-		// Normalise accelerometer measurement
-		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-		ax *= recipNorm;
-		ay *= recipNorm;
-		az *= recipNorm;
-
-		// Estimated direction of gravity
-		halfvx = q1 * q3 - q0 * q2;
-		halfvy = q0 * q1 + q2 * q3;
-		halfvz = q0 * q0 - 0.5f + q3 * q3;
-
-		// Error is sum of cross product between estimated
-		// and measured direction of gravity
-		halfex = (ay * halfvz - az * halfvy);
-		halfey = (az * halfvx - ax * halfvz);
-		halfez = (ax * halfvy - ay * halfvx);
-
-		// Compute and apply integral feedback if enabled
-		if(twoKi > 0.0f) {
-			// integral error scaled by Ki
-			integralFBx += twoKi * halfex * invSampleFreq;
-			integralFBy += twoKi * halfey * invSampleFreq;
-			integralFBz += twoKi * halfez * invSampleFreq;
-			gx += integralFBx;	// apply integral feedback
-			gy += integralFBy;
-			gz += integralFBz;
-		} else {
-			integralFBx = 0.0f;	// prevent integral windup
-			integralFBy = 0.0f;
-			integralFBz = 0.0f;
-		}
-
-		// Apply proportional feedback
-		gx += twoKp * halfex;
-		gy += twoKp * halfey;
-		gz += twoKp * halfez;
-	}
-
-	// Integrate rate of change of quaternion
-	gx *= (0.5f * invSampleFreq);		// pre-multiply common factors
-	gy *= (0.5f * invSampleFreq);
-	gz *= (0.5f * invSampleFreq);
-	qa = q0;
-	qb = q1;
-	qc = q2;
-	q0 += (-qb * gx - qc * gy - q3 * gz);
-	q1 += (qa * gx + qc * gz - q3 * gy);
-	q2 += (qa * gy - qb * gz + q3 * gx);
-	q3 += (qa * gz + qb * gy - qc * gx);
-
-	// Normalise quaternion
-	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-	q0 *= recipNorm;
-	q1 *= recipNorm;
-	q2 *= recipNorm;
-	q3 *= recipNorm;
-	anglesComputed = 0;
-}
-
-//-------------------------------------------------------------------------------------------
-// Fast inverse square-root
-// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-
-float Mahony::invSqrt(float x)
-{
-	float halfx = 0.5f * x;
-	float y = x;
-	long i = *(long*)&y;
-	i = 0x5f3759df - (i>>1);
-	y = *(float*)&i;
-	y = y * (1.5f - (halfx * y * y));
-	y = y * (1.5f - (halfx * y * y));
-	return y;
-}
-
-//-------------------------------------------------------------------------------------------
-
-void Mahony::computeAngles()
-{
-	roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
-	pitch = asinf(-2.0f * (q1*q3 - q0*q2));
-	yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
-	anglesComputed = 1;
-}
-
-
-//============================================================================================
-// END OF CODE
-//============================================================================================
--- a/Mahony.h
+++ b/Mahony.h
@ -1,72 +0,0 @@
-//=============================================================================================
-// Mahony.h
-//=============================================================================================
-//
-// Madgwick's implementation of Mayhony's AHRS algorithm.
-// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-//
-// Date			Author			Notes
-// 29/09/2011	SOH Madgwick    Initial release
-// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
-//
-//=============================================================================================
-#ifndef __Mahony_h__
-#define __Mahony_h__
-#include <math.h>
-
-//--------------------------------------------------------------------------------------------
-// Variable declaration
-
-class Mahony {
-private:
-	float twoKp;		// 2 * proportional gain (Kp)
-	float twoKi;		// 2 * integral gain (Ki)
-	float q0, q1, q2, q3;	// quaternion of sensor frame relative to auxiliary frame
-	float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
-	float invSampleFreq;
-	float roll, pitch, yaw;
-	char anglesComputed;
-	static float invSqrt(float x);
-	void computeAngles();
-
-//-------------------------------------------------------------------------------------------
-// Function declarations
-
-public:
-	Mahony();
-	void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
-	void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
-	void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
-	float getRoll() {
-		if (!anglesComputed) computeAngles();
-		return roll * 57.29578f;
-	}
-	float getPitch() {
-		if (!anglesComputed) computeAngles();
-		return pitch * 57.29578f;
-	}
-	float getYaw() {
-		if (!anglesComputed) computeAngles();
-		return yaw * 57.29578f + 180.0f;
-	}
-	float getRollRadians() {
-		if (!anglesComputed) computeAngles();
-		return roll;
-	}
-	float getPitchRadians() {
-		if (!anglesComputed) computeAngles();
-		return pitch;
-	}
-	float getYawRadians() {
-		if (!anglesComputed) computeAngles();
-		return yaw;
-	}
-	void getQuaternion(float *w, float *x, float *y, float *z) {
-       *w = q0;
-       *x = q1;
-       *y = q2;
-       *z = q3;
-   }
-};
-
-#endif
--- a/README.md
+++ b/README.md
@ -0,0 +1,8 @@
+# Adafruit AHRS library
+
+This library lets you take an accelerometer, gyroscope and magnetometer and 
+combine the data to create orientation data.
+
+Options are Mahony (lowest memory/computation), Madgwick (fair memory/computation) and NXP fusion/Kalman (highest).
+
+While in theory these can run on an Arduino UNO/Atmega328P we really recommend a SAMD21 or better. Having single-instruction floating point multiply and plenty of RAM will help a lot!
--- a/examples/ahrs_calibration/ahrs_calibration.ino
+++ b/examples/ahrs_calibration/ahrs_calibration.ino
@ -1,86 +0,0 @@
-#include <Wire.h>
-#include <Adafruit_Sensor.h>
-#include <Adafruit_L3GD20_U.h>
-#include <Adafruit_LSM303_U.h>
-
-// Create sensor instances.
-Adafruit_L3GD20_Unified       gyro(20);
-Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
-
-// This sketch can be used to output raw sensor data in a format that
-// can be understoof by MotionCal from PJRC. Download the application
-// from http://www.pjrc.com/store/prop_shield.html and make note of the
-// magentic offsets after rotating the sensors sufficiently.
-//
-// You should end up with 3 offsets for X/Y/Z, which are displayed
-// in the top-right corner of the application.
-//
-// Make sure you have the latest versions of the Adfruit_L3GD20_U and
-// Adafruit_LSM303_U libraries when running this sketch since they
-// use a new .raw field added in the latest versions (Oct 10 2016)
-
-void setup()
-{
-  Serial.begin(115200);
-
-  // Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
-  // while(!Serial);
-  
-  Serial.println(F("Adafruit 10 DOF Board AHRS Calibration Example")); Serial.println("");
-  
-  // Initialize the sensors.
-  if(!gyro.begin())
-  {
-    /* There was a problem detecting the L3GD20 ... check your connections */
-    Serial.println("Ooops, no L3GD20 detected ... Check your wiring!");
-    while(1);
-  }
-  
-  if(!accel.begin())
-  {
-    /* There was a problem detecting the LSM303DLHC ... check your connections */
-    Serial.println("Ooops, no L3M303DLHC accel detected ... Check your wiring!");
-    while(1);
-  }
-  
-  if(!mag.begin())
-  {
-    /* There was a problem detecting the LSM303DLHC ... check your connections */
-    Serial.println("Ooops, no L3M303DLHC mag detected ... Check your wiring!");
-    while(1);
-  }
-}
-
-void loop(void)
-{
-  sensors_event_t event; // Need to read raw data, which is stored at the same time
-  
-  // Get new data samples
-  gyro.getEvent(&event);
-  accel.getEvent(&event);
-  mag.getEvent(&event);
-
-  // Print the sensor data
-  Serial.print("Raw:");
-  Serial.print(accel.raw.x);
-  Serial.print(',');
-  Serial.print(accel.raw.y);
-  Serial.print(',');
-  Serial.print(accel.raw.z);
-  Serial.print(',');
-  Serial.print(gyro.raw.x);
-  Serial.print(',');
-  Serial.print(gyro.raw.y);
-  Serial.print(',');
-  Serial.print(gyro.raw.z);
-  Serial.print(',');
-  Serial.print(mag.raw.x);
-  Serial.print(',');
-  Serial.print(mag.raw.y);
-  Serial.print(',');
-  Serial.print(mag.raw.z);
-  Serial.println();  
-    
-  delay(50);
-}
--- a/examples/ahrs_calibration_ble_nrf51/.uno.test.only
+++ b/examples/ahrs_calibration_ble_nrf51/.uno.test.only
--- a/examples/ahrs_calibration_ble_nrf51/ArdPrintf.h
+++ b/examples/ahrs_calibration_ble_nrf51/ArdPrintf.h
@ -1,50 +1,50 @@
 #ifndef ARDPRINTF
 #define ARDPRINTF
 #define ARDBUFFER 20
-#include <stdarg.h>
 #include <Arduino.h>
+#include <stdarg.h>

-int ardprintf(char *str, ...)
-{
-  int i, count=0, j=0, flag=0;
-  char temp[ARDBUFFER+1];
-  for(i=0; str[i]!='\0';i++)  if(str[i]=='%')  count++;
+int ardprintf(char *str, ...) {
+  int i, count = 0, j = 0, flag = 0;
+  char temp[ARDBUFFER + 1];
+  for (i = 0; str[i] != '\0'; i++)
+    if (str[i] == '%')
+      count++;

  va_list argv;
  va_start(argv, count);
-  for(i=0,j=0; str[i]!='\0';i++)
-  {
-    if(str[i]=='%')
-    {
+  for (i = 0, j = 0; str[i] != '\0'; i++) {
+    if (str[i] == '%') {
      temp[j] = '\0';
      Serial.print(temp);
-      j=0;
+      j = 0;
      temp[0] = '\0';

-      switch(str[++i])
-      {
-        case 'd': Serial.print(va_arg(argv, int));
+      switch (str[++i]) {
+      case 'd':
+        Serial.print(va_arg(argv, int));
        break;
-        case 'l': Serial.print(va_arg(argv, long));
+      case 'l':
+        Serial.print(va_arg(argv, long));
        break;
-        case 'f': Serial.print(va_arg(argv, double));
+      case 'f':
+        Serial.print(va_arg(argv, double));
        break;
-        case 'c': Serial.print((char)va_arg(argv, int));
+      case 'c':
+        Serial.print((char)va_arg(argv, int));
        break;
-        case 's': Serial.print(va_arg(argv, char *));
+      case 's':
+        Serial.print(va_arg(argv, char *));
        break;
-        default:  ;
+      default:;
      };
-    }
-    else 
-    {
+    } else {
      temp[j] = str[i];
-      j = (j+1)%ARDBUFFER;
-      if(j==0) 
-      {
+      j = (j + 1) % ARDBUFFER;
+      if (j == 0) {
        temp[ARDBUFFER] = '\0';
        Serial.print(temp);
-        temp[0]='\0';
+        temp[0] = '\0';
      }
    }
  };
--- a/examples/ahrs_calibration_ble_nrf51/BluefruitConfig.h
+++ b/examples/ahrs_calibration_ble_nrf51/BluefruitConfig.h
@ -5,7 +5,6 @@
 #define BUFSIZE 128       // Size of the read buffer for incoming data
 #define VERBOSE_MODE true // If set to 'true' enables debug output

-
 // SOFTWARE UART SETTINGS
 // ----------------------------------------------------------------------------------------------
 // The following macros declare the pins that will be used for 'SW' serial.
@ -16,24 +15,22 @@
 #define BLUEFRUIT_UART_CTS_PIN 11   // Required for software serial!
 #define BLUEFRUIT_UART_RTS_PIN -1   // Optional, set to -1 if unused

-
 // HARDWARE UART SETTINGS
 // ----------------------------------------------------------------------------------------------
 // The following macros declare the HW serial port you are using. Uncomment
 // this line if you are connecting the BLE to Leonardo/Micro or Flora
 // ----------------------------------------------------------------------------------------------
-#ifdef Serial1    // this makes it not complain on compilation if there's no Serial1
-  #define BLUEFRUIT_HWSERIAL_NAME      Serial1
+#ifdef Serial1 // this makes it not complain on compilation if there's no
+               // Serial1
+#define BLUEFRUIT_HWSERIAL_NAME Serial1
 #endif

-
 // SHARED UART SETTINGS
 // ----------------------------------------------------------------------------------------------
 // The following sets the optional Mode pin, its recommended but not required
 // ----------------------------------------------------------------------------------------------
 #define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused

-
 // SHARED SPI SETTINGS
 // ----------------------------------------------------------------------------------------------
 // The following macros declare the pins to use for HW and SW SPI communication.
--- a/examples/ahrs_calibration_usb/ahrs_calibration_usb.ino
+++ b/examples/ahrs_calibration_usb/ahrs_calibration_usb.ino
@ -1,140 +0,0 @@
-#include <Wire.h>
-#include <Adafruit_Sensor.h>
-
-#define ST_LSM303DLHC_L3GD20        (0)
-#define ST_LSM9DS1                  (1)
-#define NXP_FXOS8700_FXAS21002      (2)
-
-// Define your target sensor(s) here based on the list above!
-// #define AHRS_VARIANT    ST_LSM303DLHC_L3GD20
-#define AHRS_VARIANT   NXP_FXOS8700_FXAS21002
-
-// Include appropriate sensor driver(s)
-#if AHRS_VARIANT == ST_LSM303DLHC_L3GD20
-#include <Adafruit_L3GD20_U.h>
-#include <Adafruit_LSM303_U.h>
-#elif AHRS_VARIANT == ST_LSM9DS1
-// ToDo!
-#elif AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-#include <Adafruit_FXAS21002C.h>
-#include <Adafruit_FXOS8700.h>
-#else
-#error "AHRS_VARIANT undefined! Please select a target sensor combination!"
-#endif
-
-// Create sensor instances.
-#if AHRS_VARIANT == ST_LSM303DLHC_L3GD20
-Adafruit_L3GD20_Unified       gyro(20);
-Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
-#elif AHRS_VARIANT == ST_LSM9DS1
-// ToDo!
-#elif AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
-Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
-#endif
-
-// NOTE: THIS IS A WORK IN PROGRESS!
-
-// This sketch can be used to output raw sensor data in a format that
-// can be understoof by MotionCal from PJRC. Download the application
-// from http://www.pjrc.com/store/prop_shield.html and make note of the
-// magentic offsets after rotating the sensors sufficiently.
-//
-// You should end up with 3 offsets for X/Y/Z, which are displayed
-// in the top-right corner of the application.
-
-void setup()
-{
-  Serial.begin(115200);
-
-  // Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
-  // while(!Serial);
-
-  Serial.println(F("Adafruit nDOF AHRS Calibration Example")); Serial.println("");
-
-  // Initialize the sensors.
-  if(!gyro.begin())
-  {
-    /* There was a problem detecting the gyro ... check your connections */
-    Serial.println("Ooops, no gyro detected ... Check your wiring!");
-    while(1);
-  }
-
-#if AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-  if(!accelmag.begin(ACCEL_RANGE_4G))
-  {
-    Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
-    while(1);
-  }
-#else
-  if (!accel.begin())
-  {
-    /* There was a problem detecting the accel ... check your connections */
-    Serial.println("Ooops, no accel detected ... Check your wiring!");
-    while (1);
-  }
-
-  if (!mag.begin())
-  {
-    /* There was a problem detecting the mag ... check your connections */
-    Serial.println("Ooops, no mag detected ... Check your wiring!");
-    while (1);
-  }
-#endif
-}
-
-void loop(void)
-{
-  sensors_event_t event; // Need to read raw data, which is stored at the same time
-
-    // Get new data samples
-    gyro.getEvent(&event);
-#if AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-    accelmag.getEvent(&event);
-#else
-    accel.getEvent(&event);
-    mag.getEvent(&event);
-#endif
-
-  // Print the sensor data
-  Serial.print("Raw:");
-#if AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-    Serial.print(accelmag.accel_raw.x);
-    Serial.print(',');
-    Serial.print(accelmag.accel_raw.y);
-    Serial.print(',');
-    Serial.print(accelmag.accel_raw.z);
-    Serial.print(',');
-#else
-    Serial.print(accel.raw.x);
-    Serial.print(',');
-    Serial.print(accel.raw.y);
-    Serial.print(',');
-    Serial.print(accel.raw.z);
-    Serial.print(',');
-#endif
-    Serial.print(gyro.raw.x);
-    Serial.print(',');
-    Serial.print(gyro.raw.y);
-    Serial.print(',');
-    Serial.print(gyro.raw.z);
-    Serial.print(',');
-#if AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-    Serial.print(accelmag.mag_raw.x);
-    Serial.print(',');
-    Serial.print(accelmag.mag_raw.y);
-    Serial.print(',');
-    Serial.print(accelmag.mag_raw.z);
-    Serial.println();
-#else
-    Serial.print(mag.raw.x);
-    Serial.print(',');
-    Serial.print(mag.raw.y);
-    Serial.print(',');
-    Serial.print(mag.raw.z);
-    Serial.println();
-#endif
-
-  delay(50);
-}
--- a/examples/ahrs_fusion_ble_nrf51/.uno.test.only
+++ b/examples/ahrs_fusion_ble_nrf51/.uno.test.only
--- a/examples/ahrs_fusion_ble_nrf51/BluefruitConfig.h
+++ b/examples/ahrs_fusion_ble_nrf51/BluefruitConfig.h
@ -5,7 +5,6 @@
 #define BUFSIZE 128       // Size of the read buffer for incoming data
 #define VERBOSE_MODE true // If set to 'true' enables debug output

-
 // SOFTWARE UART SETTINGS
 // ----------------------------------------------------------------------------------------------
 // The following macros declare the pins that will be used for 'SW' serial.
@ -16,24 +15,22 @@
 #define BLUEFRUIT_UART_CTS_PIN 11   // Required for software serial!
 #define BLUEFRUIT_UART_RTS_PIN -1   // Optional, set to -1 if unused

-
 // HARDWARE UART SETTINGS
 // ----------------------------------------------------------------------------------------------
 // The following macros declare the HW serial port you are using. Uncomment
 // this line if you are connecting the BLE to Leonardo/Micro or Flora
 // ----------------------------------------------------------------------------------------------
-#ifdef Serial1    // this makes it not complain on compilation if there's no Serial1
-  #define BLUEFRUIT_HWSERIAL_NAME      Serial1
+#ifdef Serial1 // this makes it not complain on compilation if there's no
+               // Serial1
+#define BLUEFRUIT_HWSERIAL_NAME Serial1
 #endif

-
 // SHARED UART SETTINGS
 // ----------------------------------------------------------------------------------------------
 // The following sets the optional Mode pin, its recommended but not required
 // ----------------------------------------------------------------------------------------------
 #define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused

-
 // SHARED SPI SETTINGS
 // ----------------------------------------------------------------------------------------------
 // The following macros declare the pins to use for HW and SW SPI communication.
--- a/examples/ahrs_fusion_ble_nrf51/ahrs_fusion_ble_nrf51.ino
+++ b/examples/ahrs_fusion_ble_nrf51/ahrs_fusion_ble_nrf51.ino
@ -1,8 +1,5 @@
-#include <Wire.h>
-#include <SPI.h>
-#include <Adafruit_Sensor.h>
-#include "Mahony.h"
-#include "Madgwick.h"
+#include "Adafruit_AHRS_Mahony.h"
+#include "Adafruit_AHRS_Madgwick.h"
 #include "Adafruit_BLE.h"
 #include "Adafruit_BluefruitLE_SPI.h"
 #include "Adafruit_BluefruitLE_UART.h"
@ -75,8 +72,8 @@ float gyro_zero_offsets[3]      = { 175.0F * rawToDPS * dpsToRad,

 // Mahony is lighter weight as a filter and should be used
 // on slower systems
-Mahony filter;
-//Madgwick filter;
+Adafruit_Mahony filter;
+//Adafruit_Madgwick filter;

 void setup()
 {
--- a/examples/ahrs_fusion_usb/ahrs_fusion_usb.ino
+++ b/examples/ahrs_fusion_usb/ahrs_fusion_usb.ino
@ -1,167 +0,0 @@
-#include <Wire.h>
-#include <Adafruit_Sensor.h>
-#include <Mahony.h>
-#include <Madgwick.h>
-
-// Note: This sketch is a WORK IN PROGRESS
-
-#define ST_LSM303DLHC_L3GD20        (0)
-#define ST_LSM9DS1                  (1)
-#define NXP_FXOS8700_FXAS21002      (2)
-
-// Define your target sensor(s) here based on the list above!
-// #define AHRS_VARIANT    ST_LSM303DLHC_L3GD20
-#define AHRS_VARIANT   NXP_FXOS8700_FXAS21002
-
-#if AHRS_VARIANT == ST_LSM303DLHC_L3GD20
-#include <Adafruit_L3GD20_U.h>
-#include <Adafruit_LSM303_U.h>
-#elif AHRS_VARIANT == ST_LSM9DS1
-// ToDo!
-#elif AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-#include <Adafruit_FXAS21002C.h>
-#include <Adafruit_FXOS8700.h>
-#else
-#error "AHRS_VARIANT undefined! Please select a target sensor combination!"
-#endif
-
-
-// Create sensor instances.
-#if AHRS_VARIANT == ST_LSM303DLHC_L3GD20
-Adafruit_L3GD20_Unified       gyro(20);
-Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
-#elif AHRS_VARIANT == ST_LSM9DS1
-// ToDo!
-#elif AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
-Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
-#endif
-
-// Mag calibration values are calculated via ahrs_calibration.
-// These values must be determined for each board/environment.
-// See the image in this sketch folder for the values used
-// below.
-
-// Offsets applied to raw x/y/z mag values
-float mag_offsets[3]            = { 0.93F, -7.47F, -35.23F };
-
-// Soft iron error compensation matrix
-float mag_softiron_matrix[3][3] = { {  0.943,  0.011,  0.020 },
-                                    {  0.022,  0.918, -0.008 },
-                                    {  0.020, -0.008,  1.156 } };
-
-float mag_field_strength        = 50.23F;
-
-// Offsets applied to compensate for gyro zero-drift error for x/y/z
-float gyro_zero_offsets[3]      = { 0.0F, 0.0F, 0.0F };
-
-// Mahony is lighter weight as a filter and should be used
-// on slower systems
-Mahony filter;
-//Madgwick filter;
-
-void setup()
-{
-  Serial.begin(115200);
-
-  // Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
-  // while(!Serial);
-
-  Serial.println(F("Adafruit AHRS Fusion Example")); Serial.println("");
-
-  // Initialize the sensors.
-  if(!gyro.begin())
-  {
-    /* There was a problem detecting the gyro ... check your connections */
-    Serial.println("Ooops, no gyro detected ... Check your wiring!");
-    while(1);
-  }
-
-#if AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-  if(!accelmag.begin(ACCEL_RANGE_4G))
-  {
-    Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
-    while(1);
-  }
-#else
-  if (!accel.begin())
-  {
-    /* There was a problem detecting the accel ... check your connections */
-    Serial.println("Ooops, no accel detected ... Check your wiring!");
-    while (1);
-  }
-
-  if (!mag.begin())
-  {
-    /* There was a problem detecting the mag ... check your connections */
-    Serial.println("Ooops, no mag detected ... Check your wiring!");
-    while (1);
-  }
-#endif
-
-  // Filter expects 70 samples per second
-  // Based on a Bluefruit M0 Feather ... rate should be adjuted for other MCUs
-  filter.begin(10);
-}
-
-void loop(void)
-{
-  sensors_event_t gyro_event;
-  sensors_event_t accel_event;
-  sensors_event_t mag_event;
-
-  // Get new data samples
-  gyro.getEvent(&gyro_event);
-#if AHRS_VARIANT == NXP_FXOS8700_FXAS21002
-  accelmag.getEvent(&accel_event, &mag_event);
-#else
-  accel.getEvent(&accel_event);
-  mag.getEvent(&mag_event);
-#endif
-
-  // Apply mag offset compensation (base values in uTesla)
-  float x = mag_event.magnetic.x - mag_offsets[0];
-  float y = mag_event.magnetic.y - mag_offsets[1];
-  float z = mag_event.magnetic.z - mag_offsets[2];
-
-  // Apply mag soft iron error compensation
-  float mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
-  float my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
-  float mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];
-
-  // Apply gyro zero-rate error compensation
-  float gx = gyro_event.gyro.x + gyro_zero_offsets[0];
-  float gy = gyro_event.gyro.y + gyro_zero_offsets[1];
-  float gz = gyro_event.gyro.z + gyro_zero_offsets[2];
-
-  // The filter library expects gyro data in degrees/s, but adafruit sensor
-  // uses rad/s so we need to convert them first (or adapt the filter lib
-  // where they are being converted)
-  gx *= 57.2958F;
-  gy *= 57.2958F;
-  gz *= 57.2958F;
-
-  // Update the filter
-  filter.update(gx, gy, gz,
-                accel_event.acceleration.x, accel_event.acceleration.y, accel_event.acceleration.z,
-                mx, my, mz);
-
-  // Print the orientation filter output
-  // Note: To avoid gimbal lock you should read quaternions not Euler
-  // angles, but Euler angles are used here since they are easier to
-  // understand looking at the raw values. See the ble fusion sketch for
-  // and example of working with quaternion data.
-  float roll = filter.getRoll();
-  float pitch = filter.getPitch();
-  float heading = filter.getYaw();
-  Serial.print(millis());
-  Serial.print(" - Orientation: ");
-  Serial.print(heading);
-  Serial.print(" ");
-  Serial.print(pitch);
-  Serial.print(" ");
-  Serial.println(roll);
-
-  delay(10);
-}
--- a/examples/ahrs_fusion_usb/sample_config_data.png
+++ b/examples/ahrs_fusion_usb/sample_config_data.png
--- a/examples/ahrs_lsm9ds0/ahrs_lsm9ds0.ino
+++ b/examples/ahrs_lsm9ds0/ahrs_lsm9ds0.ino
@ -1,72 +0,0 @@
-#include <SPI.h>
-#include <Wire.h>
-#include <Adafruit_Sensor.h>
-#include <Adafruit_LSM9DS0.h>
-#include <Adafruit_Simple_AHRS.h>
-
-// Create LSM9DS0 board instance.
-Adafruit_LSM9DS0     lsm(1000);  // Use I2C, ID #1000
-
-// Create simple AHRS algorithm using the LSM9DS0 instance's accelerometer and magnetometer.
-Adafruit_Simple_AHRS ahrs(&lsm.getAccel(), &lsm.getMag());
-
-// Function to configure the sensors on the LSM9DS0 board.
-// You don't need to change anything here, but have the option to select different
-// range and gain values.
-void configureLSM9DS0(void)
-{
-  // 1.) Set the accelerometer range
-  lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_2G);
-  //lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_4G);
-  //lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_6G);
-  //lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_8G);
-  //lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_16G);
-  
-  // 2.) Set the magnetometer sensitivity
-  lsm.setupMag(lsm.LSM9DS0_MAGGAIN_2GAUSS);
-  //lsm.setupMag(lsm.LSM9DS0_MAGGAIN_4GAUSS);
-  //lsm.setupMag(lsm.LSM9DS0_MAGGAIN_8GAUSS);
-  //lsm.setupMag(lsm.LSM9DS0_MAGGAIN_12GAUSS);
-
-  // 3.) Setup the gyroscope
-  lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_245DPS);
-  //lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_500DPS);
-  //lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_2000DPS);
-}
-
-void setup(void) 
-{
-  Serial.begin(115200);
-  Serial.println(F("Adafruit LSM9DS0 9 DOF Board AHRS Example")); Serial.println("");
-  
-  // Initialise the LSM9DS0 board.
-  if(!lsm.begin())
-  {
-    // There was a problem detecting the LSM9DS0 ... check your connections
-    Serial.print(F("Ooops, no LSM9DS0 detected ... Check your wiring or I2C ADDR!"));
-    while(1);
-  }
-  
-  // Setup the sensor gain and integration time.
-  configureLSM9DS0();
-}
-
-void loop(void) 
-{
-  sensors_vec_t   orientation;
-
-  // Use the simple AHRS function to get the current orientation.
-  if (ahrs.getOrientation(&orientation))
-  {
-    /* 'orientation' should have valid .roll and .pitch fields */
-    Serial.print(F("Orientation: "));
-    Serial.print(orientation.roll);
-    Serial.print(F(" "));
-    Serial.print(orientation.pitch);
-    Serial.print(F(" "));
-    Serial.print(orientation.heading);
-    Serial.println(F(""));
-  }
-  
-  delay(100);
-}
--- a/examples/ahrs_mahony/ahrs_mahony.ino
+++ b/examples/ahrs_mahony/ahrs_mahony.ino
@ -1,120 +0,0 @@
-#include <Wire.h>
-#include <Adafruit_Sensor.h>
-#include <Adafruit_L3GD20_U.h>
-#include <Adafruit_LSM303_U.h>
-#include <MahonyAHRS.h>
-#include <MadgwickAHRS.h>
-
-// Note: This sketch requires the MahonyAHRS sketch from
-// https://github.com/PaulStoffregen/MahonyAHRS, or the
-// MagdwickAHRS sketch from https://github.com/PaulStoffregen/MadgwickAHRS
-
-// Note: This sketch is a WORK IN PROGRESS
-
-// Create sensor instances.
-Adafruit_L3GD20_Unified       gyro(20);
-Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
-
-// Mag calibration values are calculated via ahrs_calibration.
-// These values must be determined for each baord/environment.
-// See the image in this sketch folder for the values used
-// below.
-
-// Offsets applied to raw x/y/z values
-float mag_offsets[3]            = { -2.20F, -5.53F, -26.34F };
-
-// Soft iron error compensation matrix
-float mag_softiron_matrix[3][3] = { { 0.934, 0.005, 0.013 },
-                                    { 0.005, 0.948, 0.012 },
-                                    { 0.013, 0.012, 1.129 } }; 
-
-float mag_field_strength        = 48.41F;
-
-// Mahony is lighter weight as a filter and should be used
-// on slower systems
-Mahony filter;
-//Madgwick filter;
-
-void setup()
-{
-  Serial.begin(115200);
-
-  // Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
-  // while(!Serial);
-  
-  Serial.println(F("Adafruit 10 DOF Board AHRS Calibration Example")); Serial.println("");
-  
-  // Initialize the sensors.
-  if(!gyro.begin())
-  {
-    /* There was a problem detecting the L3GD20 ... check your connections */
-    Serial.println("Ooops, no L3GD20 detected ... Check your wiring!");
-    while(1);
-  }
-  
-  if(!accel.begin())
-  {
-    /* There was a problem detecting the LSM303DLHC ... check your connections */
-    Serial.println("Ooops, no L3M303DLHC accel detected ... Check your wiring!");
-    while(1);
-  }
-  
-  if(!mag.begin())
-  {
-    /* There was a problem detecting the LSM303DLHC ... check your connections */
-    Serial.println("Ooops, no L3M303DLHC mag detected ... Check your wiring!");
-    while(1);
-  }
-
-  // Filter expects 50 samples per second
-  filter.begin(50);
-}
-
-void loop(void)
-{
-  sensors_event_t gyro_event;
-  sensors_event_t accel_event;
-  sensors_event_t mag_event;
-
-  // Get new data samples
-  gyro.getEvent(&gyro_event);
-  accel.getEvent(&accel_event);
-  mag.getEvent(&mag_event);
-
-  // Apply mag offset compensation (base values in uTesla)
-  float x = mag_event.magnetic.x - mag_offsets[0];
-  float y = mag_event.magnetic.y - mag_offsets[1];
-  float z = mag_event.magnetic.z - mag_offsets[2];
-
-  // Apply mag soft iron error compensation
-  float mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
-  float my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
-  float mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];
-
-  // The filter library expects gyro data in degrees/s, but adafruit sensor
-  // uses rad/s so we need to convert them first (or adapt the filter lib
-  // where they are being converted)
-  float gx = gyro_event.gyro.x * 57.2958F;
-  float gy = gyro_event.gyro.y * 57.2958F;
-  float gz = gyro_event.gyro.z * 57.2958F;
-
-  // Update the filter
-  filter.update(gx, gy, gz,
-                accel_event.acceleration.x, accel_event.acceleration.y, accel_event.acceleration.z,
-                mx, my, mz);
-
-  // Print the orientation filter output
-  float roll = filter.getRoll();
-  float pitch = filter.getPitch();
-  float heading = filter.getYaw();
-  Serial.print(millis());
-  Serial.print(" - Orientation: ");
-  Serial.print(heading);
-  Serial.print(" ");
-  Serial.print(pitch);
-  Serial.print(" ");
-  Serial.println(roll);
-    
-  delay(10);
-}
--- a/examples/ahrs_mahony/sample_config_data.png
+++ b/examples/ahrs_mahony/sample_config_data.png
--- a/examples/calibrated_orientation/LSM6DS_LIS3MDL.h
+++ b/examples/calibrated_orientation/LSM6DS_LIS3MDL.h
@ -0,0 +1,35 @@
+#include <Adafruit_LIS3MDL.h>
+Adafruit_LIS3MDL lis3mdl;
+
+// Can change this to be LSM6DSOX or whatever ya like
+// For (older) Feather Sense with LSM6DS33, use this:
+#include <Adafruit_LSM6DS33.h>
+Adafruit_LSM6DS33 lsm6ds;
+// For (newer) Feather Sense with LSM6DS3TR-C, use this:
+// #include <Adafruit_LSM6DS3TRC.h>
+// Adafruit_LSM6DS3TRC lsm6ds;
+
+bool init_sensors(void) {
+  if (!lsm6ds.begin_I2C() || !lis3mdl.begin_I2C()) {
+    return false;
+  }
+  accelerometer = lsm6ds.getAccelerometerSensor();
+  gyroscope = lsm6ds.getGyroSensor();
+  magnetometer = &lis3mdl;
+
+  return true;
+}
+
+void setup_sensors(void) {
+  // set lowest range
+  lsm6ds.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
+  lsm6ds.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
+  lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);
+
+  // set slightly above refresh rate
+  lsm6ds.setAccelDataRate(LSM6DS_RATE_104_HZ);
+  lsm6ds.setGyroDataRate(LSM6DS_RATE_104_HZ);
+  lis3mdl.setDataRate(LIS3MDL_DATARATE_1000_HZ);
+  lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
+  lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
+}
--- a/examples/calibrated_orientation/LSM9DS.h
+++ b/examples/calibrated_orientation/LSM9DS.h
@ -0,0 +1,30 @@
+#include <Adafruit_LSM9DS1.h>
+Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
+
+// Or if you have the older LSM9DS0
+// #include <Adafruit_LSM9DS0.h>
+// Adafruit_LSM9DS0 lsm9ds = Adafruit_LSM9DS0();
+
+bool init_sensors(void) {
+  if (!lsm9ds.begin()) {
+    return false;
+  }
+  accelerometer = &lsm9ds.getAccel();
+  gyroscope = &lsm9ds.getGyro();
+  magnetometer = &lsm9ds.getMag();
+
+  return true;
+}
+
+void setup_sensors(void) {
+  // set lowest range
+#ifdef __LSM9DS0_H__
+  lsm9ds.setupAccel(lsm9ds.LSM9DS0_ACCELRANGE_2G);
+  lsm9ds.setupMag(lsm9ds.LSM9DS0_MAGGAIN_4GAUSS);
+  lsm9ds.setupGyro(lsm9ds.LSM9DS0_GYROSCALE_245DPS);
+#else
+  lsm9ds.setupAccel(lsm9ds.LSM9DS1_ACCELRANGE_2G);
+  lsm9ds.setupMag(lsm9ds.LSM9DS1_MAGGAIN_4GAUSS);
+  lsm9ds.setupGyro(lsm9ds.LSM9DS1_GYROSCALE_245DPS);
+#endif
+}
--- a/examples/calibrated_orientation/LSM9DS1.h
+++ b/examples/calibrated_orientation/LSM9DS1.h
@ -0,0 +1,20 @@
+#include <Adafruit_LSM9DS1.h>
+Adafruit_LSM9DS1 lsm9ds1 = Adafruit_LSM9DS1();
+
+bool init_sensors(void) {
+  if (!lsm9ds1.begin()) {
+    return false;
+  }
+  accelerometer = &lsm9ds1.getAccel();
+  gyroscope = &lsm9ds1.getGyro();
+  magnetometer = &lsm9ds1.getMag();
+
+  return true;
+}
+
+void setup_sensors(void) {
+  // set lowest range
+  lsm9ds1.setupAccel(lsm9ds1.LSM9DS1_ACCELRANGE_2G);
+  lsm9ds1.setupMag(lsm9ds1.LSM9DS1_MAGGAIN_4GAUSS);
+  lsm9ds1.setupGyro(lsm9ds1.LSM9DS1_GYROSCALE_245DPS);
+}
--- a/examples/calibrated_orientation/NXP_FXOS_FXAS.h
+++ b/examples/calibrated_orientation/NXP_FXOS_FXAS.h
@ -0,0 +1,18 @@
+#include <Adafruit_FXAS21002C.h>
+#include <Adafruit_FXOS8700.h>
+
+Adafruit_FXOS8700 fxos = Adafruit_FXOS8700(0x8700A, 0x8700B);
+Adafruit_FXAS21002C fxas = Adafruit_FXAS21002C(0x0021002C);
+
+bool init_sensors(void) {
+  if (!fxos.begin() || !fxas.begin()) {
+    return false;
+  }
+  accelerometer = fxos.getAccelerometerSensor();
+  gyroscope = &fxas;
+  magnetometer = fxos.getMagnetometerSensor();
+
+  return true;
+}
+
+void setup_sensors(void) {}
--- a/examples/calibrated_orientation/calibrated_orientation.ino
+++ b/examples/calibrated_orientation/calibrated_orientation.ino
@ -0,0 +1,144 @@
+// Full orientation sensing using NXP/Madgwick/Mahony and a range of 9-DoF
+// sensor sets.
+// You *must* perform a magnetic calibration before this code will work.
+//
+// To view this data, use the Arduino Serial Monitor to watch the
+// scrolling angles, or run the OrientationVisualiser example in Processing.
+// Based on  https://github.com/PaulStoffregen/NXPMotionSense with adjustments
+// to Adafruit Unified Sensor interface
+
+#include <Adafruit_Sensor_Calibration.h>
+#include <Adafruit_AHRS.h>
+
+Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
+
+// uncomment one combo 9-DoF!
+#include "LSM6DS_LIS3MDL.h"  // see the the LSM6DS_LIS3MDL file in this project to change board to LSM6DS33, LSM6DS3U, LSM6DSOX, etc
+//#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
+//#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout
+
+// pick your filter! slower == better quality output
+//Adafruit_NXPSensorFusion filter; // slowest
+//Adafruit_Madgwick filter;  // faster than NXP
+Adafruit_Mahony filter;  // fastest/smalleset
+
+#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)
+  Adafruit_Sensor_Calibration_EEPROM cal;
+#else
+  Adafruit_Sensor_Calibration_SDFat cal;
+#endif
+
+#define FILTER_UPDATE_RATE_HZ 100
+#define PRINT_EVERY_N_UPDATES 10
+//#define AHRS_DEBUG_OUTPUT
+
+uint32_t timestamp;
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) yield();
+
+  if (!cal.begin()) {
+    Serial.println("Failed to initialize calibration helper");
+  } else if (! cal.loadCalibration()) {
+    Serial.println("No calibration loaded/found");
+  }
+
+  if (!init_sensors()) {
+    Serial.println("Failed to find sensors");
+    while (1) delay(10);
+  }
+  
+  accelerometer->printSensorDetails();
+  gyroscope->printSensorDetails();
+  magnetometer->printSensorDetails();
+
+  setup_sensors();
+  filter.begin(FILTER_UPDATE_RATE_HZ);
+  timestamp = millis();
+
+  Wire.setClock(400000); // 400KHz
+}
+
+
+void loop() {
+  float roll, pitch, heading;
+  float gx, gy, gz;
+  static uint8_t counter = 0;
+
+  if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
+    return;
+  }
+  timestamp = millis();
+  // Read the motion sensors
+  sensors_event_t accel, gyro, mag;
+  accelerometer->getEvent(&accel);
+  gyroscope->getEvent(&gyro);
+  magnetometer->getEvent(&mag);
+#if defined(AHRS_DEBUG_OUTPUT)
+  Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
+#endif
+
+  cal.calibrate(mag);
+  cal.calibrate(accel);
+  cal.calibrate(gyro);
+  // Gyroscope needs to be converted from Rad/s to Degree/s
+  // the rest are not unit-important
+  gx = gyro.gyro.x * SENSORS_RADS_TO_DPS;
+  gy = gyro.gyro.y * SENSORS_RADS_TO_DPS;
+  gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;
+
+  // Update the SensorFusion filter
+  filter.update(gx, gy, gz, 
+                accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, 
+                mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
+#if defined(AHRS_DEBUG_OUTPUT)
+  Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
+#endif
+
+  // only print the calculated output once in a while
+  if (counter++ <= PRINT_EVERY_N_UPDATES) {
+    return;
+  }
+  // reset the counter
+  counter = 0;
+
+#if defined(AHRS_DEBUG_OUTPUT)
+  Serial.print("Raw: ");
+  Serial.print(accel.acceleration.x, 4); Serial.print(", ");
+  Serial.print(accel.acceleration.y, 4); Serial.print(", ");
+  Serial.print(accel.acceleration.z, 4); Serial.print(", ");
+  Serial.print(gx, 4); Serial.print(", ");
+  Serial.print(gy, 4); Serial.print(", ");
+  Serial.print(gz, 4); Serial.print(", ");
+  Serial.print(mag.magnetic.x, 4); Serial.print(", ");
+  Serial.print(mag.magnetic.y, 4); Serial.print(", ");
+  Serial.print(mag.magnetic.z, 4); Serial.println("");
+#endif
+
+  // print the heading, pitch and roll
+  roll = filter.getRoll();
+  pitch = filter.getPitch();
+  heading = filter.getYaw();
+  Serial.print("Orientation: ");
+  Serial.print(heading);
+  Serial.print(", ");
+  Serial.print(pitch);
+  Serial.print(", ");
+  Serial.println(roll);
+
+  float qw, qx, qy, qz;
+  filter.getQuaternion(&qw, &qx, &qy, &qz);
+  Serial.print("Quaternion: ");
+  Serial.print(qw, 4);
+  Serial.print(", ");
+  Serial.print(qx, 4);
+  Serial.print(", ");
+  Serial.print(qy, 4);
+  Serial.print(", ");
+  Serial.println(qz, 4);  
+  
+#if defined(AHRS_DEBUG_OUTPUT)
+  Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms");
+#endif
+}
--- a/examples/calibration/LSM6DS_LIS3MDL.h
+++ b/examples/calibration/LSM6DS_LIS3MDL.h
@ -0,0 +1,35 @@
+#include <Adafruit_LIS3MDL.h>
+Adafruit_LIS3MDL lis3mdl;
+
+// Can change this to be LSM6DS33 or whatever ya like
+// For (older) Feather Sense with LSM6DS33, use this:
+#include <Adafruit_LSM6DS33.h>
+Adafruit_LSM6DS33 lsm6ds;
+// For (newer) Feather Sense with LSM6DS3TR-C, use this:
+// #include <Adafruit_LSM6DS3TRC.h>
+// Adafruit_LSM6DS3TRC lsm6ds;
+
+bool init_sensors(void) {
+  if (!lsm6ds.begin_I2C() || !lis3mdl.begin_I2C()) {
+    return false;
+  }
+  accelerometer = lsm6ds.getAccelerometerSensor();
+  gyroscope = lsm6ds.getGyroSensor();
+  magnetometer = &lis3mdl;
+
+  return true;
+}
+
+void setup_sensors(void) {
+  // set lowest range
+  lsm6ds.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
+  lsm6ds.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
+  lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);
+
+  // set slightly above refresh rate
+  lsm6ds.setAccelDataRate(LSM6DS_RATE_104_HZ);
+  lsm6ds.setGyroDataRate(LSM6DS_RATE_104_HZ);
+  lis3mdl.setDataRate(LIS3MDL_DATARATE_1000_HZ);
+  lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
+  lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
+}
--- a/examples/calibration/LSM9DS.h
+++ b/examples/calibration/LSM9DS.h
@ -0,0 +1,30 @@
+// #include <Adafruit_LSM9DS1.h>
+//  Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
+
+// Or if you have the older LSM9DS0
+#include <Adafruit_LSM9DS0.h>
+Adafruit_LSM9DS0 lsm9ds = Adafruit_LSM9DS0();
+
+bool init_sensors(void) {
+  if (!lsm9ds.begin()) {
+    return false;
+  }
+  accelerometer = &lsm9ds.getAccel();
+  gyroscope = &lsm9ds.getGyro();
+  magnetometer = &lsm9ds.getMag();
+
+  return true;
+}
+
+void setup_sensors(void) {
+  // set lowest range
+#ifdef __LSM9DS0_H__
+  lsm9ds.setupAccel(lsm9ds.LSM9DS0_ACCELRANGE_2G);
+  lsm9ds.setupMag(lsm9ds.LSM9DS0_MAGGAIN_4GAUSS);
+  lsm9ds.setupGyro(lsm9ds.LSM9DS0_GYROSCALE_245DPS);
+#else
+  lsm9ds.setupAccel(lsm9ds.LSM9DS1_ACCELRANGE_2G);
+  lsm9ds.setupMag(lsm9ds.LSM9DS1_MAGGAIN_4GAUSS);
+  lsm9ds.setupGyro(lsm9ds.LSM9DS1_GYROSCALE_245DPS);
+#endif
+}
--- a/examples/calibration/NXP_FXOS_FXAS.h
+++ b/examples/calibration/NXP_FXOS_FXAS.h
@ -0,0 +1,18 @@
+#include <Adafruit_FXAS21002C.h>
+#include <Adafruit_FXOS8700.h>
+
+Adafruit_FXOS8700 fxos = Adafruit_FXOS8700(0x8700A, 0x8700B);
+Adafruit_FXAS21002C fxas = Adafruit_FXAS21002C(0x0021002C);
+
+bool init_sensors(void) {
+  if (!fxos.begin() || !fxas.begin()) {
+    return false;
+  }
+  accelerometer = fxos.getAccelerometerSensor();
+  gyroscope = &fxas;
+  magnetometer = fxos.getMagnetometerSensor();
+
+  return true;
+}
+
+void setup_sensors(void) {}
--- a/examples/calibration/calibration.ino
+++ b/examples/calibration/calibration.ino
@ -0,0 +1,225 @@
+/***************************************************************************
+  This is an example for the Adafruit AHRS library
+  It will look for a supported magnetometer and output
+  PJRC Motion Sensor Calibration Tool-compatible serial data
+
+  PJRC & Adafruit invest time and resources providing this open source code,
+  please support PJRC and open-source hardware by purchasing products
+  from PJRC!
+
+  Written by PJRC, adapted by Limor Fried for Adafruit Industries.
+ ***************************************************************************/
+
+#include <Adafruit_Sensor_Calibration.h>
+
+Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
+
+// uncomment one combo 9-DoF!
+#include "LSM6DS_LIS3MDL.h"  // see the the LSM6DS_LIS3MDL file in this project to change board to LSM6DS33, LSM6DS3U, LSM6DSOX, etc
+//#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
+//#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout
+
+// select either EEPROM or SPI FLASH storage:
+#ifdef ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM
+  Adafruit_Sensor_Calibration_EEPROM cal;
+#else
+  Adafruit_Sensor_Calibration_SDFat cal;
+#endif
+
+sensors_event_t mag_event, gyro_event, accel_event;
+
+int loopcount = 0;
+
+void setup(void) {
+  Serial.begin(115200);
+  while (!Serial) delay(10);     // will pause Zero, Leonardo, etc until serial console opens
+  
+  Serial.println(F("Adafruit AHRS - IMU Calibration!"));
+
+  Serial.println("Calibration filesys test");
+  if (!cal.begin()) {
+    Serial.println("Failed to initialize calibration helper");
+    while (1) yield();
+  }
+  if (! cal.loadCalibration()) {
+    Serial.println("No calibration loaded/found... will start with defaults");
+  } else {
+    Serial.println("Loaded existing calibration");
+  }
+
+  if (!init_sensors()) {
+    Serial.println("Failed to find sensors");
+    while (1) delay(10);
+  }
+  accelerometer->printSensorDetails();
+  gyroscope->printSensorDetails();
+  magnetometer->printSensorDetails();
+
+  setup_sensors();
+  
+  Wire.setClock(400000); // 400KHz
+}
+
+void loop() {
+  magnetometer->getEvent(&mag_event);
+  gyroscope->getEvent(&gyro_event);
+  accelerometer->getEvent(&accel_event);
+  
+  // 'Raw' values to match expectation of MotionCal
+  Serial.print("Raw:");
+  Serial.print(int(accel_event.acceleration.x*8192/9.8)); Serial.print(",");
+  Serial.print(int(accel_event.acceleration.y*8192/9.8)); Serial.print(",");
+  Serial.print(int(accel_event.acceleration.z*8192/9.8)); Serial.print(",");
+  Serial.print(int(gyro_event.gyro.x*SENSORS_RADS_TO_DPS*16)); Serial.print(",");
+  Serial.print(int(gyro_event.gyro.y*SENSORS_RADS_TO_DPS*16)); Serial.print(",");
+  Serial.print(int(gyro_event.gyro.z*SENSORS_RADS_TO_DPS*16)); Serial.print(",");
+  Serial.print(int(mag_event.magnetic.x*10)); Serial.print(",");
+  Serial.print(int(mag_event.magnetic.y*10)); Serial.print(",");
+  Serial.print(int(mag_event.magnetic.z*10)); Serial.println("");
+
+  // unified data
+  Serial.print("Uni:");
+  Serial.print(accel_event.acceleration.x); Serial.print(",");
+  Serial.print(accel_event.acceleration.y); Serial.print(",");
+  Serial.print(accel_event.acceleration.z); Serial.print(",");
+  Serial.print(gyro_event.gyro.x, 4); Serial.print(",");
+  Serial.print(gyro_event.gyro.y, 4); Serial.print(",");
+  Serial.print(gyro_event.gyro.z, 4); Serial.print(",");
+  Serial.print(mag_event.magnetic.x); Serial.print(",");
+  Serial.print(mag_event.magnetic.y); Serial.print(",");
+  Serial.print(mag_event.magnetic.z); Serial.println("");
+  loopcount++;
+  receiveCalibration();
+
+  // occasionally print calibration
+  if (loopcount == 50 || loopcount > 100) {
+    Serial.print("Cal1:");
+    for (int i=0; i<3; i++) {
+      Serial.print(cal.accel_zerog[i], 3); 
+      Serial.print(",");
+    }
+    for (int i=0; i<3; i++) {
+      Serial.print(cal.gyro_zerorate[i], 3);
+      Serial.print(",");
+    }  
+    for (int i=0; i<3; i++) {
+      Serial.print(cal.mag_hardiron[i], 3); 
+      Serial.print(",");
+    }  
+    Serial.println(cal.mag_field, 3);
+    loopcount++;
+  }
+  if (loopcount >= 100) {
+    Serial.print("Cal2:");
+    for (int i=0; i<9; i++) {
+      Serial.print(cal.mag_softiron[i], 4); 
+      if (i < 8) Serial.print(',');
+    }
+    Serial.println();
+    loopcount = 0;
+  }
+ 
+  delay(10); 
+}
+
+/********************************************************/
+
+byte caldata[68]; // buffer to receive magnetic calibration data
+byte calcount=0;
+
+void receiveCalibration() {
+  uint16_t crc;
+  byte b, i;
+
+  while (Serial.available()) {
+    b = Serial.read();
+    if (calcount == 0 && b != 117) {
+      // first byte must be 117
+      return;
+    }
+    if (calcount == 1 && b != 84) {
+      // second byte must be 84
+      calcount = 0;
+      return;
+    }
+    // store this byte
+    caldata[calcount++] = b;
+    if (calcount < 68) {
+      // full calibration message is 68 bytes
+      return;
+    }
+    // verify the crc16 check
+    crc = 0xFFFF;
+    for (i=0; i < 68; i++) {
+      crc = crc16_update(crc, caldata[i]);
+    }
+    if (crc == 0) {
+      // data looks good, use it
+      float offsets[16];
+      memcpy(offsets, caldata+2, 16*4);
+      cal.accel_zerog[0] = offsets[0];
+      cal.accel_zerog[1] = offsets[1];
+      cal.accel_zerog[2] = offsets[2];
+      
+      cal.gyro_zerorate[0] = offsets[3];
+      cal.gyro_zerorate[1] = offsets[4];
+      cal.gyro_zerorate[2] = offsets[5];
+      
+      cal.mag_hardiron[0] = offsets[6];
+      cal.mag_hardiron[1] = offsets[7];
+      cal.mag_hardiron[2] = offsets[8];
+
+      cal.mag_field = offsets[9];
+      
+      cal.mag_softiron[0] = offsets[10];
+      cal.mag_softiron[1] = offsets[13];
+      cal.mag_softiron[2] = offsets[14];
+      cal.mag_softiron[3] = offsets[13];
+      cal.mag_softiron[4] = offsets[11];
+      cal.mag_softiron[5] = offsets[15];
+      cal.mag_softiron[6] = offsets[14];
+      cal.mag_softiron[7] = offsets[15];
+      cal.mag_softiron[8] = offsets[12];
+
+      if (! cal.saveCalibration()) {
+        Serial.println("**WARNING** Couldn't save calibration");
+      } else {
+        Serial.println("Wrote calibration");    
+      }
+      cal.printSavedCalibration();
+      calcount = 0;
+      return;
+    }
+    // look for the 117,84 in the data, before discarding
+    for (i=2; i < 67; i++) {
+      if (caldata[i] == 117 && caldata[i+1] == 84) {
+        // found possible start within data
+        calcount = 68 - i;
+        memmove(caldata, caldata + i, calcount);
+        return;
+      }
+    }
+    // look for 117 in last byte
+    if (caldata[67] == 117) {
+      caldata[0] = 117;
+      calcount = 1;
+    } else {
+      calcount = 0;
+    }
+  }
+}
+
+
+uint16_t crc16_update(uint16_t crc, uint8_t a)
+{
+  int i;
+  crc ^= a;
+  for (i = 0; i < 8; i++) {
+    if (crc & 1) {
+      crc = (crc >> 1) ^ 0xA001;
+    } else {
+      crc = (crc >> 1);
+    }
+  }
+  return crc;
+}
--- a/examples/deprecated/ahrs_10dof/ahrs_10dof.ino
+++ b/examples/deprecated/ahrs_10dof/ahrs_10dof.ino
@ -1,12 +1,13 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_U.h>
+#include <Adafruit_LSM303_Accel.h>
+#include <Adafruit_LSM303DLH_Mag.h>
 #include <Adafruit_BMP085_U.h>
 #include <Adafruit_Simple_AHRS.h>

 // Create sensor instances.
 Adafruit_LSM303_Accel_Unified  accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
+Adafruit_LSM303DLH_Mag_Unified mag(30302);
 Adafruit_BMP085_Unified        bmp(18001);

 // Create simple AHRS algorithm using the above sensors.
--- a/examples/deprecated/ahrs_9dof/ahrs_9dof.ino
+++ b/examples/deprecated/ahrs_9dof/ahrs_9dof.ino
@ -1,11 +1,12 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_U.h>
+#include <Adafruit_LSM303_Accel.h>
+#include <Adafruit_LSM303DLH_Mag.h>
 #include <Adafruit_Simple_AHRS.h>

 // Create sensor instances.
 Adafruit_LSM303_Accel_Unified  accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
+Adafruit_LSM303DLH_Mag_Unified mag(30302);

 // Create simple AHRS algorithm using the above sensors.
 Adafruit_Simple_AHRS          ahrs(&accel, &mag);
--- a/library.properties
+++ b/library.properties
@ -1,9 +1,10 @@
 name=Adafruit AHRS
-version=1.1.4
+version=2.4.0
 author=Adafruit
 maintainer=Adafruit <info@adafruit.com>
-sentence=AHRS (Altitude and Heading Reference System) for Adafruit's 9DOF and 10DOF breakouts
-paragraph=AHRS (Altitude and Heading Reference System) for Adafruit's 9DOF and 10DOF breakouts
+sentence=AHRS (Altitude and Heading Reference System) for various Adafruit motion sensors
+paragraph=Includes motion calibration example sketches, as well as calibration orientation output using Mahony, Madgwick, NXP Fusion, etc fusion filters
 category=Sensors
 url=https://github.com/adafruit/Adafruit_AHRS
 architectures=*
+depends=Adafruit Unified Sensor, Adafruit LSM6DS, Adafruit LIS3MDL, Adafruit FXOS8700, Adafruit FXAS21002C, Adafruit LSM9DS1 Library, Adafruit LSM9DS0 Library, Adafruit BMP085 Unified, Adafruit BluefruitLE nRF51, SdFat - Adafruit Fork, ArduinoJson, Adafruit SPIFlash, Adafruit Sensor Calibration, Adafruit LSM303 Accel, Adafruit LSM303DLH Mag
--- a/src/Adafruit_AHRS.h
+++ b/src/Adafruit_AHRS.h
@ -0,0 +1,26 @@
+/*!
+ * @file Adafruit_AHRS
+ *
+ * @mainpage Adafruit AHRS
+ *
+ * @section intro_sec Introduction
+ *
+ * This library lets you take an accelerometer, gyroscope and magnetometer
+ * and combine the data to create orientation data.
+ *
+ * Options are Mahony (lowest memory/computation),
+ * Madgwick (fair memory/computation), and NXP fusion/Kalman (highest).
+ *
+ * While in theory these can run on an Arduino UNO/Atmega328P we really
+ * recommend a SAMD21 or better. Having single-instruction floating point
+ * multiply and plenty of RAM will help a lot!
+ */
+
+#ifndef __ADAFRUIT_AHRS_H_
+#define __ADAFRUIT_AHRS_H_
+
+#include <Adafruit_AHRS_Madgwick.h>
+#include <Adafruit_AHRS_Mahony.h>
+#include <Adafruit_AHRS_NXPFusion.h>
+
+#endif // __ADAFRUIT_AHRS_H_
--- a/src/Adafruit_AHRS_FusionInterface.h
+++ b/src/Adafruit_AHRS_FusionInterface.h
@ -0,0 +1,105 @@
+/*!
+ * @file Adafruit_AHRS_FusionInterface.h
+ *
+ * @section license License
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2020 Ha Thach (tinyusb.org) for Adafruit Industries
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef ADAFRUIT_AHRS_FUSIONINTERFACE_H_
+#define ADAFRUIT_AHRS_FUSIONINTERFACE_H_
+
+/*!
+ * @brief The common interface for the fusion algorithms.
+ */
+class Adafruit_AHRS_FusionInterface {
+public:
+  /**************************************************************************/
+  /*!
+   * @brief Initializes the sensor fusion filter.
+   *
+   * @param sampleFrequency The sensor sample rate in herz(samples per second).
+   */
+  /**************************************************************************/
+  virtual void begin(float sampleFrequency) = 0;
+
+  /**************************************************************************/
+  /*!
+   * @brief Updates the filter with new gyroscope, accelerometer, and
+   * magnetometer data.
+   *
+   * @param gx The gyroscope x axis. In DPS.
+   * @param gy The gyroscope y axis. In DPS.
+   * @param gz The gyroscope z axis. In DPS.
+   * @param ax The accelerometer x axis. In g.
+   * @param ay The accelerometer y axis. In g.
+   * @param az The accelerometer z axis. In g.
+   * @param mx The magnetometer x axis. In uT.
+   * @param my The magnetometer y axis. In uT.
+   * @param mz The magnetometer z axis. In uT.
+   */
+  /**************************************************************************/
+  virtual void update(float gx, float gy, float gz, float ax, float ay,
+                      float az, float mx, float my, float mz) = 0;
+
+  /**************************************************************************/
+  /*!
+   * @brief Gets the current roll of the sensors.
+   *
+   * @return The current sensor roll.
+   */
+  /**************************************************************************/
+  virtual float getRoll() = 0;
+
+  /**************************************************************************/
+  /*!
+   * @brief Gets the current pitch of the sensors.
+   *
+   * @return The current sensor pitch.
+   */
+  /**************************************************************************/
+  virtual float getPitch() = 0;
+
+  /**************************************************************************/
+  /*!
+   * @brief Gets the current yaw of the sensors.
+   *
+   * @return The current sensor yaw.
+   */
+  /**************************************************************************/
+  virtual float getYaw() = 0;
+  virtual void getQuaternion(float *w, float *x, float *y, float *z) = 0;
+  virtual void setQuaternion(float w, float x, float y, float z) = 0;
+
+  /**************************************************************************/
+  /*!
+   * @brief Gets the current gravity vector of the sensor.
+   *
+   * @param x A float pointer to write the gravity vector x component to. In g.
+   * @param y A float pointer to write the gravity vector y component to. In g.
+   * @param z A float pointer to write the gravity vector z component to. In g.
+   */
+  virtual void getGravityVector(float *x, float *y, float *z) = 0;
+};
+
+#endif /* ADAFRUIT_AHRS_FUSIONINTERFACE_H_ */
--- a/src/Adafruit_AHRS_Madgwick.cpp
+++ b/src/Adafruit_AHRS_Madgwick.cpp
@ -0,0 +1,305 @@
+//=============================================================================================
+// Madgwick.c
+//=============================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date			Author          Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
+//
+//=============================================================================================
+
+//-------------------------------------------------------------------------------------------
+// Header files
+
+#include "Adafruit_AHRS_Madgwick.h"
+#include <math.h>
+
+//-------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreqDef 512.0f // sample frequency in Hz
+#define betaDef 0.1f         // 2 * proportional gain
+
+//============================================================================================
+// Functions
+
+//-------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+Adafruit_Madgwick::Adafruit_Madgwick() : Adafruit_Madgwick(betaDef) {}
+
+Adafruit_Madgwick::Adafruit_Madgwick(float gain) {
+  beta = gain;
+  q0 = 1.0f;
+  q1 = 0.0f;
+  q2 = 0.0f;
+  q3 = 0.0f;
+  invSampleFreq = 1.0f / sampleFreqDef;
+  anglesComputed = false;
+}
+
+void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
+                               float az, float mx, float my, float mz,
+                               float dt) {
+  float recipNorm;
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+  float hx, hy;
+  float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
+      _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
+      q2q2, q2q3, q3q3;
+
+  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in
+  // magnetometer normalisation)
+  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+    updateIMU(gx, gy, gz, ax, ay, az, dt);
+    return;
+  }
+
+  // Convert gyroscope degrees/sec to radians/sec
+  gx *= 0.0174533f;
+  gy *= 0.0174533f;
+  gz *= 0.0174533f;
+
+  // Rate of change of quaternion from gyroscope
+  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in
+  // accelerometer normalisation)
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+    // Normalise accelerometer measurement
+    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Normalise magnetometer measurement
+    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    mx *= recipNorm;
+    my *= recipNorm;
+    mz *= recipNorm;
+
+    // Auxiliary variables to avoid repeated arithmetic
+    _2q0mx = 2.0f * q0 * mx;
+    _2q0my = 2.0f * q0 * my;
+    _2q0mz = 2.0f * q0 * mz;
+    _2q1mx = 2.0f * q1 * mx;
+    _2q0 = 2.0f * q0;
+    _2q1 = 2.0f * q1;
+    _2q2 = 2.0f * q2;
+    _2q3 = 2.0f * q3;
+    _2q0q2 = 2.0f * q0 * q2;
+    _2q2q3 = 2.0f * q2 * q3;
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
+    q1q3 = q1 * q3;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;
+
+    // Reference direction of Earth's magnetic field
+    hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 +
+         _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+    hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 +
+         my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+    _2bx = sqrtf(hx * hx + hy * hy);
+    _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 +
+           _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+    _4bx = 2.0f * _2bx;
+    _4bz = 2.0f * _2bz;
+
+    // Gradient decent algorithm corrective step
+    s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
+         _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * q3 + _2bz * q1) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * q2 + _2bz * q0) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * q3 - _4bz * q1) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         (-_4bx * q2 - _2bz * q0) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * q1 + _2bz * q3) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * q0 - _4bz * q2) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
+         (-_4bx * q3 + _2bz * q1) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * q0 + _2bz * q2) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
+                        s3 * s3); // normalise step magnitude
+    s0 *= recipNorm;
+    s1 *= recipNorm;
+    s2 *= recipNorm;
+    s3 *= recipNorm;
+
+    // Apply feedback step
+    qDot1 -= beta * s0;
+    qDot2 -= beta * s1;
+    qDot3 -= beta * s2;
+    qDot4 -= beta * s3;
+  }
+
+  // Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * dt;
+  q1 += qDot2 * dt;
+  q2 += qDot3 * dt;
+  q3 += qDot4 * dt;
+
+  // Normalise quaternion
+  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+  anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
+                                  float ay, float az, float dt) {
+  float recipNorm;
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
+      q3q3;
+
+  // Convert gyroscope degrees/sec to radians/sec
+  gx *= 0.0174533f;
+  gy *= 0.0174533f;
+  gz *= 0.0174533f;
+
+  // Rate of change of quaternion from gyroscope
+  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in
+  // accelerometer normalisation)
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+    // Normalise accelerometer measurement
+    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Auxiliary variables to avoid repeated arithmetic
+    _2q0 = 2.0f * q0;
+    _2q1 = 2.0f * q1;
+    _2q2 = 2.0f * q2;
+    _2q3 = 2.0f * q3;
+    _4q0 = 4.0f * q0;
+    _4q1 = 4.0f * q1;
+    _4q2 = 4.0f * q2;
+    _8q1 = 8.0f * q1;
+    _8q2 = 8.0f * q2;
+    q0q0 = q0 * q0;
+    q1q1 = q1 * q1;
+    q2q2 = q2 * q2;
+    q3q3 = q3 * q3;
+
+    // Gradient decent algorithm corrective step
+    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 +
+         _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+    s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 +
+         _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+    s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
+                        s3 * s3); // normalise step magnitude
+    s0 *= recipNorm;
+    s1 *= recipNorm;
+    s2 *= recipNorm;
+    s3 *= recipNorm;
+
+    // Apply feedback step
+    qDot1 -= beta * s0;
+    qDot2 -= beta * s1;
+    qDot3 -= beta * s2;
+    qDot4 -= beta * s3;
+  }
+
+  // Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * dt;
+  q1 += qDot2 * dt;
+  q2 += qDot3 * dt;
+  q3 += qDot4 * dt;
+
+  // Normalise quaternion
+  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+  anglesComputed = 0;
+}
+
+void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
+                               float az, float mx, float my, float mz) {
+  update(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq);
+}
+void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
+                                  float ay, float az) {
+  updateIMU(gx, gy, gz, ax, ay, az, invSampleFreq);
+};
+
+//-------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+float Adafruit_Madgwick::invSqrt(float x) {
+  float halfx = 0.5f * x;
+  union {
+    float f;
+    long i;
+  } conv = {x};
+  conv.i = 0x5f3759df - (conv.i >> 1);
+  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  return conv.f;
+}
+
+//-------------------------------------------------------------------------------------------
+
+void Adafruit_Madgwick::computeAngles() {
+  roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
+  pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
+  yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
+  grav[0] = 2.0f * (q1 * q3 - q0 * q2);
+  grav[1] = 2.0f * (q0 * q1 + q2 * q3);
+  grav[2] = 2.0f * (q0 * q0 - 0.5f + q3 * q3);
+  anglesComputed = 1;
+}
--- a/src/Adafruit_AHRS_Madgwick.h
+++ b/src/Adafruit_AHRS_Madgwick.h
@ -0,0 +1,108 @@
+//=============================================================================================
+// Madgwick.h
+//=============================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date			Author          Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+//
+//=============================================================================================
+#ifndef __Adafruit_Madgwick_h__
+#define __Adafruit_Madgwick_h__
+
+#include "Adafruit_AHRS_FusionInterface.h"
+#include <math.h>
+
+//--------------------------------------------------------------------------------------------
+// Variable declaration
+class Adafruit_Madgwick : public Adafruit_AHRS_FusionInterface {
+private:
+  static float invSqrt(float x);
+  float beta; // algorithm gain
+  float q0;
+  float q1;
+  float q2;
+  float q3; // quaternion of sensor frame relative to auxiliary frame
+  float invSampleFreq;
+  float roll, pitch, yaw;
+  float grav[3];
+  bool anglesComputed = false;
+  void computeAngles();
+
+  //-------------------------------------------------------------------------------------------
+  // Function declarations
+public:
+  Adafruit_Madgwick();
+  Adafruit_Madgwick(float gain);
+  void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
+  void update(float gx, float gy, float gz, float ax, float ay, float az,
+              float mx, float my, float mz);
+  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+  void update(float gx, float gy, float gz, float ax, float ay, float az,
+              float mx, float my, float mz, float dt);
+  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az,
+                 float dt);
+  // float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 *
+  // q0 + 2.0f * q3 * q3 - 1.0f);}; float getRoll(){return -1.0f * asinf(2.0f *
+  // q1 * q3 + 2.0f * q0 * q2);}; float getYaw(){return atan2f(2.0f * q1 * q2
+  // - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
+  float getBeta() { return beta; }
+  void setBeta(float beta) { this->beta = beta; }
+  float getRoll() {
+    if (!anglesComputed)
+      computeAngles();
+    return roll * 57.29578f;
+  }
+  float getPitch() {
+    if (!anglesComputed)
+      computeAngles();
+    return pitch * 57.29578f;
+  }
+  float getYaw() {
+    if (!anglesComputed)
+      computeAngles();
+    return yaw * 57.29578f + 180.0f;
+  }
+  float getRollRadians() {
+    if (!anglesComputed)
+      computeAngles();
+    return roll;
+  }
+  float getPitchRadians() {
+    if (!anglesComputed)
+      computeAngles();
+    return pitch;
+  }
+  float getYawRadians() {
+    if (!anglesComputed)
+      computeAngles();
+    return yaw;
+  }
+  void getQuaternion(float *w, float *x, float *y, float *z) {
+    *w = q0;
+    *x = q1;
+    *y = q2;
+    *z = q3;
+  }
+  void setQuaternion(float w, float x, float y, float z) {
+    q0 = w;
+    q1 = x;
+    q2 = y;
+    q3 = z;
+  }
+  void getGravityVector(float *x, float *y, float *z) {
+    if (!anglesComputed)
+      computeAngles();
+    *x = grav[0];
+    *y = grav[1];
+    *z = grav[2];
+  }
+};
+#endif
--- a/src/Adafruit_AHRS_Mahony.cpp
+++ b/src/Adafruit_AHRS_Mahony.cpp
@ -0,0 +1,288 @@
+//=============================================================================================
+// Adafruit_Mahony.c
+//=============================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date			Author			Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+//
+// Algorithm paper:
+// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
+//
+//=============================================================================================
+
+//-------------------------------------------------------------------------------------------
+// Header files
+
+#include "Adafruit_AHRS_Mahony.h"
+#include <math.h>
+
+//-------------------------------------------------------------------------------------------
+// Definitions
+
+#define DEFAULT_SAMPLE_FREQ 512.0f // sample frequency in Hz
+#define twoKpDef (2.0f * 0.5f)     // 2 * proportional gain
+#define twoKiDef (2.0f * 0.0f)     // 2 * integral gain
+
+//============================================================================================
+// Functions
+
+//-------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+Adafruit_Mahony::Adafruit_Mahony() : Adafruit_Mahony(twoKpDef, twoKiDef) {}
+
+Adafruit_Mahony::Adafruit_Mahony(float prop_gain, float int_gain) {
+  twoKp = prop_gain; // 2 * proportional gain (Kp)
+  twoKi = int_gain;  // 2 * integral gain (Ki)
+  q0 = 1.0f;
+  q1 = 0.0f;
+  q2 = 0.0f;
+  q3 = 0.0f;
+  integralFBx = 0.0f;
+  integralFBy = 0.0f;
+  integralFBz = 0.0f;
+  anglesComputed = false;
+  invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
+}
+
+void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
+                             float az, float mx, float my, float mz, float dt) {
+  float recipNorm;
+  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+  float hx, hy, bx, bz;
+  float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
+  float halfex, halfey, halfez;
+  float qa, qb, qc;
+
+  // Use IMU algorithm if magnetometer measurement invalid
+  // (avoids NaN in magnetometer normalisation)
+  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+    updateIMU(gx, gy, gz, ax, ay, az);
+    return;
+  }
+
+  // Convert gyroscope degrees/sec to radians/sec
+  gx *= 0.0174533f;
+  gy *= 0.0174533f;
+  gz *= 0.0174533f;
+
+  // Compute feedback only if accelerometer measurement valid
+  // (avoids NaN in accelerometer normalisation)
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+    // Normalise accelerometer measurement
+    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Normalise magnetometer measurement
+    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    mx *= recipNorm;
+    my *= recipNorm;
+    mz *= recipNorm;
+
+    // Auxiliary variables to avoid repeated arithmetic
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
+    q1q3 = q1 * q3;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;
+
+    // Reference direction of Earth's magnetic field
+    hx = 2.0f *
+         (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+    hy = 2.0f *
+         (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+    bx = sqrtf(hx * hx + hy * hy);
+    bz = 2.0f *
+         (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
+
+    // Estimated direction of gravity and magnetic field
+    halfvx = q1q3 - q0q2;
+    halfvy = q0q1 + q2q3;
+    halfvz = q0q0 - 0.5f + q3q3;
+    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+
+    // Error is sum of cross product between estimated direction
+    // and measured direction of field vectors
+    halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
+    halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
+    halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
+
+    // Compute and apply integral feedback if enabled
+    if (twoKi > 0.0f) {
+      // integral error scaled by Ki
+      integralFBx += twoKi * halfex * dt;
+      integralFBy += twoKi * halfey * dt;
+      integralFBz += twoKi * halfez * dt;
+      gx += integralFBx; // apply integral feedback
+      gy += integralFBy;
+      gz += integralFBz;
+    } else {
+      integralFBx = 0.0f; // prevent integral windup
+      integralFBy = 0.0f;
+      integralFBz = 0.0f;
+    }
+
+    // Apply proportional feedback
+    gx += twoKp * halfex;
+    gy += twoKp * halfey;
+    gz += twoKp * halfez;
+  }
+
+  // Integrate rate of change of quaternion
+  gx *= (0.5f * dt); // pre-multiply common factors
+  gy *= (0.5f * dt);
+  gz *= (0.5f * dt);
+  qa = q0;
+  qb = q1;
+  qc = q2;
+  q0 += (-qb * gx - qc * gy - q3 * gz);
+  q1 += (qa * gx + qc * gz - q3 * gy);
+  q2 += (qa * gy - qb * gz + q3 * gx);
+  q3 += (qa * gz + qb * gy - qc * gx);
+
+  // Normalise quaternion
+  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+  anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
+                                float ay, float az, float dt) {
+  float recipNorm;
+  float halfvx, halfvy, halfvz;
+  float halfex, halfey, halfez;
+  float qa, qb, qc;
+
+  // Convert gyroscope degrees/sec to radians/sec
+  gx *= 0.0174533f;
+  gy *= 0.0174533f;
+  gz *= 0.0174533f;
+
+  // Compute feedback only if accelerometer measurement valid
+  // (avoids NaN in accelerometer normalisation)
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+    // Normalise accelerometer measurement
+    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Estimated direction of gravity
+    halfvx = q1 * q3 - q0 * q2;
+    halfvy = q0 * q1 + q2 * q3;
+    halfvz = q0 * q0 - 0.5f + q3 * q3;
+
+    // Error is sum of cross product between estimated
+    // and measured direction of gravity
+    halfex = (ay * halfvz - az * halfvy);
+    halfey = (az * halfvx - ax * halfvz);
+    halfez = (ax * halfvy - ay * halfvx);
+
+    // Compute and apply integral feedback if enabled
+    if (twoKi > 0.0f) {
+      // integral error scaled by Ki
+      integralFBx += twoKi * halfex * dt;
+      integralFBy += twoKi * halfey * dt;
+      integralFBz += twoKi * halfez * dt;
+      gx += integralFBx; // apply integral feedback
+      gy += integralFBy;
+      gz += integralFBz;
+    } else {
+      integralFBx = 0.0f; // prevent integral windup
+      integralFBy = 0.0f;
+      integralFBz = 0.0f;
+    }
+
+    // Apply proportional feedback
+    gx += twoKp * halfex;
+    gy += twoKp * halfey;
+    gz += twoKp * halfez;
+  }
+
+  // Integrate rate of change of quaternion
+  gx *= (0.5f * dt); // pre-multiply common factors
+  gy *= (0.5f * dt);
+  gz *= (0.5f * dt);
+  qa = q0;
+  qb = q1;
+  qc = q2;
+  q0 += (-qb * gx - qc * gy - q3 * gz);
+  q1 += (qa * gx + qc * gz - q3 * gy);
+  q2 += (qa * gy - qb * gz + q3 * gx);
+  q3 += (qa * gz + qb * gy - qc * gx);
+
+  // Normalise quaternion
+  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+  anglesComputed = 0;
+}
+
+void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
+                             float az, float mx, float my, float mz) {
+  update(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq);
+}
+
+void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
+                                float ay, float az) {
+  updateIMU(gx, gy, gz, ax, ay, az, invSampleFreq);
+};
+
+//-------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+float Adafruit_Mahony::invSqrt(float x) {
+  float halfx = 0.5f * x;
+  union {
+    float f;
+    long i;
+  } conv = {x};
+  conv.i = 0x5f3759df - (conv.i >> 1);
+  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  return conv.f;
+}
+
+//-------------------------------------------------------------------------------------------
+
+void Adafruit_Mahony::computeAngles() {
+  roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
+  pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
+  yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
+  grav[0] = 2.0f * (q1 * q3 - q0 * q2);
+  grav[1] = 2.0f * (q0 * q1 + q2 * q3);
+  grav[2] = 2.0f * (q0 * q0 - 0.5f + q3 * q3);
+  anglesComputed = 1;
+}
+
+//============================================================================================
+// END OF CODE
+//============================================================================================
--- a/src/Adafruit_AHRS_Mahony.h
+++ b/src/Adafruit_AHRS_Mahony.h
@ -0,0 +1,106 @@
+//=============================================================================================
+// Adafruit_AHRS_Mahony.h
+//=============================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// Date			Author			Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+//
+//=============================================================================================
+#ifndef __Adafruit_Mahony_h__
+#define __Adafruit_Mahony_h__
+
+#include "Adafruit_AHRS_FusionInterface.h"
+#include <math.h>
+
+//--------------------------------------------------------------------------------------------
+// Variable declaration
+
+class Adafruit_Mahony : public Adafruit_AHRS_FusionInterface {
+private:
+  float twoKp; // 2 * proportional gain (Kp)
+  float twoKi; // 2 * integral gain (Ki)
+  float q0, q1, q2,
+      q3; // quaternion of sensor frame relative to auxiliary frame
+  float integralFBx, integralFBy,
+      integralFBz; // integral error terms scaled by Ki
+  float invSampleFreq;
+  float roll, pitch, yaw;
+  float grav[3];
+  bool anglesComputed = false;
+  static float invSqrt(float x);
+  void computeAngles();
+
+  //-------------------------------------------------------------------------------------------
+  // Function declarations
+
+public:
+  Adafruit_Mahony();
+  Adafruit_Mahony(float prop_gain, float int_gain);
+  void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
+  void update(float gx, float gy, float gz, float ax, float ay, float az,
+              float mx, float my, float mz);
+  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+  void update(float gx, float gy, float gz, float ax, float ay, float az,
+              float mx, float my, float mz, float dt);
+  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az,
+                 float dt);
+  float getKp() { return twoKp / 2.0f; }
+  void setKp(float Kp) { twoKp = 2.0f * Kp; }
+  float getKi() { return twoKi / 2.0f; }
+  void setKi(float Ki) { twoKi = 2.0f * Ki; }
+  float getRoll() {
+    if (!anglesComputed)
+      computeAngles();
+    return roll * 57.29578f;
+  }
+  float getPitch() {
+    if (!anglesComputed)
+      computeAngles();
+    return pitch * 57.29578f;
+  }
+  float getYaw() {
+    if (!anglesComputed)
+      computeAngles();
+    return yaw * 57.29578f + 180.0f;
+  }
+  float getRollRadians() {
+    if (!anglesComputed)
+      computeAngles();
+    return roll;
+  }
+  float getPitchRadians() {
+    if (!anglesComputed)
+      computeAngles();
+    return pitch;
+  }
+  float getYawRadians() {
+    if (!anglesComputed)
+      computeAngles();
+    return yaw;
+  }
+  void getQuaternion(float *w, float *x, float *y, float *z) {
+    *w = q0;
+    *x = q1;
+    *y = q2;
+    *z = q3;
+  }
+  void setQuaternion(float w, float x, float y, float z) {
+    q0 = w;
+    q1 = x;
+    q2 = y;
+    q3 = z;
+  }
+  void getGravityVector(float *x, float *y, float *z) {
+    if (!anglesComputed)
+      computeAngles();
+    *x = grav[0];
+    *y = grav[1];
+    *z = grav[2];
+  }
+};
+
+#endif
--- a/src/Adafruit_AHRS_NXPFusion.cpp
+++ b/src/Adafruit_AHRS_NXPFusion.cpp
--- a/src/Adafruit_AHRS_NXPFusion.h
+++ b/src/Adafruit_AHRS_NXPFusion.h
@ -0,0 +1,217 @@
+/*!
+ * @file Adafruit_AHRS_NXPFusion.h
+ *
+ * @section license License
+ *
+ * This is a modification of
+ * https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/blob/master/Sources/tasks.h
+ * by PJRC / Paul Stoffregen https://github.com/PaulStoffregen/NXPMotionSense
+ *
+ * Copyright (c) 2014, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor, Inc. nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __Adafruit_Nxp_Fusion_h_
+#define __Adafruit_Nxp_Fusion_h_
+
+#include "Adafruit_AHRS_FusionInterface.h"
+#include <Arduino.h>
+
+/*!
+ * @brief Kalman/NXP Fusion algorithm.
+ */
+class Adafruit_NXPSensorFusion : public Adafruit_AHRS_FusionInterface {
+public:
+  /**************************************************************************/
+  /*!
+   * @brief Initializes the 9DOF Kalman filter.
+   *
+   * @param sampleFrequency The sensor sample rate in herz(samples per second).
+   */
+  /**************************************************************************/
+  void begin(float sampleFrequency = 100.0f);
+
+  /**************************************************************************/
+  /*!
+   * @brief Updates the filter with new gyroscope, accelerometer, and
+   * magnetometer data. For roll, pitch, and yaw the accelerometer values can be
+   * either m/s^2 or g, but for linear acceleration they have to be in g.
+   *
+   * 9DOF orientation function implemented using a 12 element Kalman filter
+   *
+   * void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
+   * const AccelSensor_t *Accel, const MagSensor_t *Mag,
+   * const GyroSensor_t *Gyro, const MagCalibration_t *MagCal)
+   *
+   * @param gx The gyroscope x axis. In DPS.
+   * @param gy The gyroscope y axis. In DPS.
+   * @param gz The gyroscope z axis. In DPS.
+   * @param ax The accelerometer x axis. In g.
+   * @param ay The accelerometer y axis. In g.
+   * @param az The accelerometer z axis. In g.
+   * @param mx The magnetometer x axis. In uT.
+   * @param my The magnetometer y axis. In uT.
+   * @param mz The magnetometer z axis. In uT.
+   */
+  /**************************************************************************/
+  void update(float gx, float gy, float gz, float ax, float ay, float az,
+              float mx, float my, float mz);
+
+  float getRoll() { return PhiPl; }
+  float getPitch() { return ThePl; }
+  float getYaw() { return PsiPl; }
+
+  void getQuaternion(float *w, float *x, float *y, float *z) {
+    *w = qPl.q0;
+    *x = qPl.q1;
+    *y = qPl.q2;
+    *z = qPl.q3;
+  }
+
+  void setQuaternion(float w, float x, float y, float z) {
+    qPl.q0 = w;
+    qPl.q1 = x;
+    qPl.q2 = y;
+    qPl.q3 = z;
+  }
+  /**************************************************************************/
+  /*!
+   * @brief Get the linear acceleration part of the acceleration value given to
+   * update.
+   *
+   * @param x The pointer to write the linear acceleration x axis to. In g.
+   * @param y The pointer to write the linear acceleration y axis to. In g.
+   * @param z The pointer to write the linear acceleration z axis to. In g.
+   */
+  /**************************************************************************/
+  void getLinearAcceleration(float *x, float *y, float *z) const {
+    *x = aSePl[0];
+    *y = aSePl[1];
+    *z = aSePl[2];
+  }
+
+  /**************************************************************************/
+  /*!
+   * @brief Get the gravity vector from the gyroscope values.
+   *
+   * @param x A float pointer to write the gravity vector x component to. In g.
+   * @param y A float pointer to write the gravity vector y component to. In g.
+   * @param z A float pointer to write the gravity vector z component to. In g.
+   */
+  /**************************************************************************/
+  void getGravityVector(float *x, float *y, float *z) {
+    *x = gSeGyMi[0];
+    *y = gSeGyMi[1];
+    *z = gSeGyMi[2];
+  }
+
+  /**************************************************************************/
+  /*!
+   * @brief Get the geomagnetic vector in global frame.
+   *
+   * @param x The pointer to write the geomagnetic vector x axis to. In uT.
+   * @param y The pointer to write the geomagnetic vector y axis to. In uT.
+   * @param z The pointer to write the geomagnetic vector z axis to. In uT.
+   */
+  /**************************************************************************/
+  void getGeomagneticVector(float *x, float *y, float *z) const {
+    *x = mGl[0];
+    *y = mGl[1];
+    *z = mGl[2];
+  }
+
+  typedef struct {
+    float q0; // w
+    float q1; // x
+    float q2; // y
+    float q3; // z
+  } Quaternion_t;
+
+  // These are Madgwick & Mahony - extrinsic rotation reference (wrong!)
+  // float getPitch() {return atan2f(2.0f * qPl.q2 * qPl.q3 - 2.0f * qPl.q0 *
+  // qPl.q1, 2.0f * qPl.q0 * qPl.q0 + 2.0f * qPl.q3 * qPl.q3 - 1.0f);}; float
+  // getRoll() {return -1.0f * asinf(2.0f * qPl.q1 * qPl.q3 + 2.0f * qPl.q0 *
+  // qPl.q2);}; float getYaw() {return atan2f(2.0f * qPl.q1 * qPl.q2 - 2.0f *
+  // qPl.q0 * qPl.q3, 2.0f * qPl.q0 * qPl.q0 + 2.0f * qPl.q1 * qPl.q1 - 1.0f);};
+
+private:
+  float PhiPl; // roll (deg)
+  float ThePl; // pitch (deg)
+  float PsiPl; // yaw (deg)
+  float RhoPl; // compass (deg)
+  float ChiPl; // tilt from vertical (deg)
+  // orientation matrix, quaternion and rotation vector
+  float RPl[3][3];  // a posteriori orientation matrix
+  Quaternion_t qPl; // a posteriori orientation quaternion
+  float RVecPl[3];  // rotation vector
+  // angular velocity
+  float Omega[3]; // angular velocity (deg/s)
+  // systick timer for benchmarking
+  int32_t systick; // systick timer;
+  // end: elements common to all motion state vectors
+
+  // elements transmitted over bluetooth in kalman packet
+  float bPl[3];     // gyro offset (deg/s)
+  float ThErrPl[3]; // orientation error (deg)
+  float bErrPl[3];  // gyro offset error (deg/s)
+  // end elements transmitted in kalman packet
+
+  float dErrGlPl[3]; // magnetic disturbance error (uT, global frame)
+  float dErrSePl[3]; // magnetic disturbance error (uT, sensor frame)
+  float aErrSePl[3]; // linear acceleration error (g, sensor frame)
+  float aSeMi[3];    // linear acceleration (g, sensor frame)
+  float DeltaPl;     // inclination angle (deg)
+  float aSePl[3];    // linear acceleration (g, sensor frame)
+  float aGlPl[3];    // linear acceleration (g, global frame)
+  float gErrSeMi[3]; // difference (g, sensor frame) of gravity vector (accel)
+                     // and gravity vector (gyro)
+  float mErrSeMi[3]; // difference (uT, sensor frame) of geomagnetic vector
+                     // (magnetometer) and geomagnetic vector (gyro)
+  float gSeGyMi[3];  // gravity vector (g, sensor frame) measurement from gyro
+  float
+      mSeGyMi[3]; // geomagnetic vector (uT, sensor frame) measurement from gyro
+  float mGl[3];   // geomagnetic vector (uT, global frame)
+  float QvAA;     // accelerometer terms of Qv
+  float QvMM;     // magnetometer terms of Qv
+  float PPlus12x12[12][12]; // covariance matrix P+
+  float K12x6[12][6];       // kalman filter gain matrix K
+  float Qw12x12[12][12];    // covariance matrix Qw
+  float C6x12[6][12];       // measurement matrix C
+  float RMi[3][3];          // a priori orientation matrix
+  Quaternion_t Deltaq;      // delta quaternion
+  Quaternion_t qMi;         // a priori orientation quaternion
+  float casq;               // FCA * FCA;
+  float cdsq;               // FCD * FCD;
+  float Fastdeltat;         // sensor sampling interval (s) = 1 / SENSORFS
+  float deltat;     // kalman filter sampling interval (s) = OVERSAMPLE_RATIO /
+                    // SENSORFS
+  float deltatsq;   // fdeltat * fdeltat
+  float QwbplusQvG; // FQWB + FQVG
+  int8_t
+      FirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF
+  int8_t resetflag;         // flag to request re-initialization on next pass
+};
+
+#endif
--- a/src/Adafruit_AHRS_NXPmatrix.c
+++ b/src/Adafruit_AHRS_NXPmatrix.c
@ -0,0 +1,467 @@
+// Copyright (c) 2014, Freescale Semiconductor, Inc.
+// All rights reserved.
+// vim: set ts=4:
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of Freescale Semiconductor, Inc. nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
+// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// This file contains matrix manipulation functions.
+//
+
+#include <inttypes.h>
+#include <math.h>
+
+// compile time constants that are private to this file
+#define CORRUPTMATRIX 0.001F // column vector modulus limit for rotation matrix
+
+// vector components
+#define X 0
+#define Y 1
+#define Z 2
+
+// function sets the 3x3 matrix A to the identity matrix
+void f3x3matrixAeqI(float A[][3]) {
+  float *pAij; // pointer to A[i][j]
+  int8_t i, j; // loop counters
+
+  for (i = 0; i < 3; i++) {
+    // set pAij to &A[i][j=0]
+    pAij = A[i];
+    for (j = 0; j < 3; j++) {
+      *(pAij++) = 0.0F;
+    }
+    A[i][i] = 1.0F;
+  }
+}
+
+// function sets the matrix A to the identity matrix
+void fmatrixAeqI(float *A[], int16_t rc) {
+  // rc = rows and columns in A
+
+  float *pAij; // pointer to A[i][j]
+  int8_t i, j; // loop counters
+
+  for (i = 0; i < rc; i++) {
+    // set pAij to &A[i][j=0]
+    pAij = A[i];
+    for (j = 0; j < rc; j++) {
+      *(pAij++) = 0.0F;
+    }
+    A[i][i] = 1.0F;
+  }
+}
+
+// function sets every entry in the 3x3 matrix A to a constant scalar
+void f3x3matrixAeqScalar(float A[][3], float Scalar) {
+  float *pAij; // pointer to A[i][j]
+  int8_t i, j; // counters
+
+  for (i = 0; i < 3; i++) {
+    // set pAij to &A[i][j=0]
+    pAij = A[i];
+    for (j = 0; j < 3; j++) {
+      *(pAij++) = Scalar;
+    }
+  }
+}
+
+// function multiplies all elements of 3x3 matrix A by the specified scalar
+void f3x3matrixAeqAxScalar(float A[][3], float Scalar) {
+  float *pAij; // pointer to A[i][j]
+  int8_t i, j; // loop counters
+
+  for (i = 0; i < 3; i++) {
+    // set pAij to &A[i][j=0]
+    pAij = A[i];
+    for (j = 0; j < 3; j++) {
+      *(pAij++) *= Scalar;
+    }
+  }
+}
+
+// function negates all elements of 3x3 matrix A
+void f3x3matrixAeqMinusA(float A[][3]) {
+  float *pAij; // pointer to A[i][j]
+  int8_t i, j; // loop counters
+
+  for (i = 0; i < 3; i++) {
+    // set pAij to &A[i][j=0]
+    pAij = A[i];
+    for (j = 0; j < 3; j++) {
+      *pAij = -*pAij;
+      pAij++;
+    }
+  }
+}
+
+// function directly calculates the symmetric inverse of a symmetric 3x3 matrix
+// only the on and above diagonal terms in B are used and need to be specified
+void f3x3matrixAeqInvSymB(float A[][3], float B[][3]) {
+  float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2]
+  float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2]
+  float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2]
+  float ftmp;           // determinant and then reciprocal
+
+  // calculate useful products
+  fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
+  fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
+  fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
+
+  // set ftmp to the determinant of the input matrix B
+  ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 +
+         B[0][2] * fB01B12mB11B02;
+
+  // set A to the inverse of B for any determinant except zero
+  if (ftmp != 0.0F) {
+    ftmp = 1.0F / ftmp;
+    A[0][0] = fB11B22mB12B12 * ftmp;
+    A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
+    A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
+    A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
+    A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
+    A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
+  } else {
+    // provide the identity matrix if the determinant is zero
+    f3x3matrixAeqI(A);
+  }
+}
+
+// function calculates the determinant of a 3x3 matrix
+float f3x3matrixDetA(float A[][3]) {
+  return (A[X][X] * (A[Y][Y] * A[Z][Z] - A[Y][Z] * A[Z][Y]) +
+          A[X][Y] * (A[Y][Z] * A[Z][X] - A[Y][X] * A[Z][Z]) +
+          A[X][Z] * (A[Y][X] * A[Z][Y] - A[Y][Y] * A[Z][X]));
+}
+
+// function computes all eigenvalues and eigenvectors of a real symmetric matrix
+// A[0..n-1][0..n-1] stored in the top left of a 10x10 array A[10][10] A[][] is
+// changed on output. eigval[0..n-1] returns the eigenvalues of A[][].
+// eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
+// the eigenvectors are not sorted by value
+void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n) {
+  // maximum number of iterations to achieve convergence: in practice 6 is
+  // typical
+#define NITERATIONS 15
+
+  // various trig functions of the jacobi rotation angle phi
+  float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
+  // scratch variable to prevent over-writing during rotations
+  float ftmp;
+  // residue from remaining non-zero above diagonal terms
+  float residue;
+  // matrix row and column indices
+  int8_t ir, ic;
+  // general loop counter
+  int8_t j;
+  // timeout ctr for number of passes of the algorithm
+  int8_t ctr;
+
+  // initialize eigenvectors matrix and eigenvalues array
+  for (ir = 0; ir < n; ir++) {
+    // loop over all columns
+    for (ic = 0; ic < n; ic++) {
+      // set on diagonal and off-diagonal elements to zero
+      eigvec[ir][ic] = 0.0F;
+    }
+
+    // correct the diagonal elements to 1.0
+    eigvec[ir][ir] = 1.0F;
+
+    // initialize the array of eigenvalues to the diagonal elements of m
+    eigval[ir] = A[ir][ir];
+  }
+
+  // initialize the counter and loop until converged or NITERATIONS reached
+  ctr = 0;
+  do {
+    // compute the absolute value of the above diagonal elements as exit
+    // criterion
+    residue = 0.0F;
+    // loop over rows excluding last row
+    for (ir = 0; ir < n - 1; ir++) {
+      // loop over above diagonal columns
+      for (ic = ir + 1; ic < n; ic++) {
+        // accumulate the residual off diagonal terms which are being driven to
+        // zero
+        residue += fabsf(A[ir][ic]);
+      }
+    }
+
+    // check if we still have work to do
+    if (residue > 0.0F) {
+      // loop over all rows with the exception of the last row (since only
+      // rotating above diagonal elements)
+      for (ir = 0; ir < n - 1; ir++) {
+        // loop over columns ic (where ic is always greater than ir since above
+        // diagonal)
+        for (ic = ir + 1; ic < n; ic++) {
+          // only continue with this element if the element is non-zero
+          if (fabsf(A[ir][ic]) > 0.0F) {
+            // calculate cot(2*phi) where phi is the Jacobi rotation angle
+            cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
+
+            // calculate tan(phi) correcting sign to ensure the smaller solution
+            // is used
+            tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
+            if (cot2phi < 0.0F) {
+              tanphi = -tanphi;
+            }
+
+            // calculate the sine and cosine of the Jacobi rotation angle phi
+            cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
+            sinphi = tanphi * cosphi;
+
+            // calculate tan(phi/2)
+            tanhalfphi = sinphi / (1.0F + cosphi);
+
+            // set tmp = tan(phi) times current matrix element used in update of
+            // leading diagonal elements
+            ftmp = tanphi * A[ir][ic];
+
+            // apply the jacobi rotation to diagonal elements [ir][ir] and
+            // [ic][ic] stored in the eigenvalue array eigval[ir] = eigval[ir] -
+            // tan(phi) *  A[ir][ic]
+            eigval[ir] -= ftmp;
+            // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
+            eigval[ic] += ftmp;
+
+            // by definition, applying the jacobi rotation on element ir, ic
+            // results in 0.0
+            A[ir][ic] = 0.0F;
+
+            // apply the jacobi rotation to all elements of the eigenvector
+            // matrix
+            for (j = 0; j < n; j++) {
+              // store eigvec[j][ir]
+              ftmp = eigvec[j][ir];
+              // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] +
+              // tan(phi/2) * eigvec[j][ir])
+              eigvec[j][ir] =
+                  ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
+              // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] -
+              // tan(phi/2) * eigvec[j][ic])
+              eigvec[j][ic] =
+                  eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
+            }
+
+            // apply the jacobi rotation only to those elements of matrix m that
+            // can change
+            for (j = 0; j <= ir - 1; j++) {
+              // store A[j][ir]
+              ftmp = A[j][ir];
+              // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) *
+              // A[j][ir])
+              A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
+              // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) *
+              // A[j][ic])
+              A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
+            }
+            for (j = ir + 1; j <= ic - 1; j++) {
+              // store A[ir][j]
+              ftmp = A[ir][j];
+              // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) *
+              // A[ir][j])
+              A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
+              // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) *
+              // A[j][ic])
+              A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
+            }
+            for (j = ic + 1; j < n; j++) {
+              // store A[ir][j]
+              ftmp = A[ir][j];
+              // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) *
+              // A[ir][j])
+              A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
+              // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) *
+              // A[ic][j])
+              A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
+            }
+          } // end of test for matrix element already zero
+        } // end of loop over columns
+      } // end of loop over rows
+    } // end of test for non-zero residue
+  } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
+}
+
+// function uses Gauss-Jordan elimination to compute the inverse of matrix A in
+// situ on exit, A is replaced with its inverse
+void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[],
+                    int8_t iPivot[], int8_t isize) {
+  float largest;               // largest element used for pivoting
+  float scaling;               // scaling factor in pivoting
+  float recippiv;              // reciprocal of pivot element
+  float ftmp;                  // temporary variable used in swaps
+  int8_t i, j, k, l, m;        // index counters
+  int8_t iPivotRow, iPivotCol; // row and column of pivot element
+
+  // to avoid compiler warnings
+  iPivotRow = iPivotCol = 0;
+
+  // initialize the pivot array to 0
+  for (j = 0; j < isize; j++) {
+    iPivot[j] = 0;
+  }
+
+  // main loop i over the dimensions of the square matrix A
+  for (i = 0; i < isize; i++) {
+    // zero the largest element found for pivoting
+    largest = 0.0F;
+    // loop over candidate rows j
+    for (j = 0; j < isize; j++) {
+      // check if row j has been previously pivoted
+      if (iPivot[j] != 1) {
+        // loop over candidate columns k
+        for (k = 0; k < isize; k++) {
+          // check if column k has previously been pivoted
+          if (iPivot[k] == 0) {
+            // check if the pivot element is the largest found so far
+            if (fabsf(A[j][k]) >= largest) {
+              // and store this location as the current best candidate for
+              // pivoting
+              iPivotRow = j;
+              iPivotCol = k;
+              largest = (float)fabsf(A[iPivotRow][iPivotCol]);
+            }
+          } else if (iPivot[k] > 1) {
+            // zero determinant situation: exit with identity matrix
+            fmatrixAeqI(A, isize);
+            return;
+          }
+        }
+      }
+    }
+    // increment the entry in iPivot to denote it has been selected for pivoting
+    iPivot[iPivotCol]++;
+
+    // check the pivot rows iPivotRow and iPivotCol are not the same before
+    // swapping
+    if (iPivotRow != iPivotCol) {
+      // loop over columns l
+      for (l = 0; l < isize; l++) {
+        // and swap all elements of rows iPivotRow and iPivotCol
+        ftmp = A[iPivotRow][l];
+        A[iPivotRow][l] = A[iPivotCol][l];
+        A[iPivotCol][l] = ftmp;
+      }
+    }
+
+    // record that on the i-th iteration rows iPivotRow and iPivotCol were
+    // swapped
+    iRowInd[i] = iPivotRow;
+    iColInd[i] = iPivotCol;
+
+    // check for zero on-diagonal element (singular matrix) and return with
+    // identity matrix if detected
+    if (A[iPivotCol][iPivotCol] == 0.0F) {
+      // zero determinant situation: exit with identity matrix
+      fmatrixAeqI(A, isize);
+      return;
+    }
+
+    // calculate the reciprocal of the pivot element knowing it's non-zero
+    recippiv = 1.0F / A[iPivotCol][iPivotCol];
+    // by definition, the diagonal element normalizes to 1
+    A[iPivotCol][iPivotCol] = 1.0F;
+    // multiply all of row iPivotCol by the reciprocal of the pivot element
+    // including the diagonal element the diagonal element
+    // A[iPivotCol][iPivotCol] now has value equal to the reciprocal of its
+    // previous value
+    for (l = 0; l < isize; l++) {
+      A[iPivotCol][l] *= recippiv;
+    }
+    // loop over all rows m of A
+    for (m = 0; m < isize; m++) {
+      if (m != iPivotCol) {
+        // scaling factor for this row m is in column iPivotCol
+        scaling = A[m][iPivotCol];
+        // zero this element
+        A[m][iPivotCol] = 0.0F;
+        // loop over all columns l of A and perform elimination
+        for (l = 0; l < isize; l++) {
+          A[m][l] -= A[iPivotCol][l] * scaling;
+        }
+      }
+    }
+  } // end of loop i over the matrix dimensions
+
+  // finally, loop in inverse order to apply the missing column swaps
+  for (l = isize - 1; l >= 0; l--) {
+    // set i and j to the two columns to be swapped
+    i = iRowInd[l];
+    j = iColInd[l];
+
+    // check that the two columns i and j to be swapped are not the same
+    if (i != j) {
+      // loop over all rows k to swap columns i and j of A
+      for (k = 0; k < isize; k++) {
+        ftmp = A[k][i];
+        A[k][i] = A[k][j];
+        A[k][j] = ftmp;
+      }
+    }
+  }
+}
+
+// function re-orthonormalizes a 3x3 rotation matrix
+void fmatrixAeqRenormRotA(float A[][3]) {
+  float ftmp; // scratch variable
+
+  // normalize the X column of the low pass filtered orientation matrix
+  ftmp = sqrtf(A[X][X] * A[X][X] + A[Y][X] * A[Y][X] + A[Z][X] * A[Z][X]);
+  if (ftmp > CORRUPTMATRIX) {
+    // normalize the x column vector
+    ftmp = 1.0F / ftmp;
+    A[X][X] *= ftmp;
+    A[Y][X] *= ftmp;
+    A[Z][X] *= ftmp;
+  } else {
+    // set x column vector to {1, 0, 0}
+    A[X][X] = 1.0F;
+    A[Y][X] = A[Z][X] = 0.0F;
+  }
+
+  // force the y column vector to be orthogonal to x using y = y-(x.y)x
+  ftmp = A[X][X] * A[X][Y] + A[Y][X] * A[Y][Y] + A[Z][X] * A[Z][Y];
+  A[X][Y] -= ftmp * A[X][X];
+  A[Y][Y] -= ftmp * A[Y][X];
+  A[Z][Y] -= ftmp * A[Z][X];
+
+  // normalize the y column vector
+  ftmp = sqrtf(A[X][Y] * A[X][Y] + A[Y][Y] * A[Y][Y] + A[Z][Y] * A[Z][Y]);
+  if (ftmp > CORRUPTMATRIX) {
+    // normalize the y column vector
+    ftmp = 1.0F / ftmp;
+    A[X][Y] *= ftmp;
+    A[Y][Y] *= ftmp;
+    A[Z][Y] *= ftmp;
+  } else {
+    // set y column vector to {0, 1, 0}
+    A[Y][Y] = 1.0F;
+    A[X][Y] = A[Z][Y] = 0.0F;
+  }
+
+  // finally set the z column vector to x vector cross y vector (automatically
+  // normalized)
+  A[X][Z] = A[Y][X] * A[Z][Y] - A[Z][X] * A[Y][Y];
+  A[Y][Z] = A[Z][X] * A[X][Y] - A[X][X] * A[Z][Y];
+  A[Z][Z] = A[X][X] * A[Y][Y] - A[Y][X] * A[X][Y];
+}
--- a/src/Adafruit_Sensor_Set.h
+++ b/src/Adafruit_Sensor_Set.h
@ -4,11 +4,10 @@
 #include <Adafruit_Sensor.h>

 // Interface for a device that has multiple sensors that can be queried by type.
-class Adafruit_Sensor_Set
-{
+class Adafruit_Sensor_Set {
 public:
  virtual ~Adafruit_Sensor_Set() {}
-  virtual Adafruit_Sensor* getSensor(sensors_type_t type) { return NULL; }
+  virtual Adafruit_Sensor *getSensor(sensors_type_t type) { return NULL; }
 };

 #endif
--- a/src/Adafruit_Simple_AHRS.cpp
+++ b/src/Adafruit_Simple_AHRS.cpp
@ -0,0 +1,101 @@
+/*!
+ * @file Adafruit_Simple_AHRS.cpp
+ */
+
+#include "Adafruit_Simple_AHRS.h"
+
+/**************************************************************************/
+/*!
+ * @brief Create a simple AHRS from a device with multiple sensors.
+ */
+/**************************************************************************/
+Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor *accelerometer,
+                                           Adafruit_Sensor *magnetometer)
+    : _accel(accelerometer), _mag(magnetometer) {}
+
+/**************************************************************************/
+/*!
+ * @brief Create a simple AHRS from a device with multiple sensors.
+ */
+/**************************************************************************/
+Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor_Set &sensors)
+    : _accel(sensors.getSensor(SENSOR_TYPE_ACCELEROMETER)),
+      _mag(sensors.getSensor(SENSOR_TYPE_MAGNETIC_FIELD)) {}
+
+/**************************************************************************/
+/*!
+ * @brief Compute orientation based on accelerometer and magnetometer data.
+ */
+/**************************************************************************/
+bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t *orientation) {
+  // Validate input and available sensors.
+  if (orientation == NULL || _accel == NULL || _mag == NULL)
+    return false;
+
+  // Grab an acceleromter and magnetometer reading.
+  sensors_event_t accel_event;
+  _accel->getEvent(&accel_event);
+  sensors_event_t mag_event;
+  _mag->getEvent(&mag_event);
+
+  float const PI_F = 3.14159265F;
+
+  // roll: Rotation around the X-axis. -180 <= roll <= 180
+  // a positive roll angle is defined to be a clockwise rotation about the
+  // positive X-axis
+  //
+  //                    y
+  //      roll = atan2(---)
+  //                    z
+  //
+  // where:  y, z are returned value from accelerometer sensor
+  orientation->roll =
+      (float)atan2(accel_event.acceleration.y, accel_event.acceleration.z);
+
+  // pitch: Rotation around the Y-axis. -180 <= roll <= 180
+  // a positive pitch angle is defined to be a clockwise rotation about the
+  // positive Y-axis
+  //
+  //                                 -x
+  //      pitch = atan(-------------------------------)
+  //                    y * sin(roll) + z * cos(roll)
+  //
+  // where:  x, y, z are returned value from accelerometer sensor
+  if (accel_event.acceleration.y * sin(orientation->roll) +
+          accel_event.acceleration.z * cos(orientation->roll) ==
+      0)
+    orientation->pitch =
+        accel_event.acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
+  else
+    orientation->pitch =
+        (float)atan(-accel_event.acceleration.x /
+                    (accel_event.acceleration.y * sin(orientation->roll) +
+                     accel_event.acceleration.z * cos(orientation->roll)));
+
+  // heading: Rotation around the Z-axis. -180 <= roll <= 180
+  // a positive heading angle is defined to be a clockwise rotation about the
+  // positive Z-axis
+  //
+  //                                       z * sin(roll) - y * cos(roll)
+  //   heading =
+  //   atan2(--------------------------------------------------------------------------)
+  //                    x * cos(pitch) + y * sin(pitch) * sin(roll) + z *
+  //                    sin(pitch) * cos(roll))
+  //
+  // where:  x, y, z are returned value from magnetometer sensor
+  orientation->heading =
+      (float)atan2(mag_event.magnetic.z * sin(orientation->roll) -
+                       mag_event.magnetic.y * cos(orientation->roll),
+                   mag_event.magnetic.x * cos(orientation->pitch) +
+                       mag_event.magnetic.y * sin(orientation->pitch) *
+                           sin(orientation->roll) +
+                       mag_event.magnetic.z * sin(orientation->pitch) *
+                           cos(orientation->roll));
+
+  // Convert angular data to degree
+  orientation->roll = orientation->roll * 180 / PI_F;
+  orientation->pitch = orientation->pitch * 180 / PI_F;
+  orientation->heading = orientation->heading * 180 / PI_F;
+
+  return true;
+}
--- a/src/Adafruit_Simple_AHRS.h
+++ b/src/Adafruit_Simple_AHRS.h
@ -0,0 +1,50 @@
+/*!
+ * @file Adafruit_Simple_AHRS.cpp
+ */
+#ifndef __ADAFRUIT_SIMPLE_AHRS_H__
+#define __ADAFRUIT_SIMPLE_AHRS_H__
+
+#include "Adafruit_Sensor_Set.h"
+#include <Adafruit_Sensor.h>
+
+/*!
+ * @brief Simple sensor fusion AHRS using an accelerometer and magnetometer.
+ */
+class Adafruit_Simple_AHRS {
+public:
+  /**************************************************************************/
+  /*!
+   * @brief Create a simple AHRS from a device with multiple sensors.
+   *
+   * @param accelerometer The accelerometer to use for this sensor fusion.
+   * @param magnetometer The magnetometer to use for this sensor fusion.
+   */
+  /**************************************************************************/
+  Adafruit_Simple_AHRS(Adafruit_Sensor *accelerometer,
+                       Adafruit_Sensor *magnetometer);
+
+  /**************************************************************************/
+  /*!
+   * @brief Create a simple AHRS from a device with multiple sensors.
+   *
+   * @param sensors A set of sensors containing the accelerometer and
+   * magnetometer for this sensor fusion.
+   */
+  /**************************************************************************/
+  Adafruit_Simple_AHRS(Adafruit_Sensor_Set &sensors);
+
+  /**************************************************************************/
+  /*!
+   * @brief Compute orientation based on accelerometer and magnetometer data.
+   *
+   * @return Whether the orientation was computed.
+   */
+  /**************************************************************************/
+  bool getOrientation(sensors_vec_t *orientation);
+
+private:
+  Adafruit_Sensor *_accel;
+  Adafruit_Sensor *_mag;
+};
+
+#endif
Author	SHA1	Message	Date
Tyeth Gundry	69fbb4021a	Bump version to 2.4.0 in library.properties	2025-08-26 15:16:15 +01:00
Dan Halbert	7d37a88d79	Merge pull request #43 from dhalbert/clang-fixes clang format	2025-08-24 18:45:47 -04:00
Dan Halbert	6d80c3abc3	clang format	2025-08-24 18:10:35 -04:00
Dan Halbert	b14657f111	Merge pull request #42 from Every-Flavor-Robotics/master Fix Gravity Vector Calculation (#35)	2025-08-24 17:40:46 -04:00
Swapnil Pande	1f304b21f1	Fix gravity vector calculation for Mahony filter	2025-08-19 15:13:26 -04:00
Swapnil Pande	b7e1039f9c	Merge branch 'adafruit:master' into master	2025-03-26 11:09:18 -04:00
Tyeth Gundry	8cd362a6ae	Update library.properties - bump version to 2.3.6	2024-03-26 13:07:45 +00:00
Carter Nelson	871109abcd	Merge pull request #39 from caternuson/iss38 Update for LSM6DS3TRC	2024-03-20 13:35:39 -07:00
caternuson	bfe18e94d9	clang	2024-03-20 11:44:51 -07:00
caternuson	bf743c13b1	update for LSM6DS3TRC	2024-03-20 11:27:41 -07:00
Swapnil Pande	3e8f6ca264	Fixed incorrect gravity vector computation	2024-02-25 13:55:05 -05:00
Tyeth Gundry	3b5285f636	Merge pull request #36 from tyeth/bump-version-2023-11-14-20-20-06 Update version number to 2.3.5	2023-11-29 00:51:16 +00:00
tyeth	8d4ebb8526	Warnings	2023-11-29 00:43:18 +00:00
Tyeth Gundry	09ff9f7b07	Bump version number to 2.3.5	2023-11-14 20:20:07 +00:00
dherrada	55a9541375	Update CI action versions	2023-05-12 11:23:56 -04:00
Paint Your Dragon	69b3a854be	Bump version for ESP32 fixed warnings	2022-10-24 17:48:01 -07:00
Paint Your Dragon	007b8b2678	Merge pull request #30 from adafruit/fix-warnings fix warning emitted by old Adafruit_LSM303_U (archived repo)	2022-10-24 17:36:42 -07:00
hathach	278089246a	fix lib depends	2022-10-21 17:47:15 +07:00
hathach	971568365d	fix warning emitted by old Adafruit_LSM303_U (archived repo)	2022-10-21 17:32:04 +07:00
Eva Herrada	7d3fbca5ca	Bump to 2.3.2	2022-01-18 17:44:04 -05:00
ToMe25	a55d7baefd	Fix getGravityVector not calculating the actual values (#27 ) * Fix getGravityVector not calculating the actual values * Initializes anglesComputed as false	2022-01-16 13:00:46 -05:00
ToMe25	ef7478b39c	Add gravity vector output to Madgwick and Mahony filter (#26 )	2022-01-15 21:15:50 -05:00
Dylan Herrada	c423dcc1b0	Bump to 2.3.1	2021-10-25 15:48:20 -04:00
Patrick Roncagliolo	8da69bad25	Variable delta time, orientation setters (ROS-like) (#16 ) * Add orientation setters This patch enables the user to init the filters with an arbitrary orientation. This is usually done to place the Z axis aligned with gravity vector. * Variable time deltas, gains as params * Fix constructors * Lint * Update Adafruit_AHRS_Mahony.h * Update Adafruit_AHRS_NXPFusion.h * Update Adafruit_AHRS_FusionInterface.h * Update Adafruit_AHRS_FusionInterface.h * Update Adafruit_AHRS_FusionInterface.h	2021-10-23 12:03:30 -04:00
Dylan Herrada	3a8d00bc7b	Bump to 2.3.0	2021-07-22 11:21:56 -04:00
Frédéric Bourgeois	f7534cf5f9	Getter & setter for algorithm parameters (#23 ) * Getter & setter for algorithm parameters Getter and setter for Madgwick (beta) and Mahony (Kp & Ki). * clang-format	2021-07-17 13:38:34 -04:00
Dylan Herrada	1425f14fbd	Bump to 2.2.4	2021-02-09 16:36:11 -05:00
ToMe25	a6eba1cb7b	Add some doxygen documentation (#19 ) * Add some initial NXP docu gitignore the html folder I feel like its better to commit the Doxyfile, as i had to add the src folder as an input for it to generate anything. * Add Simple AHRS documentation Slightly improve existing NXP Fusion docu * Formatting * Minor Fixes * Fix mainpage * Document some classes correctly * Delete Doxyfile	2021-02-09 11:41:23 -05:00
ToMe25	1031b89cf1	Add getters for NXP linear acceleration, gravity vector, and geomagnetic vector (#18 ) * Add getters for NXP linear acceleration, gravity vector, and geomagnetic vector. Also prevent Adafruit_AHRS_NXPFusion.h from being defined twice. * Fix Formatting I don't think i could have known the new line for "in g.", but the other ones were just me forgetting to format it correctly :)	2021-02-07 13:12:47 -05:00
Dylan Herrada	0083676f40	Bump to 2.0.3	2021-01-05 12:42:57 -05:00
Manoj Nathwani	ca140c7521	better calibration example comments (#15 ) * better calibration example comments see https://github.com/adafruit/Adafruit_AHRS/issues/14 * Update calibration.ino	2020-12-30 18:09:00 -05:00
siddacious	5be5374f73	Update library.properties	2020-04-24 18:17:57 -07:00
siddacious	d41404df31	Update library.properties	2020-04-24 18:16:53 -07:00
siddacious	007b6f9173	Merge pull request #11 from moritzmaier/master Update Adafruit_AHRS_NXPFusion.cpp resolves #10	2020-04-16 14:52:55 -07:00
Moritz Maier	53cb09aa01	Update Adafruit_AHRS_NXPFusion.cpp	2020-04-14 21:53:54 +02:00
Moritz Maier	28fa8e2e26	Update Adafruit_AHRS_NXPFusion.cpp This ensures that the first orientation lock is performed, i.e. absolute heading is computed, where yaw = 0 means you head towards magnetic north	2020-04-14 17:00:38 +02:00
siddacious	aa0dab372d	Update library.properties	2020-02-28 17:47:56 -08:00
Ha Thach	7a6947e935	added abstract class Adafruit_AHRS_FusionInterface (#9 ) * added abstract class Adafruit_AHRS_FusionInterface * clang format * only add virtual to superclass function	2020-02-26 00:19:02 -05:00
siddacious	f90ae54ef2	Update library.properties	2020-02-17 18:43:17 -08:00
siddacious	42a3f53c79	Update library.properties	2020-02-17 18:42:19 -08:00
Lady Ada	e117cb6d9d	add quat output	2020-02-17 11:47:58 -05:00
Lady Ada	4524baf63d	add quat to nxp, fix calib type, default to LSM6DS33	2020-02-17 11:47:09 -05:00
Lady Ada	e5f94f55b6	simplify output for webserial	2020-02-17 11:27:41 -05:00
Lady Ada	261423f611	make LSM6DS the defaults	2020-02-16 17:42:33 -05:00
ladyada	bc1488f9ec	add Adafruit Sensor Calibration dep	2020-02-07 18:03:21 -05:00
Lady Ada	2998f95f73	re-remove	2020-02-02 15:33:14 -05:00
Lady Ada	29c5afebd7	add readme	2020-02-02 13:31:34 -05:00
Limor "Ladyada" Fried	e07ec313ef	full refactor (#7 ) * moved files and added NXP fusion * rename and make unique names * renamed files and made a multi-filter tester * USB and Mahony examples are now superceded by one calibrated_orientation example * add lsm9ds0 fusion demo to calibrated_orientation and removed standalone version also moving really old 10dof/9dof demos * make one unified calibrator * add CI * add some deps * add more deps * more deps * update naming * more deps * skip and dep * manually add conversion const * more fixfix * change default to mahony * fix skips * clangclang * moreclang	2020-02-02 13:26:56 -05:00