only add virtual to superclass function

.github/workflows/githubci.yml

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

.gitignore

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

examples/calibrated_orientation/LSM6DS_LIS3MDL.h

@@ -2,12 +2,8 @@
 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"  // see the the LSM6DS_LIS3MDL file in this project to change board to LSM6DS33, LSM6DS3U, LSM6DSOX, etc
+#include "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
 //#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
 //#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout
 

examples/calibration/LSM6DS_LIS3MDL.h

@@ -2,12 +2,8 @@
 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,4 +1,4 @@
-// #include <Adafruit_LSM9DS1.h>
+//#include <Adafruit_LSM9DS1.h>
 // Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
 
 // Or if you have the older LSM9DS0

examples/calibration/calibration.ino

@@ -15,7 +15,7 @@
 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 "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
 //#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,12 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_Accel.h>
+#include <Adafruit_LSM303_U.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_LSM303DLH_Mag_Unified mag(30302);
+Adafruit_LSM303_Mag_Unified   mag(30302);
 Adafruit_BMP085_Unified       bmp(18001);
 
 // Create simple AHRS algorithm using the above sensors.

examples/deprecated/ahrs_9dof/ahrs_9dof.ino

@@ -1,12 +1,11 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_Accel.h>
+#include <Adafruit_LSM303_U.h>
-#include <Adafruit_LSM303DLH_Mag.h>
 #include <Adafruit_Simple_AHRS.h>
 
 // Create sensor instances.
 Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303DLH_Mag_Unified mag(30302);
+Adafruit_LSM303_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.4.0
+version=2.1.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 BMP085 Unified, Adafruit BluefruitLE nRF51, SdFat - Adafruit Fork, ArduinoJson, Adafruit SPIFlash, Adafruit Sensor Calibration, Adafruit LSM303 Accel, Adafruit LSM303DLH Mag
+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

src/Adafruit_AHRS.h

@@ -1,21 +1,3 @@
-/*!
- * @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,8 +1,4 @@
-/*!
+/*
- * @file Adafruit_AHRS_FusionInterface.h
- *
- * @section license License
- *
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Ha Thach (tinyusb.org) for Adafruit Industries
@@ -29,77 +25,17 @@
 #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,21 +34,18 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Adafruit_Madgwick::Adafruit_Madgwick() : Adafruit_Madgwick(betaDef) {}
+Adafruit_Madgwick::Adafruit_Madgwick() {
-
+  beta = 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;
+  anglesComputed = 0;
 }
 
 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;
@@ -60,7 +57,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, dt);
+    updateIMU(gx, gy, gz, ax, ay, az);
     return;
   }
 
@@ -170,10 +167,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 * dt;
+  q0 += qDot1 * invSampleFreq;
-  q1 += qDot2 * dt;
+  q1 += qDot2 * invSampleFreq;
-  q2 += qDot3 * dt;
+  q2 += qDot3 * invSampleFreq;
-  q3 += qDot4 * dt;
+  q3 += qDot4 * invSampleFreq;
 
   // Normalise quaternion
   recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -188,7 +185,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 dt) {
+                                  float ay, float az) {
   float recipNorm;
   float s0, s1, s2, s3;
   float qDot1, qDot2, qDot3, qDot4;
@@ -253,10 +250,10 @@ void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
   }
 
   // Integrate rate of change of quaternion to yield quaternion
-  q0 += qDot1 * dt;
+  q0 += qDot1 * invSampleFreq;
-  q1 += qDot2 * dt;
+  q1 += qDot2 * invSampleFreq;
-  q2 += qDot3 * dt;
+  q2 += qDot3 * invSampleFreq;
-  q3 += qDot4 * dt;
+  q3 += qDot4 * invSampleFreq;
 
   // Normalise quaternion
   recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
@@ -267,29 +264,19 @@ 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;
-  union {
+  float y = x;
-    float f;
+  long i = *(long *)&y;
-    long i;
+  i = 0x5f3759df - (i >> 1);
-  } conv = {x};
+  y = *(float *)&i;
-  conv.i = 0x5f3759df - (conv.i >> 1);
+  y = y * (1.5f - (halfx * y * y));
-  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  y = y * (1.5f - (halfx * y * y));
-  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  return y;
-  return conv.f;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -298,8 +285,5 @@ 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,30 +31,24 @@ private:
   float q2;
   float q3; // quaternion of sensor frame relative to auxiliary frame
   float invSampleFreq;
-  float roll, pitch, yaw;
+  float roll;
-  float grav[3];
+  float pitch;
-  bool anglesComputed = false;
+  float yaw;
+  char anglesComputed;
   void computeAngles();
 
   //-------------------------------------------------------------------------------------------
   // Function declarations
 public:
-  Adafruit_Madgwick();
+  Adafruit_Madgwick(void);
-  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();
@@ -91,18 +85,5 @@ 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,11 +37,9 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Adafruit_Mahony::Adafruit_Mahony() : Adafruit_Mahony(twoKpDef, twoKiDef) {}
+Adafruit_Mahony::Adafruit_Mahony() {
-
+  twoKp = twoKpDef; // 2 * proportional gain (Kp)
-Adafruit_Mahony::Adafruit_Mahony(float prop_gain, float int_gain) {
+  twoKi = twoKiDef; // 2 * integral gain (Ki)
-  twoKp = prop_gain; // 2 * proportional gain (Kp)
-  twoKi = int_gain;  // 2 * integral gain (Ki)
   q0 = 1.0f;
   q1 = 0.0f;
   q2 = 0.0f;
@@ -49,12 +47,12 @@ Adafruit_Mahony::Adafruit_Mahony(float prop_gain, float int_gain) {
   integralFBx = 0.0f;
   integralFBy = 0.0f;
   integralFBz = 0.0f;
-  anglesComputed = false;
+  anglesComputed = 0;
   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 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;
@@ -128,9 +126,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 * dt;
+      integralFBx += twoKi * halfex * invSampleFreq;
-      integralFBy += twoKi * halfey * dt;
+      integralFBy += twoKi * halfey * invSampleFreq;
-      integralFBz += twoKi * halfez * dt;
+      integralFBz += twoKi * halfez * invSampleFreq;
       gx += integralFBx; // apply integral feedback
       gy += integralFBy;
       gz += integralFBz;
@@ -147,9 +145,9 @@ void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
   }
 
   // Integrate rate of change of quaternion
-  gx *= (0.5f * dt); // pre-multiply common factors
+  gx *= (0.5f * invSampleFreq); // pre-multiply common factors
-  gy *= (0.5f * dt);
+  gy *= (0.5f * invSampleFreq);
-  gz *= (0.5f * dt);
+  gz *= (0.5f * invSampleFreq);
   qa = q0;
   qb = q1;
   qc = q2;
@@ -171,7 +169,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 dt) {
+                                float ay, float az) {
   float recipNorm;
   float halfvx, halfvy, halfvz;
   float halfex, halfey, halfez;
@@ -206,9 +204,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 * dt;
+      integralFBx += twoKi * halfex * invSampleFreq;
-      integralFBy += twoKi * halfey * dt;
+      integralFBy += twoKi * halfey * invSampleFreq;
-      integralFBz += twoKi * halfez * dt;
+      integralFBz += twoKi * halfez * invSampleFreq;
       gx += integralFBx; // apply integral feedback
       gy += integralFBy;
       gz += integralFBz;
@@ -225,9 +223,9 @@ void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
   }
 
   // Integrate rate of change of quaternion
-  gx *= (0.5f * dt); // pre-multiply common factors
+  gx *= (0.5f * invSampleFreq); // pre-multiply common factors
-  gy *= (0.5f * dt);
+  gy *= (0.5f * invSampleFreq);
-  gz *= (0.5f * dt);
+  gz *= (0.5f * invSampleFreq);
   qa = q0;
   qb = q1;
   qc = q2;
@@ -245,30 +243,19 @@ 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;
-  union {
+  float y = x;
-    float f;
+  long i = *(long *)&y;
-    long i;
+  i = 0x5f3759df - (i >> 1);
-  } conv = {x};
+  y = *(float *)&i;
-  conv.i = 0x5f3759df - (conv.i >> 1);
+  y = y * (1.5f - (halfx * y * y));
-  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  y = y * (1.5f - (halfx * y * y));
-  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  return y;
-  return conv.f;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -277,9 +264,6 @@ 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,8 +29,7 @@ private:
       integralFBz; // integral error terms scaled by Ki
   float invSampleFreq;
   float roll, pitch, yaw;
-  float grav[3];
+  char anglesComputed;
-  bool anglesComputed = false;
   static float invSqrt(float x);
   void computeAngles();
 
@@ -39,19 +38,10 @@ 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();
@@ -88,19 +78,6 @@ 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,39 +1,34 @@
-/*!
+// Copyright (c) 2014, Freescale Semiconductor, Inc.
- * @file Adafruit_AHRS_NXPFusion.cpp
+// All rights reserved.
- *
+// vim: set ts=4:
- * @section license License
+//
- *
+// Redistribution and use in source and binary forms, with or without
- * Copyright (c) 2014, Freescale Semiconductor, Inc.
+// modification, are permitted provided that the following conditions are met:
- * All rights reserved.
+//     * Redistributions of source code must retain the above copyright
- * vim: set ts=4:
+//       notice, this list of conditions and the following disclaimer.
- *
+//     * Redistributions in binary form must reproduce the above copyright
- * Redistribution and use in source and binary forms, with or without
+//       notice, this list of conditions and the following disclaimer in the
- * modification, are permitted provided that the following conditions are met:
+//       documentation and/or other materials provided with the distribution.
- *     * Redistributions of source code must retain the above copyright
+//     * Neither the name of Freescale Semiconductor, Inc. nor the
- *       notice, this list of conditions and the following disclaimer.
+//       names of its contributors may be used to endorse or promote products
- *     * Redistributions in binary form must reproduce the above copyright
+//       derived from this software without specific prior written permission.
- *       notice, this list of conditions and the following disclaimer in the
+//
- *       documentation and/or other materials provided with the distribution.
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- *     * Neither the name of Freescale Semiconductor, Inc. nor the
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- *       names of its contributors may be used to endorse or promote products
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- *       derived from this software without specific prior written permission.
+// ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
- *
+// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+//
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// This is the file that contains the fusion routines.  It is STRONGLY
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// RECOMMENDED that the casual developer NOT TOUCH THIS FILE.  The mathematics
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// behind this file is extremely complex, and it will be very easy (almost
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+// inevitable) that you screw it up.
- *
+//
- * 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"
 
@@ -116,12 +111,8 @@ 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
@@ -130,7 +121,7 @@ void Adafruit_NXPSensorFusion::begin(float sampleFrequency) {
 
   // compute and store useful product terms to save floating point calculations
   // later
-  Fastdeltat = 1.0f / sampleFrequency;
+  Fastdeltat = 1.0f / sampleRate;
   deltat = Fastdeltat;
   deltatsq = deltat * deltat;
   casq = FCA_9DOF_GBY_KALMAN * FCA_9DOF_GBY_KALMAN;
@@ -195,12 +186,11 @@ void Adafruit_NXPSensorFusion::begin(float sampleFrequency) {
   resetflag = 0;
 }
 
-/**************************************************************************/
+// 9DOF orientation function implemented using a 12 element Kalman filter
-/*!
+// void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
- * @brief Updates the filter with new gyroscope, accelerometer, and magnetometer
+// const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro,
- * data.
+// const MagCalibration_t *MagCal)
- */
+
-/**************************************************************************/
 void Adafruit_NXPSensorFusion::update(float gx, float gy, float gz, float ax,
                                       float ay, float az, float mx, float my,
                                       float mz) {
@@ -248,7 +238,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) <= 50.0f && fabsf(my) <= 50.0f && fabsf(mz) <= 50.0f) {
+  if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {
     ValidMagCal = 1;
   } else {
     ValidMagCal = 0;
@@ -312,7 +302,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 the a priori acceleration (a-) (g, sensor frame) from decayed a
+    // compute a priori acceleration (a-) (g, sensor frame) from decayed a
     // posteriori estimate (g, sensor frame)
     aSeMi[i] = FCA_9DOF_GBY_KALMAN * aSePl[i];
 

src/Adafruit_AHRS_NXPFusion.h

@@ -1,81 +1,40 @@
-/*!
- * @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
- * @brief Kalman/NXP Fusion algorithm.
+// 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
 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);
 
@@ -90,58 +49,6 @@ 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
@@ -213,5 +120,3 @@ private:
       FirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF
   int8_t resetflag;         // flag to request re-initialization on next pass
 };
-
-#endif

src/Adafruit_Simple_AHRS.cpp

@@ -1,32 +1,16 @@
-/*!
- * @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)

src/Adafruit_Simple_AHRS.h

@@ -1,45 +1,15 @@
-/*!
- * @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: