Bump version to 2.4.0 in library.properties

clang format

.github/workflows/githubci.yml

@@ -7,11 +7,11 @@ jobs:
     runs-on: ubuntu-latest
     
     steps:
-    - uses: actions/setup-python@v1
+    - uses: actions/setup-python@v4
       with:
         python-version: '3.x'
-    - uses: actions/checkout@v2
-    - uses: actions/checkout@v2
+    - uses: actions/checkout@v3
+    - uses: actions/checkout@v3
       with:
          repository: adafruit/ci-arduino
          path: ci

.gitignore

@@ -1 +1,3 @@
 serialconfig.txt
+.pio
+html

examples/calibrated_orientation/LSM6DS_LIS3MDL.h

@@ -2,8 +2,12 @@
 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()) {

examples/calibrated_orientation/LSM9DS.h

@@ -2,7 +2,7 @@
 Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
 
 // Or if you have the older LSM9DS0
-//#include <Adafruit_LSM9DS0.h>
+// #include <Adafruit_LSM9DS0.h>
 // Adafruit_LSM9DS0 lsm9ds = Adafruit_LSM9DS0();
 
 bool init_sensors(void) {

examples/calibrated_orientation/calibrated_orientation.ino

@@ -13,7 +13,7 @@
 Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
 
 // uncomment one combo 9-DoF!
-#include "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
+#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
 

examples/calibration/LSM6DS_LIS3MDL.h

@@ -2,8 +2,12 @@
 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()) {

examples/calibration/LSM9DS.h

@@ -1,5 +1,5 @@
-//#include <Adafruit_LSM9DS1.h>
-// Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
+// #include <Adafruit_LSM9DS1.h>
+//  Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
 
 // Or if you have the older LSM9DS0
 #include <Adafruit_LSM9DS0.h>

examples/calibration/calibration.ino

@@ -15,7 +15,7 @@
 Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
 
 // uncomment one combo 9-DoF!
-#include "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
+#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
 

examples/deprecated/ahrs_10dof/ahrs_10dof.ino

@@ -1,13 +1,14 @@
 #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_BMP085_Unified       bmp(18001);
+Adafruit_LSM303_Accel_Unified  accel(30301);
+Adafruit_LSM303DLH_Mag_Unified mag(30302);
+Adafruit_BMP085_Unified        bmp(18001);
 
 // Create simple AHRS algorithm using the above sensors.
 Adafruit_Simple_AHRS          ahrs(&accel, &mag);

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_LSM303_Accel_Unified  accel(30301);
+Adafruit_LSM303DLH_Mag_Unified mag(30302);
 
 // Create simple AHRS algorithm using the above sensors.
 Adafruit_Simple_AHRS          ahrs(&accel, &mag);

library.properties

@@ -1,5 +1,5 @@
 name=Adafruit AHRS
-version=2.1.0
+version=2.4.0
 author=Adafruit
 maintainer=Adafruit <info@adafruit.com>
 sentence=AHRS (Altitude and Heading Reference System) for various Adafruit motion sensors
@@ -7,4 +7,4 @@ paragraph=Includes motion calibration example sketches, as well as calibration o
 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 LSM303DLHC, Adafruit BMP085 Unified, Adafruit BluefruitLE nRF51, SdFat - Adafruit Fork, ArduinoJson, Adafruit SPIFlash, Adafruit Sensor Calibration
+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

src/Adafruit_AHRS.h

@@ -1,3 +1,21 @@
+/*!
+ * @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_
 

src/Adafruit_AHRS_FusionInterface.h

@@ -1,4 +1,8 @@
-/*
+/*!
+ * @file Adafruit_AHRS_FusionInterface.h
+ *
+ * @section license License
+ *
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Ha Thach (tinyusb.org) for Adafruit Industries
@@ -25,17 +29,77 @@
 #ifndef ADAFRUIT_AHRS_FUSIONINTERFACE_H_
 #define ADAFRUIT_AHRS_FUSIONINTERFACE_H_
 
-// Interface for Fusion Algorithm
+/*!
+ * @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_ */

src/Adafruit_AHRS_Madgwick.cpp

@@ -34,18 +34,21 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Adafruit_Madgwick::Adafruit_Madgwick() {
-  beta = betaDef;
+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 = 0;
+  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 az, float mx, float my, float mz,
+                               float dt) {
   float recipNorm;
   float s0, s1, s2, s3;
   float qDot1, qDot2, qDot3, qDot4;
@@ -57,7 +60,7 @@ void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
   // 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);
+    updateIMU(gx, gy, gz, ax, ay, az, dt);
     return;
   }
 
@@ -167,10 +170,10 @@ void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
   }
 
   // Integrate rate of change of quaternion to yield quaternion
-  q0 += qDot1 * invSampleFreq;
-  q1 += qDot2 * invSampleFreq;
-  q2 += qDot3 * invSampleFreq;
-  q3 += qDot4 * invSampleFreq;
+  q0 += qDot1 * dt;
+  q1 += qDot2 * dt;
+  q2 += qDot3 * dt;
+  q3 += qDot4 * dt;
 
   // Normalise quaternion
   recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -185,7 +188,7 @@ void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
 // IMU algorithm update
 
 void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
-                                  float ay, float az) {
+                                  float ay, float az, float dt) {
   float recipNorm;
   float s0, s1, s2, s3;
   float qDot1, qDot2, qDot3, qDot4;
@@ -250,10 +253,10 @@ void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
   }
 
   // Integrate rate of change of quaternion to yield quaternion
-  q0 += qDot1 * invSampleFreq;
-  q1 += qDot2 * invSampleFreq;
-  q2 += qDot3 * invSampleFreq;
-  q3 += qDot4 * invSampleFreq;
+  q0 += qDot1 * dt;
+  q1 += qDot2 * dt;
+  q2 += qDot3 * dt;
+  q3 += qDot4 * dt;
 
   // Normalise quaternion
   recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -264,19 +267,29 @@ void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
   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;
-  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;
+  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;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -285,5 +298,8 @@ 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;
 }

src/Adafruit_AHRS_Madgwick.h

@@ -31,24 +31,30 @@ private:
   float q2;
   float q3; // quaternion of sensor frame relative to auxiliary frame
   float invSampleFreq;
-  float roll;
-  float pitch;
-  float yaw;
-  char anglesComputed;
+  float roll, pitch, yaw;
+  float grav[3];
+  bool anglesComputed = false;
   void computeAngles();
 
   //-------------------------------------------------------------------------------------------
   // Function declarations
 public:
-  Adafruit_Madgwick(void);
+  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();
@@ -85,5 +91,18 @@ public:
     *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

src/Adafruit_AHRS_Mahony.cpp

@@ -37,9 +37,11 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Adafruit_Mahony::Adafruit_Mahony() {
-  twoKp = twoKpDef; // 2 * proportional gain (Kp)
-  twoKi = twoKiDef; // 2 * integral gain (Ki)
+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;
@@ -47,12 +49,12 @@ Adafruit_Mahony::Adafruit_Mahony() {
   integralFBx = 0.0f;
   integralFBy = 0.0f;
   integralFBz = 0.0f;
-  anglesComputed = 0;
+  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 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;
@@ -126,9 +128,9 @@ void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
     // 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;
+      integralFBx += twoKi * halfex * dt;
+      integralFBy += twoKi * halfey * dt;
+      integralFBz += twoKi * halfez * dt;
       gx += integralFBx; // apply integral feedback
       gy += integralFBy;
       gz += integralFBz;
@@ -145,9 +147,9 @@ void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
   }
 
   // Integrate rate of change of quaternion
-  gx *= (0.5f * invSampleFreq); // pre-multiply common factors
-  gy *= (0.5f * invSampleFreq);
-  gz *= (0.5f * invSampleFreq);
+  gx *= (0.5f * dt); // pre-multiply common factors
+  gy *= (0.5f * dt);
+  gz *= (0.5f * dt);
   qa = q0;
   qb = q1;
   qc = q2;
@@ -169,7 +171,7 @@ void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
 // IMU algorithm update
 
 void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
-                                float ay, float az) {
+                                float ay, float az, float dt) {
   float recipNorm;
   float halfvx, halfvy, halfvz;
   float halfex, halfey, halfez;
@@ -204,9 +206,9 @@ void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
     // 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;
+      integralFBx += twoKi * halfex * dt;
+      integralFBy += twoKi * halfey * dt;
+      integralFBz += twoKi * halfez * dt;
       gx += integralFBx; // apply integral feedback
       gy += integralFBy;
       gz += integralFBz;
@@ -223,9 +225,9 @@ void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
   }
 
   // Integrate rate of change of quaternion
-  gx *= (0.5f * invSampleFreq); // pre-multiply common factors
-  gy *= (0.5f * invSampleFreq);
-  gz *= (0.5f * invSampleFreq);
+  gx *= (0.5f * dt); // pre-multiply common factors
+  gy *= (0.5f * dt);
+  gz *= (0.5f * dt);
   qa = q0;
   qb = q1;
   qc = q2;
@@ -243,19 +245,30 @@ void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
   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;
-  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;
+  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;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -264,6 +277,9 @@ 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;
 }
 

src/Adafruit_AHRS_Mahony.h

@@ -29,7 +29,8 @@ private:
       integralFBz; // integral error terms scaled by Ki
   float invSampleFreq;
   float roll, pitch, yaw;
-  char anglesComputed;
+  float grav[3];
+  bool anglesComputed = false;
   static float invSqrt(float x);
   void computeAngles();
 
@@ -38,10 +39,19 @@ private:
 
 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();
@@ -78,6 +88,19 @@ public:
     *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

src/Adafruit_AHRS_NXPFusion.cpp

@@ -1,34 +1,39 @@
-// 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 is the file that contains the fusion routines.  It is STRONGLY
-// RECOMMENDED that the casual developer NOT TOUCH THIS FILE.  The mathematics
-// behind this file is extremely complex, and it will be very easy (almost
-// inevitable) that you screw it up.
-//
+/*!
+ * @file Adafruit_AHRS_NXPFusion.cpp
+ *
+ * @section license License
+ *
+ * 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 is the file that contains the fusion routines.  It is STRONGLY
+ * RECOMMENDED that the casual developer NOT TOUCH THIS FILE.  The mathematics
+ * behind this file is extremely complex, and it will be very easy (almost
+ * inevitable) that you screw it up.
+ */
 
 #include "Adafruit_AHRS_NXPFusion.h"
 
@@ -111,8 +116,12 @@ void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[],
 void fmatrixAeqRenormRotA(float A[][3]);
 }
 
-// initialize the 9DOF Kalman filter
-void Adafruit_NXPSensorFusion::begin(float sampleRate) {
+/**************************************************************************/
+/*!
+ * @brief Initializes the 9DOF Kalman filter.
+ */
+/**************************************************************************/
+void Adafruit_NXPSensorFusion::begin(float sampleFrequency) {
   int8_t i, j;
 
   // reset the flag denoting that a first 9DOF orientation lock has been
@@ -121,7 +130,7 @@ void Adafruit_NXPSensorFusion::begin(float sampleRate) {
 
   // compute and store useful product terms to save floating point calculations
   // later
-  Fastdeltat = 1.0f / sampleRate;
+  Fastdeltat = 1.0f / sampleFrequency;
   deltat = Fastdeltat;
   deltatsq = deltat * deltat;
   casq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
@@ -186,11 +195,12 @@ void Adafruit_NXPSensorFusion::begin(float sampleRate) {
   resetflag = 0;
 }
 
-// 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)
-
+/**************************************************************************/
+/*!
+ * @brief Updates the filter with new gyroscope, accelerometer, and magnetometer
+ * data.
+ */
+/**************************************************************************/
 void Adafruit_NXPSensorFusion::update(float gx, float gy, float gz, float ax,
                                       float ay, float az, float mx, float my,
                                       float mz) {
@@ -238,7 +248,7 @@ void Adafruit_NXPSensorFusion::update(float gx, float gy, float gz, float ax,
   // initial orientation lock to accelerometer and magnetometer eCompass
   // orientation
   // *********************************************************************************
-  if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {
+  if (fabsf(mx) <= 50.0f && fabsf(my) <= 50.0f && fabsf(mz) <= 50.0f) {
     ValidMagCal = 1;
   } else {
     ValidMagCal = 0;
@@ -302,7 +312,7 @@ void Adafruit_NXPSensorFusion::update(float gx, float gy, float gz, float ax,
     // (with magnitude 1g) NED gravity is along positive z axis
     gSeGyMi[i] = RMi[i][Z];
 
-    // compute a priori acceleration (a-) (g, sensor frame) from decayed a
+    // compute the a priori acceleration (a-) (g, sensor frame) from decayed a
     // posteriori estimate (g, sensor frame)
     aSeMi[i] = FCA_9DOF_GBY_KALMAN * aSePl[i];
 
@@ -623,7 +633,7 @@ void Adafruit_NXPSensorFusion::update(float gx, float gy, float gz, float ax,
       mGl[X] = DEFAULTB * fcosdelta; // TODO: MagCal->B
       mGl[Z] = DEFAULTB * fsindelta;
     } // end hyp == 0.0F
-  }   // end ValidMagCal
+  } // end ValidMagCal
 
   // *********************************************************************************
   // compute the a posteriori Euler angles from the orientation matrix

src/Adafruit_AHRS_NXPFusion.h

@@ -1,40 +1,81 @@
+/*!
+ * @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>
 
-// 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.
-//
-
-// changed class name to avoid collision
+/*!
+ * @brief Kalman/NXP Fusion algorithm.
+ */
 class Adafruit_NXPSensorFusion : public Adafruit_AHRS_FusionInterface {
 public:
-  void begin(float sampleRate = 100.0f);
+  /**************************************************************************/
+  /*!
+   * @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);
 
@@ -49,6 +90,58 @@ public:
     *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
@@ -120,3 +213,5 @@ private:
       FirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF
   int8_t resetflag;         // flag to request re-initialization on next pass
 };
+
+#endif

src/Adafruit_AHRS_NXPmatrix.c

@@ -296,9 +296,9 @@ void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n) {
               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
+        } // 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
 }
 

src/Adafruit_Simple_AHRS.cpp

@@ -1,16 +1,32 @@
+/*!
+ * @file Adafruit_Simple_AHRS.cpp
+ */
+
 #include "Adafruit_Simple_AHRS.h"
 
-// Create a simple AHRS from an explicit accelerometer and magnetometer sensor.
+/**************************************************************************/
+/*!
+ * @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) {}
 
-// Create a simple AHRS from a device with multiple sensors.
+/**************************************************************************/
+/*!
+ * @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)) {}
 
-// Compute orientation based on accelerometer and magnetometer data.
+/**************************************************************************/
+/*!
+ * @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)
@@ -82,4 +98,4 @@ bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t *orientation) {
   orientation->heading = orientation->heading * 180 / PI_F;
 
   return true;
-}
+}

src/Adafruit_Simple_AHRS.h

@@ -1,15 +1,45 @@
+/*!
+ * @file Adafruit_Simple_AHRS.cpp
+ */
 #ifndef __ADAFRUIT_SIMPLE_AHRS_H__
 #define __ADAFRUIT_SIMPLE_AHRS_H__
 
 #include "Adafruit_Sensor_Set.h"
 #include <Adafruit_Sensor.h>
 
-// Simple sensor fusion AHRS using an accelerometer and magnetometer.
+/*!
+ * @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: