moreclang

also moving really old 10dof/9dof demos

.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@v3
+    - uses: actions/checkout@v2
+    - uses: actions/checkout@v2
       with:
          repository: adafruit/ci-arduino
          path: ci
@@ -19,6 +19,11 @@ jobs:
     - name: pre-install
       run: bash ci/actions_install.sh
 
+    # manually install calib
+    - name: extra libraries
+      run: |
+        git clone --quiet https://github.com/adafruit/Adafruit_Sensor_Calibration.git /home/runner/Arduino/libraries/Adafruit_Sensor_Calibration
+
     - name: test platforms
       run: python3 ci/build_platform.py main_platforms
 

.gitignore

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

README.md

@@ -1,8 +0,0 @@
-# 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!

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

@@ -10,28 +10,23 @@
 #include <Adafruit_Sensor_Calibration.h>
 #include <Adafruit_AHRS.h>
 
+#define SENSORS_RADS_TO_DPS (180.0 / 3.141592653589793238463)
+
 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 "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
+#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
 //#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout
 
+Adafruit_Sensor_Calibration cal;
+
 // 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() {
@@ -64,7 +59,6 @@ void setup() {
 void loop() {
   float roll, pitch, heading;
   float gx, gy, gz;
-  static uint8_t counter = 0;
 
   if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
     return;
@@ -75,9 +69,7 @@ void loop() {
   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
+  //Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
 
   cal.calibrate(mag);
   cal.calibrate(accel);
@@ -92,18 +84,8 @@ void loop() {
   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
+  //Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
 
-  // 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(", ");
@@ -114,7 +96,6 @@ void loop() {
   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();
@@ -122,23 +103,9 @@ void loop() {
   heading = filter.getYaw();
   Serial.print("Orientation: ");
   Serial.print(heading);
-  Serial.print(", ");
+  Serial.print(" ");
   Serial.print(pitch);
-  Serial.print(", ");
+  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
 }

examples/calibration/LSM6DS_LIS3MDL.h

@@ -1,13 +1,9 @@
 #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;
+// Can change this to be LSM6DSOX or whatever ya like
+#include <Adafruit_LSM6DSOX.h>
+Adafruit_LSM6DSOX 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

@@ -12,20 +12,16 @@
 
 #include <Adafruit_Sensor_Calibration.h>
 
+#define SENSORS_RADS_TO_DPS (180.0 / 3.141592653589793238463)
+
 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
 
-// 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
-
+Adafruit_Sensor_Calibration cal;
 sensors_event_t mag_event, gyro_event, accel_event;
 
 int loopcount = 0;

examples/deprecated/ahrs_10dof/ahrs_10dof.ino

@@ -1,14 +1,13 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_Accel.h>
-#include <Adafruit_LSM303DLH_Mag.h>
+#include <Adafruit_LSM303_U.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_BMP085_Unified        bmp(18001);
+Adafruit_LSM303_Accel_Unified accel(30301);
+Adafruit_LSM303_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,12 +1,11 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_Accel.h>
-#include <Adafruit_LSM303DLH_Mag.h>
+#include <Adafruit_LSM303_U.h>
 #include <Adafruit_Simple_AHRS.h>
 
 // Create sensor instances.
-Adafruit_LSM303_Accel_Unified  accel(30301);
-Adafruit_LSM303DLH_Mag_Unified mag(30302);
+Adafruit_LSM303_Accel_Unified accel(30301);
+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.0.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

src/Adafruit_AHRS.h

@@ -1,26 +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_
-
 #include <Adafruit_AHRS_Madgwick.h>
 #include <Adafruit_AHRS_Mahony.h>
 #include <Adafruit_AHRS_NXPFusion.h>
-
-#endif // __ADAFRUIT_AHRS_H_

src/Adafruit_AHRS_FusionInterface.h

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

src/Adafruit_AHRS_Madgwick.cpp

@@ -34,21 +34,18 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Adafruit_Madgwick::Adafruit_Madgwick() : Adafruit_Madgwick(betaDef) {}
-
-Adafruit_Madgwick::Adafruit_Madgwick(float gain) {
-  beta = gain;
+Adafruit_Madgwick::Adafruit_Madgwick() {
+  beta = betaDef;
   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 dt) {
+                               float az, float mx, float my, float mz) {
   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;
-  q1 += qDot2 * dt;
-  q2 += qDot3 * dt;
-  q3 += qDot4 * dt;
+  q0 += qDot1 * invSampleFreq;
+  q1 += qDot2 * invSampleFreq;
+  q2 += qDot3 * invSampleFreq;
+  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;
-  q1 += qDot2 * dt;
-  q2 += qDot3 * dt;
-  q3 += qDot4 * dt;
+  q0 += qDot1 * invSampleFreq;
+  q1 += qDot2 * invSampleFreq;
+  q2 += qDot3 * invSampleFreq;
+  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 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;
+  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;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -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

@@ -16,13 +16,11 @@
 //=============================================================================================
 #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 {
+class Adafruit_Madgwick {
 private:
   static float invSqrt(float x);
   float beta; // algorithm gain
@@ -31,30 +29,24 @@ private:
   float q2;
   float q3; // quaternion of sensor frame relative to auxiliary frame
   float invSampleFreq;
-  float roll, pitch, yaw;
-  float grav[3];
-  bool anglesComputed = false;
+  float roll;
+  float pitch;
+  float yaw;
+  char anglesComputed;
   void computeAngles();
 
   //-------------------------------------------------------------------------------------------
   // Function declarations
 public:
-  Adafruit_Madgwick();
-  Adafruit_Madgwick(float gain);
+  Adafruit_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);
-  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 +83,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(float prop_gain, float int_gain) {
-  twoKp = prop_gain; // 2 * proportional gain (Kp)
-  twoKi = int_gain;  // 2 * integral gain (Ki)
+Adafruit_Mahony::Adafruit_Mahony() {
+  twoKp = twoKpDef; // 2 * proportional gain (Kp)
+  twoKi = twoKiDef; // 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;
-      integralFBy += twoKi * halfey * dt;
-      integralFBz += twoKi * halfez * dt;
+      integralFBx += twoKi * halfex * invSampleFreq;
+      integralFBy += twoKi * halfey * invSampleFreq;
+      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
-  gy *= (0.5f * dt);
-  gz *= (0.5f * dt);
+  gx *= (0.5f * invSampleFreq); // pre-multiply common factors
+  gy *= (0.5f * invSampleFreq);
+  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;
-      integralFBy += twoKi * halfey * dt;
-      integralFBz += twoKi * halfez * dt;
+      integralFBx += twoKi * halfex * invSampleFreq;
+      integralFBy += twoKi * halfey * invSampleFreq;
+      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
-  gy *= (0.5f * dt);
-  gz *= (0.5f * dt);
+  gx *= (0.5f * invSampleFreq); // pre-multiply common factors
+  gy *= (0.5f * invSampleFreq);
+  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 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;
+  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;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -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

@@ -12,14 +12,12 @@
 //=============================================================================================
 #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 {
+class Adafruit_Mahony {
 private:
   float twoKp; // 2 * proportional gain (Kp)
   float twoKi; // 2 * integral gain (Ki)
@@ -29,8 +27,7 @@ private:
       integralFBz; // integral error terms scaled by Ki
   float invSampleFreq;
   float roll, pitch, yaw;
-  float grav[3];
-  bool anglesComputed = false;
+  char anglesComputed;
   static float invSqrt(float x);
   void computeAngles();
 
@@ -39,19 +36,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 +76,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 @@
-/*!
- * @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.
- */
+// 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"
 
@@ -116,12 +111,8 @@ void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[],
 void fmatrixAeqRenormRotA(float A[][3]);
 }
 
-/**************************************************************************/
-/*!
- * @brief Initializes the 9DOF Kalman filter.
- */
-/**************************************************************************/
-void Adafruit_NXPSensorFusion::begin(float sampleFrequency) {
+// initialize the 9DOF Kalman filter
+void Adafruit_NXPSensorFusion::begin(float sampleRate) {
   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;
 }
 
-/**************************************************************************/
-/*!
- * @brief Updates the filter with new gyroscope, accelerometer, and magnetometer
- * data.
- */
-/**************************************************************************/
+// 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)
+
 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];
 
@@ -633,7 +623,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,81 +1,39 @@
-/*!
- * @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);
+// 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
 
-  /**************************************************************************/
-  /*!
-   * @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.
-   */
-  /**************************************************************************/
+// 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:
+  void begin(float sampleRate = 100.0f);
   void update(float gx, float gy, float gz, float ax, float ay, float az,
               float mx, float my, float mz);
 
@@ -83,65 +41,6 @@ public:
   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
@@ -213,5 +112,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_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,32 +1,16 @@
-/*!
- * @file Adafruit_Simple_AHRS.cpp
- */
-
 #include "Adafruit_Simple_AHRS.h"
 
-/**************************************************************************/
-/*!
- * @brief Create a simple AHRS from a device with multiple sensors.
- */
-/**************************************************************************/
+// 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) {}
 
-/**************************************************************************/
-/*!
- * @brief Create a simple AHRS from a device with multiple sensors.
- */
-/**************************************************************************/
+// 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.
- */
-/**************************************************************************/
+// 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>
 
-/*!
- * @brief Simple sensor fusion AHRS using an accelerometer and magnetometer.
- */
+// 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: