Missed saving file in processing.

.github/ISSUE_TEMPLATE.md

@@ -1,46 +0,0 @@
-Thank you for opening an issue on an Adafruit Arduino library repository.  To
-improve the speed of resolution please review the following guidelines and
-common troubleshooting steps below before creating the issue:
-
-- **Do not use GitHub issues for troubleshooting projects and issues.**  Instead use
-  the forums at http://forums.adafruit.com to ask questions and troubleshoot why
-  something isn't working as expected.  In many cases the problem is a common issue
-  that you will more quickly receive help from the forum community.  GitHub issues
-  are meant for known defects in the code.  If you don't know if there is a defect
-  in the code then start with troubleshooting on the forum first.
-
-- **If following a tutorial or guide be sure you didn't miss a step.** Carefully
-  check all of the steps and commands to run have been followed.  Consult the
-  forum if you're unsure or have questions about steps in a guide/tutorial.
-
-- **For Arduino projects check these very common issues to ensure they don't apply**:
-
-  - For uploading sketches or communicating with the board make sure you're using
-    a **USB data cable** and **not** a **USB charge-only cable**.  It is sometimes
-    very hard to tell the difference between a data and charge cable!  Try using the
-    cable with other devices or swapping to another cable to confirm it is not
-    the problem.
-
-  - **Be sure you are supplying adequate power to the board.**  Check the specs of
-    your board and plug in an external power supply.  In many cases just
-    plugging a board into your computer is not enough to power it and other
-    peripherals.
-
-  - **Double check all soldering joints and connections.**  Flakey connections
-    cause many mysterious problems.  See the [guide to excellent soldering](https://learn.adafruit.com/adafruit-guide-excellent-soldering/tools) for examples of good solder joints.
-
-  - **Ensure you are using an official Arduino or Adafruit board.** We can't
-    guarantee a clone board will have the same functionality and work as expected
-    with this code and don't support them.
-
-If you're sure this issue is a defect in the code and checked the steps above
-please fill in the following fields to provide enough troubleshooting information.
-You may delete the guideline and text above to just leave the following details:
-
-- Arduino board:  **INSERT ARDUINO BOARD NAME/TYPE HERE**
-
-- Arduino IDE version (found in Arduino -> About Arduino menu):  **INSERT ARDUINO
-  VERSION HERE**
-
-- List the steps to reproduce the problem below (if possible attach a sketch or
-  copy the sketch code in too): **LIST REPRO STEPS BELOW**

.github/PULL_REQUEST_TEMPLATE.md

@@ -1,26 +0,0 @@
-Thank you for creating a pull request to contribute to Adafruit's GitHub code!
-Before you open the request please review the following guidelines and tips to
-help it be more easily integrated:
-
-- **Describe the scope of your change--i.e. what the change does and what parts
-  of the code were modified.**  This will help us understand any risks of integrating
-  the code.
-
-- **Describe any known limitations with your change.**  For example if the change
-  doesn't apply to a supported platform of the library please mention it.
-
-- **Please run any tests or examples that can exercise your modified code.**  We
-  strive to not break users of the code and running tests/examples helps with this
-  process.
-
-Thank you again for contributing!  We will try to test and integrate the change
-as soon as we can, but be aware we have many GitHub repositories to manage and
-can't immediately respond to every request.  There is no need to bump or check in
-on a pull request (it will clutter the discussion of the request).
-
-Also don't be worried if the request is closed or not integrated--sometimes the
-priorities of Adafruit's GitHub code (education, ease of use) might not match the
-priorities of the pull request.  Don't fret, the open source community thrives on
-forks and GitHub makes it easy to keep your changes in a forked repo.
-
-After reviewing the guidelines above you can delete this text from the pull request.

.github/workflows/githubci.yml

@@ -1,32 +0,0 @@
-name: Arduino Library CI
-
-on: [pull_request, push, repository_dispatch]
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    
-    steps:
-    - uses: actions/setup-python@v4
-      with:
-        python-version: '3.x'
-    - uses: actions/checkout@v3
-    - uses: actions/checkout@v3
-      with:
-         repository: adafruit/ci-arduino
-         path: ci
-
-    - name: pre-install
-      run: bash ci/actions_install.sh
-
-    - name: test platforms
-      run: python3 ci/build_platform.py main_platforms
-
-    - name: clang
-      run: python3 ci/run-clang-format.py -e "ci/*" -e "bin/*" -r . 
-
-    - name: doxygen
-      env:
-        GH_REPO_TOKEN: ${{ secrets.GH_REPO_TOKEN }}
-        PRETTYNAME : "Adafruit AHRS Library"
-      run: bash ci/doxy_gen_and_deploy.sh

.gitignore

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

Adafruit_10DOF.cpp

@@ -0,0 +1,39 @@
+#include "Adafruit_10DOF.h"
+
+Adafruit_10DOF::Adafruit_10DOF():
+  _accel(30301),
+  _mag(30302),
+  _gyro(30303),
+  _bmp(18001)
+{}
+
+bool Adafruit_10DOF::begin() {
+  if (!_accel.begin()) {
+    return false;
+  }
+  if (!_mag.begin()) {
+    return false;
+  }
+  if (!_gyro.begin()) {
+    return false;
+  }
+  if (!_bmp.begin()) {
+    return false;
+  }
+  return true;
+}
+
+Adafruit_Sensor* Adafruit_10DOF::getSensor(sensors_type_t type) {
+  switch (type) {
+    case SENSOR_TYPE_ACCELEROMETER:
+      return &_accel;
+    case SENSOR_TYPE_MAGNETIC_FIELD:
+      return &_mag;
+    case SENSOR_TYPE_GYROSCOPE:
+      return &_gyro;
+    case SENSOR_TYPE_PRESSURE:
+      return &_bmp;
+    default:
+      return NULL;
+  }
+}

Adafruit_10DOF.h

@@ -0,0 +1,30 @@
+#ifndef __ADAFRUIT_10DOF_H__
+#define __ADAFRUIT_10DOF_H__
+
+#include <Adafruit_Sensor.h>
+#include <Adafruit_LSM303_U.h>
+#include <Adafruit_L3GD20_U.h>
+#include <Adafruit_BMP085_U.h>
+#include "Adafruit_Sensor_Set.h"
+
+// 10DOF board with an accelerometer, magnetometer, gyroscope, and pressure sensor.
+class Adafruit_10DOF: public Adafruit_Sensor_Set
+{
+public:
+  Adafruit_10DOF();
+  bool begin();
+  Adafruit_Sensor* getSensor(sensors_type_t type);
+  Adafruit_LSM303_Accel_Unified& getAccel() { return _accel; }
+  Adafruit_LSM303_Mag_Unified& getMag() { return _mag; }
+  Adafruit_L3GD20_Unified& getGyro() { return _gyro; }
+  Adafruit_BMP085_Unified& getBMP() { return _bmp; }
+  
+private:
+  Adafruit_LSM303_Accel_Unified _accel;
+  Adafruit_LSM303_Mag_Unified _mag;
+  Adafruit_L3GD20_Unified _gyro;
+  Adafruit_BMP085_Unified _bmp;
+
+};
+
+#endif

Adafruit_9DOF.cpp

@@ -0,0 +1,33 @@
+#include "Adafruit_9DOF.h"
+
+Adafruit_9DOF::Adafruit_9DOF():
+  _accel(30301),
+  _mag(30302),
+  _gyro(30303)
+{}
+
+bool Adafruit_9DOF::begin() {
+  if (!_accel.begin()) {
+    return false;
+  }
+  if (!_mag.begin()) {
+    return false;
+  }
+  if (!_gyro.begin()) {
+    return false;
+  }
+  return true;
+}
+
+Adafruit_Sensor* Adafruit_9DOF::getSensor(sensors_type_t type) {
+  switch (type) {
+    case SENSOR_TYPE_ACCELEROMETER:
+      return &_accel;
+    case SENSOR_TYPE_MAGNETIC_FIELD:
+      return &_mag;
+    case SENSOR_TYPE_GYROSCOPE:
+      return &_gyro;
+    default:
+      return NULL;
+  }
+}

Adafruit_9DOF.h

@@ -0,0 +1,33 @@
+#ifndef __ADAFRUIT_9DOF_H__
+#define __ADAFRUIT_9DOF_H__
+
+#if (ARDUINO >= 100)
+ #include "Arduino.h"
+#else
+ #include "WProgram.h"
+#endif
+
+#include <Adafruit_Sensor.h>
+#include <Adafruit_LSM303_U.h>
+#include <Adafruit_L3GD20_U.h>
+#include "Adafruit_Sensor_Set.h"
+
+// 9DOF board with an accelerometer, magnetometer, and gyroscope.
+class Adafruit_9DOF: public Adafruit_Sensor_Set
+{
+public:
+  Adafruit_9DOF();
+  bool begin();
+  Adafruit_Sensor* getSensor(sensors_type_t type);
+  Adafruit_LSM303_Accel_Unified& getAccel() { return _accel; }
+  Adafruit_LSM303_Mag_Unified& getMag() { return _mag; }
+  Adafruit_L3GD20_Unified& getGyro() { return _gyro; }
+  
+private:
+  Adafruit_LSM303_Accel_Unified _accel;
+  Adafruit_LSM303_Mag_Unified _mag;
+  Adafruit_L3GD20_Unified _gyro;
+
+};
+
+#endif

Adafruit_Sensor_Set.h

@@ -4,10 +4,11 @@
 #include <Adafruit_Sensor.h>
 
 // Interface for a device that has multiple sensors that can be queried by type.
-class Adafruit_Sensor_Set {
+class Adafruit_Sensor_Set
+{
 public:
   virtual ~Adafruit_Sensor_Set() {}
-  virtual Adafruit_Sensor *getSensor(sensors_type_t type) { return NULL; }
+  virtual Adafruit_Sensor* getSensor(sensors_type_t type) { return NULL; }
 };
 
 #endif

Adafruit_Simple_AHRS.cpp

@@ -0,0 +1,72 @@
+#include "Adafruit_Simple_AHRS.h"
+
+// Create a simple AHRS from an explicit accelerometer and magnetometer sensor.
+Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafruit_Sensor* magnetometer):
+  _accel(accelerometer),
+  _mag(magnetometer)
+{}
+
+// Create a simple AHRS from a device with multiple sensors.
+Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor_Set& sensors):
+  _accel(sensors.getSensor(SENSOR_TYPE_ACCELEROMETER)),
+  _mag(sensors.getSensor(SENSOR_TYPE_MAGNETIC_FIELD))
+{}
+
+// Compute orientation based on accelerometer and magnetometer data.
+bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t* orientation) {
+  // Validate input and available sensors.
+  if (orientation == NULL || _accel == NULL || _mag == NULL) return false;
+
+  // Grab an acceleromter and magnetometer reading.
+  sensors_event_t accel_event;
+  _accel->getEvent(&accel_event);
+  sensors_event_t mag_event;
+  _mag->getEvent(&mag_event);
+
+  float const PI_F = 3.14159265F;
+
+  // roll: Rotation around the X-axis. -180 <= roll <= 180                                          
+  // a positive roll angle is defined to be a clockwise rotation about the positive X-axis          
+  //                                                                                                
+  //                    y                                                                           
+  //      roll = atan2(---)                                                                         
+  //                    z                                                                           
+  //                                                                                                
+  // where:  y, z are returned value from accelerometer sensor                                      
+  orientation->roll = (float)atan2(accel_event.acceleration.y, accel_event.acceleration.z);
+
+  // pitch: Rotation around the Y-axis. -180 <= roll <= 180                                         
+  // a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis         
+  //                                                                                                
+  //                                 -x                                                             
+  //      pitch = atan(-------------------------------)                                             
+  //                    y * sin(roll) + z * cos(roll)                                               
+  //                                                                                                
+  // where:  x, y, z are returned value from accelerometer sensor                                   
+  if (accel_event.acceleration.y * sin(orientation->roll) + accel_event.acceleration.z * cos(orientation->roll) == 0)
+    orientation->pitch = accel_event.acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
+  else
+    orientation->pitch = (float)atan(-accel_event.acceleration.x / (accel_event.acceleration.y * sin(orientation->roll) + \
+                                                                     accel_event.acceleration.z * cos(orientation->roll)));
+
+  // heading: Rotation around the Z-axis. -180 <= roll <= 180                                       
+  // a positive heading angle is defined to be a clockwise rotation about the positive Z-axis       
+  //                                                                                                
+  //                                       z * sin(roll) - y * cos(roll)                            
+  //   heading = atan2(--------------------------------------------------------------------------)  
+  //                    x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll))   
+  //                                                                                                
+  // where:  x, y, z are returned value from magnetometer sensor                                    
+  orientation->heading = (float)atan2(mag_event.magnetic.z * sin(orientation->roll) - mag_event.magnetic.y * cos(orientation->roll), \
+                                      mag_event.magnetic.x * cos(orientation->pitch) + \
+                                      mag_event.magnetic.y * sin(orientation->pitch) * sin(orientation->roll) + \
+                                      mag_event.magnetic.z * sin(orientation->pitch) * cos(orientation->roll));
+
+
+  // Convert angular data to degree 
+  orientation->roll = orientation->roll * 180 / PI_F;
+  orientation->pitch = orientation->pitch * 180 / PI_F;
+  orientation->heading = orientation->heading * 180 / PI_F;
+
+  return true;
+}

Adafruit_Simple_AHRS.h

@@ -0,0 +1,21 @@
+#ifndef __ADAFRUIT_SIMPLE_AHRS_H__
+#define __ADAFRUIT_SIMPLE_AHRS_H__
+
+#include <Adafruit_Sensor.h>
+#include "Adafruit_Sensor_Set.h"
+
+// Simple sensor fusion AHRS using an accelerometer and magnetometer.
+class Adafruit_Simple_AHRS 
+{
+public:
+  Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafruit_Sensor* magnetometer);
+  Adafruit_Simple_AHRS(Adafruit_Sensor_Set& sensors);
+  bool getOrientation(sensors_vec_t* orientation);
+
+private:
+  Adafruit_Sensor* _accel;
+  Adafruit_Sensor* _mag;
+
+};
+
+#endif

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/ahrs_10dof/ahrs_10dof.ino

@@ -1,17 +1,13 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_Accel.h>
-#include <Adafruit_LSM303DLH_Mag.h>
+#include <Adafruit_L3GD20_U.h>
+#include <Adafruit_LSM303_U.h>
 #include <Adafruit_BMP085_U.h>
+#include <Adafruit_10DOF.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);
-
-// Create simple AHRS algorithm using the above sensors.
-Adafruit_Simple_AHRS          ahrs(&accel, &mag);
+Adafruit_10DOF board;
+Adafruit_Simple_AHRS ahrs(board);
 
 // Update this with the correct SLP for accurate altitude measurements
 float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
@@ -19,12 +15,10 @@ float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
 void setup()
 {
   Serial.begin(115200);
-  Serial.println(F("Adafruit 10 DOF Board AHRS Example")); Serial.println("");
+  Serial.println(F("Adafruit 10 DOF Pitch/Roll/Heading Example")); Serial.println("");
   
-  // Initialize the sensors.
-  accel.begin();
-  mag.begin();
-  bmp.begin();
+  // Initialize the board.
+  board.begin();
 }
 
 void loop(void)
@@ -46,15 +40,15 @@ void loop(void)
 
   // Calculate the altitude using the barometric pressure sensor
   sensors_event_t bmp_event;
-  bmp.getEvent(&bmp_event);
+  board.getBMP().getEvent(&bmp_event);
   if (bmp_event.pressure)
   {
     /* Get ambient temperature in C */
     float temperature;
-    bmp.getTemperature(&temperature);
+    board.getBMP().getTemperature(&temperature);
     /* Convert atmospheric pressure, SLP and temp to altitude */
     Serial.print(F("Alt: "));
-    Serial.print(bmp.pressureToAltitude(seaLevelPressure,
+    Serial.print(board.getBMP().pressureToAltitude(seaLevelPressure,
                                         bmp_event.pressure,
                                         temperature)); 
     Serial.println(F(""));

examples/ahrs_9dof/ahrs_9dof.ino

@@ -1,24 +1,24 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_Accel.h>
-#include <Adafruit_LSM303DLH_Mag.h>
+#include <Adafruit_L3GD20_U.h>
+#include <Adafruit_LSM303_U.h>
+#include <Adafruit_BMP085_U.h>
+#include <Adafruit_9DOF.h>
 #include <Adafruit_Simple_AHRS.h>
 
-// Create sensor instances.
-Adafruit_LSM303_Accel_Unified  accel(30301);
-Adafruit_LSM303DLH_Mag_Unified mag(30302);
+Adafruit_9DOF board;
+Adafruit_Simple_AHRS ahrs(board);
 
-// Create simple AHRS algorithm using the above sensors.
-Adafruit_Simple_AHRS          ahrs(&accel, &mag);
+// Update this with the correct SLP for accurate altitude measurements
+float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
 
 void setup()
 {
   Serial.begin(115200);
-  Serial.println(F("Adafruit 9 DOF Board AHRS Example")); Serial.println("");
+  Serial.println(F("Adafruit 10 DOF Pitch/Roll/Heading Example")); Serial.println("");
   
-  // Initialize the sensors.
-  accel.begin();
-  mag.begin();
+  // Initialize the board.
+  board.begin();
 }
 
 void loop(void)

examples/ahrs_calibration_ble_nrf51/ArdPrintf.h

@@ -1,55 +0,0 @@
-#ifndef ARDPRINTF
-#define ARDPRINTF
-#define ARDBUFFER 20
-#include <Arduino.h>
-#include <stdarg.h>
-
-int ardprintf(char *str, ...) {
-  int i, count = 0, j = 0, flag = 0;
-  char temp[ARDBUFFER + 1];
-  for (i = 0; str[i] != '\0'; i++)
-    if (str[i] == '%')
-      count++;
-
-  va_list argv;
-  va_start(argv, count);
-  for (i = 0, j = 0; str[i] != '\0'; i++) {
-    if (str[i] == '%') {
-      temp[j] = '\0';
-      Serial.print(temp);
-      j = 0;
-      temp[0] = '\0';
-
-      switch (str[++i]) {
-      case 'd':
-        Serial.print(va_arg(argv, int));
-        break;
-      case 'l':
-        Serial.print(va_arg(argv, long));
-        break;
-      case 'f':
-        Serial.print(va_arg(argv, double));
-        break;
-      case 'c':
-        Serial.print((char)va_arg(argv, int));
-        break;
-      case 's':
-        Serial.print(va_arg(argv, char *));
-        break;
-      default:;
-      };
-    } else {
-      temp[j] = str[i];
-      j = (j + 1) % ARDBUFFER;
-      if (j == 0) {
-        temp[ARDBUFFER] = '\0';
-        Serial.print(temp);
-        temp[0] = '\0';
-      }
-    }
-  };
-  Serial.println();
-  return count + 1;
-}
-#undef ARDBUFFER
-#endif

examples/ahrs_calibration_ble_nrf51/BluefruitConfig.h

@@ -1,53 +0,0 @@
-// COMMON SETTINGS
-// ----------------------------------------------------------------------------------------------
-// These settings are used in both SW UART, HW UART and SPI mode
-// ----------------------------------------------------------------------------------------------
-#define BUFSIZE 128       // Size of the read buffer for incoming data
-#define VERBOSE_MODE true // If set to 'true' enables debug output
-
-// SOFTWARE UART SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following macros declare the pins that will be used for 'SW' serial.
-// You should use this option if you are connecting the UART Friend to an UNO
-// ----------------------------------------------------------------------------------------------
-#define BLUEFRUIT_SWUART_RXD_PIN 9  // Required for software serial!
-#define BLUEFRUIT_SWUART_TXD_PIN 10 // Required for software serial!
-#define BLUEFRUIT_UART_CTS_PIN 11   // Required for software serial!
-#define BLUEFRUIT_UART_RTS_PIN -1   // Optional, set to -1 if unused
-
-// HARDWARE UART SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following macros declare the HW serial port you are using. Uncomment
-// this line if you are connecting the BLE to Leonardo/Micro or Flora
-// ----------------------------------------------------------------------------------------------
-#ifdef Serial1 // this makes it not complain on compilation if there's no
-               // Serial1
-#define BLUEFRUIT_HWSERIAL_NAME Serial1
-#endif
-
-// SHARED UART SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following sets the optional Mode pin, its recommended but not required
-// ----------------------------------------------------------------------------------------------
-#define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused
-
-// SHARED SPI SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following macros declare the pins to use for HW and SW SPI communication.
-// SCK, MISO and MOSI should be connected to the HW SPI pins on the Uno when
-// using HW SPI.  This should be used with nRF51822 based Bluefruit LE modules
-// that use SPI (Bluefruit LE SPI Friend).
-// ----------------------------------------------------------------------------------------------
-#define BLUEFRUIT_SPI_CS 8
-#define BLUEFRUIT_SPI_IRQ 7
-#define BLUEFRUIT_SPI_RST 6 // Optional but recommended, set to -1 if unused
-
-// SOFTWARE SPI SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following macros declare the pins to use for SW SPI communication.
-// This should be used with nRF51822 based Bluefruit LE modules that use SPI
-// (Bluefruit LE SPI Friend).
-// ----------------------------------------------------------------------------------------------
-#define BLUEFRUIT_SPI_SCK 13
-#define BLUEFRUIT_SPI_MISO 12
-#define BLUEFRUIT_SPI_MOSI 11

examples/ahrs_calibration_ble_nrf51/ahrs_calibration_ble_nrf51.ino

@@ -1,229 +0,0 @@
-#include <Arduino.h>
-#include <SPI.h>
-#if not defined (_VARIANT_ARDUINO_DUE_X_) && not defined (_VARIANT_ARDUINO_ZERO_)
-#include <SoftwareSerial.h>
-#endif
-
-#define BLUEFRUIT51_ST_LSM303DLHC_L3GD20        (0)
-#define BLUEFRUIT51_ST_LSM9DS1                  (1)
-#define BLUEFRUIT51_NXP_FXOS8700_FXAS21002      (2)
-
-// Define your target sensor(s) here based on the list above!
-// #define AHRS_VARIANT    BLUEFRUIT51_ST_LSM303DLHC_L3GD20
-#define AHRS_VARIANT   BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-
-#include "Adafruit_BLE.h"
-#include "Adafruit_BluefruitLE_SPI.h"
-#include "Adafruit_BluefruitLE_UART.h"
-#include "BluefruitConfig.h"
-
-// Include
-#include "ArdPrintf.h"
-
-// Config
-#define FACTORYRESET_ENABLE      1
-
-// Include generic sensors headers
-#include <Wire.h>
-#include <Adafruit_Sensor.h>
-
-// Include appropriate sensor driver(s)
-#if AHRS_VARIANT == BLUEFRUIT51_ST_LSM303DLHC_L3GD20
-#include <Adafruit_L3GD20_U.h>
-#include <Adafruit_LSM303_U.h>
-#elif AHRS_VARIANT == BLUEFRUIT51_ST_LSM9DS1
-// ToDo!
-#elif AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-#include <Adafruit_FXAS21002C.h>
-#include <Adafruit_FXOS8700.h>
-#else
-#error "AHRS_VARIANT undefined! Please select a target sensor combination!"
-#endif
-
-// Bluetooth
-// ...hardware SPI, using SCK/MOSI/MISO hardware SPI pins and then user selected CS/IRQ/RST
-Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);
-
-// Create sensor instances.
-#if AHRS_VARIANT == BLUEFRUIT51_ST_LSM303DLHC_L3GD20
-Adafruit_L3GD20_Unified       gyro(20);
-Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
-#elif AHRS_VARIANT == BLUEFRUIT51_ST_LSM9DS1
-// ToDo!
-#elif AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
-Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
-#endif
-
-void setup()
-{
-  Serial.begin(115200);
-
-  // Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
-  // while(!Serial);
-
-  Serial.println(F("Adafruit nDOF AHRS Calibration Example")); Serial.println("");
-
-  // Initialize the sensors.
-  if (!gyro.begin())
-  {
-    /* There was a problem detecting the gyro ... check your connections */
-    Serial.println("Ooops, no gyroscope detected ... Check your wiring!");
-    while (1);
-  }
-
-#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-  if(!accelmag.begin(ACCEL_RANGE_4G))
-  {
-    Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
-    while(1);
-  }
-#else
-  if (!accel.begin())
-  {
-    /* There was a problem detecting the accel ... check your connections */
-    Serial.println("Ooops, no accel detected ... Check your wiring!");
-    while (1);
-  }
-
-  if (!mag.begin())
-  {
-    /* There was a problem detecting the mag ... check your connections */
-    Serial.println("Ooops, no mag detected ... Check your wiring!");
-    while (1);
-  }
-#endif
-
-  // Initialise the module
-  Serial.print(F("Initialising the Bluefruit LE module: "));
-
-  if ( !ble.begin(VERBOSE_MODE) )
-  {
-    error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
-  }
-  Serial.println( F("OK!") );
-
-  // Factory Reset
-  if ( FACTORYRESET_ENABLE )
-  {
-    /* Perform a factory reset to make sure everything is in a known state */
-    Serial.println(F("Performing a factory reset: "));
-    if ( ! ble.factoryReset() ) {
-      error(F("Couldn't factory reset"));
-    }
-  }
-
-  /* Disable command echo from Bluefruit */
-  ble.echo(false);
-
-  Serial.println("Requesting Bluefruit info:");
-  /* Print Bluefruit information */
-  ble.info();
-
-  /* Wait for a connection before starting the test */
-  Serial.println("Waiting for a BLE connection to continue ...");
-
-  ble.verbose(false);  // debug info is a little annoying after this point!
-
-  // Wait for the connection to complete
-  delay(1000);
-
-  Serial.println(F("CONNECTED!"));
-  Serial.println(F("**********"));
-
-  // Set module to DATA mode
-  Serial.println( F("Switching to DATA mode!") );
-  ble.setMode(BLUEFRUIT_MODE_DATA);
-
-  Serial.println(F("******************************"));
-}
-
-void loop(void)
-{
-  while ( ble.isConnected() ) {
-    sensors_event_t event; // Need to read raw data, which is stored at the same time
-
-    // Get new data samples
-    gyro.getEvent(&event);
-#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-    accelmag.getEvent(&event);
-#else
-    accel.getEvent(&event);
-    mag.getEvent(&event);
-#endif
-
-    // Print the sensor data
-    Serial.print("Raw:");
-#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-    Serial.print(accelmag.accel_raw.x);
-    Serial.print(',');
-    Serial.print(accelmag.accel_raw.y);
-    Serial.print(',');
-    Serial.print(accelmag.accel_raw.z);
-    Serial.print(',');
-#else
-    Serial.print(accel.raw.x);
-    Serial.print(',');
-    Serial.print(accel.raw.y);
-    Serial.print(',');
-    Serial.print(accel.raw.z);
-    Serial.print(',');
-#endif
-    Serial.print(gyro.raw.x);
-    Serial.print(',');
-    Serial.print(gyro.raw.y);
-    Serial.print(',');
-    Serial.print(gyro.raw.z);
-    Serial.print(',');
-#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-    Serial.print(accelmag.mag_raw.x);
-    Serial.print(',');
-    Serial.print(accelmag.mag_raw.y);
-    Serial.print(',');
-    Serial.print(accelmag.mag_raw.z);
-    Serial.println();
-#else
-    Serial.print(mag.raw.x);
-    Serial.print(',');
-    Serial.print(mag.raw.y);
-    Serial.print(',');
-    Serial.print(mag.raw.z);
-    Serial.println();
-#endif
-
-    // Send sensor data to Bluetooth
-    char prefix[] = "R";
-    ble.write(prefix, strlen(prefix));
-    //ble.write((uint8_t *)&event, sizeof(sensors_event_t));
-#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-    ble.write((uint8_t *)&accelmag.accel_raw.x, sizeof(int16_t));
-    ble.write((uint8_t *)&accelmag.accel_raw.y, sizeof(int16_t));
-    ble.write((uint8_t *)&accelmag.accel_raw.z, sizeof(int16_t));
-#else
-    ble.write((uint8_t *)&accel.raw.x, sizeof(int16_t));
-    ble.write((uint8_t *)&accel.raw.y, sizeof(int16_t));
-    ble.write((uint8_t *)&accel.raw.z, sizeof(int16_t));
-#endif
-    ble.write((uint8_t *)&gyro.raw.x, sizeof(int16_t));
-    ble.write((uint8_t *)&gyro.raw.y, sizeof(int16_t));
-    ble.write((uint8_t *)&gyro.raw.z, sizeof(int16_t));
-#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-    ble.write((uint8_t *)&accelmag.mag_raw.x, sizeof(int16_t));
-    ble.write((uint8_t *)&accelmag.mag_raw.y, sizeof(int16_t));
-    ble.write((uint8_t *)&accelmag.mag_raw.z, sizeof(int16_t));
-#else
-    ble.write((uint8_t *)&mag.raw.x, sizeof(int16_t));
-    ble.write((uint8_t *)&mag.raw.y, sizeof(int16_t));
-    ble.write((uint8_t *)&mag.raw.z, sizeof(int16_t));
-#endif
-
-    delay(100);
-  }
-}
-
-// A small helper
-void error(const __FlashStringHelper*err) {
-  Serial.println(err);
-  while (1);
-}

examples/ahrs_fusion_ble_nrf51/BluefruitConfig.h

@@ -1,53 +0,0 @@
-// COMMON SETTINGS
-// ----------------------------------------------------------------------------------------------
-// These settings are used in both SW UART, HW UART and SPI mode
-// ----------------------------------------------------------------------------------------------
-#define BUFSIZE 128       // Size of the read buffer for incoming data
-#define VERBOSE_MODE true // If set to 'true' enables debug output
-
-// SOFTWARE UART SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following macros declare the pins that will be used for 'SW' serial.
-// You should use this option if you are connecting the UART Friend to an UNO
-// ----------------------------------------------------------------------------------------------
-#define BLUEFRUIT_SWUART_RXD_PIN 9  // Required for software serial!
-#define BLUEFRUIT_SWUART_TXD_PIN 10 // Required for software serial!
-#define BLUEFRUIT_UART_CTS_PIN 11   // Required for software serial!
-#define BLUEFRUIT_UART_RTS_PIN -1   // Optional, set to -1 if unused
-
-// HARDWARE UART SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following macros declare the HW serial port you are using. Uncomment
-// this line if you are connecting the BLE to Leonardo/Micro or Flora
-// ----------------------------------------------------------------------------------------------
-#ifdef Serial1 // this makes it not complain on compilation if there's no
-               // Serial1
-#define BLUEFRUIT_HWSERIAL_NAME Serial1
-#endif
-
-// SHARED UART SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following sets the optional Mode pin, its recommended but not required
-// ----------------------------------------------------------------------------------------------
-#define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused
-
-// SHARED SPI SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following macros declare the pins to use for HW and SW SPI communication.
-// SCK, MISO and MOSI should be connected to the HW SPI pins on the Uno when
-// using HW SPI.  This should be used with nRF51822 based Bluefruit LE modules
-// that use SPI (Bluefruit LE SPI Friend).
-// ----------------------------------------------------------------------------------------------
-#define BLUEFRUIT_SPI_CS 8
-#define BLUEFRUIT_SPI_IRQ 7
-#define BLUEFRUIT_SPI_RST 6 // Optional but recommended, set to -1 if unused
-
-// SOFTWARE SPI SETTINGS
-// ----------------------------------------------------------------------------------------------
-// The following macros declare the pins to use for SW SPI communication.
-// This should be used with nRF51822 based Bluefruit LE modules that use SPI
-// (Bluefruit LE SPI Friend).
-// ----------------------------------------------------------------------------------------------
-#define BLUEFRUIT_SPI_SCK 13
-#define BLUEFRUIT_SPI_MISO 12
-#define BLUEFRUIT_SPI_MOSI 11

examples/ahrs_fusion_ble_nrf51/ahrs_fusion_ble_nrf51.ino

@@ -1,257 +0,0 @@
-#include "Adafruit_AHRS_Mahony.h"
-#include "Adafruit_AHRS_Madgwick.h"
-#include "Adafruit_BLE.h"
-#include "Adafruit_BluefruitLE_SPI.h"
-#include "Adafruit_BluefruitLE_UART.h"
-#include "BluefruitConfig.h"
-
-// Note: This sketch is a WORK IN PROGRESS
-
-#define BLUEFRUIT51_ST_LSM303DLHC_L3GD20        (0)
-#define BLUEFRUIT51_ST_LSM9DS1                  (1)
-#define BLUEFRUIT51_NXP_FXOS8700_FXAS21002      (2)
-
-// Define your target sensor(s) here based on the list above!
-// #define AHRS_VARIANT    BLUEFRUIT51_ST_LSM303DLHC_L3GD20
-#define AHRS_VARIANT   BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-
-// Config
-#define FACTORYRESET_ENABLE      1
-
-#if AHRS_VARIANT == BLUEFRUIT51_ST_LSM303DLHC_L3GD20
-#include <Adafruit_L3GD20_U.h>
-#include <Adafruit_LSM303_U.h>
-#elif AHRS_VARIANT == BLUEFRUIT51_ST_LSM9DS1
-// ToDo!
-#elif AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-#include <Adafruit_FXAS21002C.h>
-#include <Adafruit_FXOS8700.h>
-#else
-#error "AHRS_VARIANT undefined! Please select a target sensor combination!"
-#endif
-
-// Create sensor instances.
-#if AHRS_VARIANT == BLUEFRUIT51_ST_LSM303DLHC_L3GD20
-Adafruit_L3GD20_Unified       gyro(20);
-Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
-#elif AHRS_VARIANT == BLUEFRUIT51_ST_LSM9DS1
-// ToDo!
-#elif AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
-Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
-#endif
-
-// Bluetooth
-// ...hardware SPI, using SCK/MOSI/MISO hardware SPI pins and then user selected CS/IRQ/RST
-Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);
-
-// Mag calibration values are calculated via ahrs_calibration.
-// These values must be determined for each baord/environment.
-// See the image in this sketch folder for the values used
-// below.
-
-// Offsets applied to raw x/y/z mag values
-float mag_offsets[3]            = { 2.45F, -4.55F, -26.93F };
-
-// Soft iron error compensation matrix
-float mag_softiron_matrix[3][3] = { {  0.961,  -0.001,  0.025 },
-                                    {  0.001,  0.886,  0.015 },
-                                    {  0.025,  0.015,  1.176 } };
-
-float mag_field_strength        = 44.12F;
-
-// Offsets applied to compensate for gyro zero-drift error for x/y/z
-// Raw values converted to rad/s based on 250dps sensitiviy (1 lsb = 0.00875 rad/s)
-float rawToDPS = 0.00875F;
-float dpsToRad = 0.017453293F;
-float gyro_zero_offsets[3]      = { 175.0F * rawToDPS * dpsToRad,
-                                   -729.0F * rawToDPS * dpsToRad,
-                                    101.0F * rawToDPS * dpsToRad };
-
-
-// Mahony is lighter weight as a filter and should be used
-// on slower systems
-Adafruit_Mahony filter;
-//Adafruit_Madgwick filter;
-
-void setup()
-{
-  Serial.begin(115200);
-
-  // Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
-  //while(!Serial);
-
-  Serial.println(F("Adafruit AHRS BLE Fusion Example")); Serial.println("");
-
-  // Initialize the sensors.
-  if(!gyro.begin())
-  {
-    /* There was a problem detecting the gyro ... check your connections */
-    Serial.println("Ooops, no gyro detected ... Check your wiring!");
-    while(1);
-  }
-
-#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-  if(!accelmag.begin(ACCEL_RANGE_4G))
-  {
-    Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
-    while(1);
-  }
-#else
-  if (!accel.begin())
-  {
-    /* There was a problem detecting the accel ... check your connections */
-    Serial.println("Ooops, no accel detected ... Check your wiring!");
-    while (1);
-  }
-
-  if (!mag.begin())
-  {
-    /* There was a problem detecting the mag ... check your connections */
-    Serial.println("Ooops, no mag detected ... Check your wiring!");
-    while (1);
-  }
-#endif
-
-  // Filter expects 25 samples per second
-  // The filter only seems to be responsive with a much smaller value, though!
-  // ToDo: Figure out the disparity between actual refresh rate and the value
-  // provided here!
-  filter.begin(5);
-
-    // Initialise the module
-  Serial.print(F("Initialising the Bluefruit LE module: "));
-
-  if ( !ble.begin(VERBOSE_MODE) )
-  {
-    Serial.println(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
-  }
-  Serial.println( F("OK!") );
-
-  // Factory Reset
-  if ( FACTORYRESET_ENABLE )
-  {
-    /* Perform a factory reset to make sure everything is in a known state */
-    Serial.println(F("Performing a factory reset: "));
-    if ( ! ble.factoryReset() ) {
-      Serial.println(F("Couldn't factory reset"));
-    }
-  }
-
-  /* Disable command echo from Bluefruit */
-  ble.echo(false);
-
-  Serial.println("Requesting Bluefruit info:");
-  /* Print Bluefruit information */
-  ble.info();
-
-  /* Wait for a connection before starting the test */
-  Serial.println("Waiting for a BLE connection to continue ...");
-
-  ble.verbose(false);  // debug info is a little annoying after this point!
-
-  // Wait for the connection to complete
-  delay(1000);
-
-  Serial.println(F("CONNECTED!"));
-  Serial.println(F("**********"));
-
-  // Set module to DATA mode
-  Serial.println( F("Switching to DATA mode!") );
-  ble.setMode(BLUEFRUIT_MODE_DATA);
-
-  Serial.println(F("******************************"));
-}
-
-void loop(void)
-{
-  sensors_event_t gyro_event;
-  sensors_event_t accel_event;
-  sensors_event_t mag_event;
-
-  while ( ble.isConnected() ) {
-
-    // Get new data samples
-    gyro.getEvent(&gyro_event);
-#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
-    accelmag.getEvent(&accel_event, &mag_event);
-#else
-    accel.getEvent(&accel_event);
-    mag.getEvent(&mag_event);
-#endif
-
-    // Apply mag offset compensation (base values in uTesla)
-    float x = mag_event.magnetic.x - mag_offsets[0];
-    float y = mag_event.magnetic.y - mag_offsets[1];
-    float z = mag_event.magnetic.z - mag_offsets[2];
-
-    // Apply mag soft iron error compensation
-    float mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
-    float my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
-    float mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];
-
-    // Apply gyro zero-rate error compensation
-    float gx = gyro_event.gyro.x - gyro_zero_offsets[0];
-    float gy = gyro_event.gyro.y - gyro_zero_offsets[1];
-    float gz = gyro_event.gyro.z - gyro_zero_offsets[2];
-
-    // The filter library expects gyro data in degrees/s, but adafruit sensor
-    // uses rad/s so we need to convert them first (or adapt the filter lib
-    // where they are being converted)
-    gx *= 57.2958F;
-    gy *= 57.2958F;
-    gz *= 57.2958F;
-
-    // Update the filter
-    filter.update(gx, gy, gz,
-                  accel_event.acceleration.x, accel_event.acceleration.y, accel_event.acceleration.z,
-                  mx, my, mz);
-
-  /*
-    // Print the orientation filter output
-    float roll = filter.getRoll();
-    float pitch = filter.getPitch();
-    float heading = filter.getYaw();
-    Serial.print(millis());
-    Serial.print(" - Orientation: ");
-    Serial.print(pitch);
-    Serial.print(" ");
-    Serial.print(heading);
-    Serial.print(" ");
-    Serial.println(roll);
-
-    // Send sensor data to Bluetooth
-    char prefix[] = "V";
-    ble.write(prefix, strlen(prefix));
-    ble.write((uint8_t *)&pitch, sizeof(float));
-    ble.write((uint8_t *)&heading, sizeof(float));
-    ble.write((uint8_t *)&roll, sizeof(float));
-  */
-
-    // Print the orientation filter output in quaternions.
-    // This avoids the gimbal lock problem with Euler angles when you get
-    // close to 180 degrees (causing the model to rotate or flip, etc.)
-    float qw, qx, qy, qz;
-    filter.getQuaternion(&qw, &qx, &qy, &qz);
-    Serial.print(millis());
-    Serial.print(" - Quat: ");
-    Serial.print(qw);
-    Serial.print(" ");
-    Serial.print(qx);
-    Serial.print(" ");
-    Serial.print(qy);
-    Serial.print(" ");
-    Serial.println(qz);
-
-     // Send sensor data to Bluetooth
-    char prefix[] = "V";
-    ble.write(prefix, strlen(prefix));
-    ble.write((uint8_t *)&qw, sizeof(float));
-    ble.write((uint8_t *)&qx, sizeof(float));
-    ble.write((uint8_t *)&qy, sizeof(float));
-    ble.write((uint8_t *)&qz, sizeof(float));
-
-    delay(10);
-  }
-}

examples/calibrated_orientation/LSM6DS_LIS3MDL.h

@@ -1,35 +0,0 @@
-#include <Adafruit_LIS3MDL.h>
-Adafruit_LIS3MDL lis3mdl;
-
-// Can change this to be LSM6DSOX or whatever ya like
-// For (older) Feather Sense with LSM6DS33, use this:
-#include <Adafruit_LSM6DS33.h>
-Adafruit_LSM6DS33 lsm6ds;
-// For (newer) Feather Sense with LSM6DS3TR-C, use this:
-// #include <Adafruit_LSM6DS3TRC.h>
-// Adafruit_LSM6DS3TRC lsm6ds;
-
-bool init_sensors(void) {
-  if (!lsm6ds.begin_I2C() || !lis3mdl.begin_I2C()) {
-    return false;
-  }
-  accelerometer = lsm6ds.getAccelerometerSensor();
-  gyroscope = lsm6ds.getGyroSensor();
-  magnetometer = &lis3mdl;
-
-  return true;
-}
-
-void setup_sensors(void) {
-  // set lowest range
-  lsm6ds.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
-  lsm6ds.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
-  lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);
-
-  // set slightly above refresh rate
-  lsm6ds.setAccelDataRate(LSM6DS_RATE_104_HZ);
-  lsm6ds.setGyroDataRate(LSM6DS_RATE_104_HZ);
-  lis3mdl.setDataRate(LIS3MDL_DATARATE_1000_HZ);
-  lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
-  lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
-}

examples/calibrated_orientation/LSM9DS.h

@@ -1,30 +0,0 @@
-#include <Adafruit_LSM9DS1.h>
-Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
-
-// Or if you have the older LSM9DS0
-// #include <Adafruit_LSM9DS0.h>
-// Adafruit_LSM9DS0 lsm9ds = Adafruit_LSM9DS0();
-
-bool init_sensors(void) {
-  if (!lsm9ds.begin()) {
-    return false;
-  }
-  accelerometer = &lsm9ds.getAccel();
-  gyroscope = &lsm9ds.getGyro();
-  magnetometer = &lsm9ds.getMag();
-
-  return true;
-}
-
-void setup_sensors(void) {
-  // set lowest range
-#ifdef __LSM9DS0_H__
-  lsm9ds.setupAccel(lsm9ds.LSM9DS0_ACCELRANGE_2G);
-  lsm9ds.setupMag(lsm9ds.LSM9DS0_MAGGAIN_4GAUSS);
-  lsm9ds.setupGyro(lsm9ds.LSM9DS0_GYROSCALE_245DPS);
-#else
-  lsm9ds.setupAccel(lsm9ds.LSM9DS1_ACCELRANGE_2G);
-  lsm9ds.setupMag(lsm9ds.LSM9DS1_MAGGAIN_4GAUSS);
-  lsm9ds.setupGyro(lsm9ds.LSM9DS1_GYROSCALE_245DPS);
-#endif
-}

examples/calibrated_orientation/LSM9DS1.h

@@ -1,20 +0,0 @@
-#include <Adafruit_LSM9DS1.h>
-Adafruit_LSM9DS1 lsm9ds1 = Adafruit_LSM9DS1();
-
-bool init_sensors(void) {
-  if (!lsm9ds1.begin()) {
-    return false;
-  }
-  accelerometer = &lsm9ds1.getAccel();
-  gyroscope = &lsm9ds1.getGyro();
-  magnetometer = &lsm9ds1.getMag();
-
-  return true;
-}
-
-void setup_sensors(void) {
-  // set lowest range
-  lsm9ds1.setupAccel(lsm9ds1.LSM9DS1_ACCELRANGE_2G);
-  lsm9ds1.setupMag(lsm9ds1.LSM9DS1_MAGGAIN_4GAUSS);
-  lsm9ds1.setupGyro(lsm9ds1.LSM9DS1_GYROSCALE_245DPS);
-}

examples/calibrated_orientation/NXP_FXOS_FXAS.h

@@ -1,18 +0,0 @@
-#include <Adafruit_FXAS21002C.h>
-#include <Adafruit_FXOS8700.h>
-
-Adafruit_FXOS8700 fxos = Adafruit_FXOS8700(0x8700A, 0x8700B);
-Adafruit_FXAS21002C fxas = Adafruit_FXAS21002C(0x0021002C);
-
-bool init_sensors(void) {
-  if (!fxos.begin() || !fxas.begin()) {
-    return false;
-  }
-  accelerometer = fxos.getAccelerometerSensor();
-  gyroscope = &fxas;
-  magnetometer = fxos.getMagnetometerSensor();
-
-  return true;
-}
-
-void setup_sensors(void) {}

examples/calibrated_orientation/calibrated_orientation.ino

@@ -1,144 +0,0 @@
-// Full orientation sensing using NXP/Madgwick/Mahony and a range of 9-DoF
-// sensor sets.
-// You *must* perform a magnetic calibration before this code will work.
-//
-// To view this data, use the Arduino Serial Monitor to watch the
-// scrolling angles, or run the OrientationVisualiser example in Processing.
-// Based on  https://github.com/PaulStoffregen/NXPMotionSense with adjustments
-// to Adafruit Unified Sensor interface
-
-#include <Adafruit_Sensor_Calibration.h>
-#include <Adafruit_AHRS.h>
-
-Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
-
-// uncomment one combo 9-DoF!
-#include "LSM6DS_LIS3MDL.h"  // see the the LSM6DS_LIS3MDL file in this project to change board to LSM6DS33, LSM6DS3U, LSM6DSOX, etc
-//#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
-//#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout
-
-// pick your filter! slower == better quality output
-//Adafruit_NXPSensorFusion filter; // slowest
-//Adafruit_Madgwick filter;  // faster than NXP
-Adafruit_Mahony filter;  // fastest/smalleset
-
-#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)
-  Adafruit_Sensor_Calibration_EEPROM cal;
-#else
-  Adafruit_Sensor_Calibration_SDFat cal;
-#endif
-
-#define FILTER_UPDATE_RATE_HZ 100
-#define PRINT_EVERY_N_UPDATES 10
-//#define AHRS_DEBUG_OUTPUT
-
-uint32_t timestamp;
-
-void setup() {
-  Serial.begin(115200);
-  while (!Serial) yield();
-
-  if (!cal.begin()) {
-    Serial.println("Failed to initialize calibration helper");
-  } else if (! cal.loadCalibration()) {
-    Serial.println("No calibration loaded/found");
-  }
-
-  if (!init_sensors()) {
-    Serial.println("Failed to find sensors");
-    while (1) delay(10);
-  }
-  
-  accelerometer->printSensorDetails();
-  gyroscope->printSensorDetails();
-  magnetometer->printSensorDetails();
-
-  setup_sensors();
-  filter.begin(FILTER_UPDATE_RATE_HZ);
-  timestamp = millis();
-
-  Wire.setClock(400000); // 400KHz
-}
-
-
-void loop() {
-  float roll, pitch, heading;
-  float gx, gy, gz;
-  static uint8_t counter = 0;
-
-  if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
-    return;
-  }
-  timestamp = millis();
-  // Read the motion sensors
-  sensors_event_t accel, gyro, mag;
-  accelerometer->getEvent(&accel);
-  gyroscope->getEvent(&gyro);
-  magnetometer->getEvent(&mag);
-#if defined(AHRS_DEBUG_OUTPUT)
-  Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
-#endif
-
-  cal.calibrate(mag);
-  cal.calibrate(accel);
-  cal.calibrate(gyro);
-  // Gyroscope needs to be converted from Rad/s to Degree/s
-  // the rest are not unit-important
-  gx = gyro.gyro.x * SENSORS_RADS_TO_DPS;
-  gy = gyro.gyro.y * SENSORS_RADS_TO_DPS;
-  gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;
-
-  // Update the SensorFusion filter
-  filter.update(gx, gy, gz, 
-                accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, 
-                mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
-#if defined(AHRS_DEBUG_OUTPUT)
-  Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
-#endif
-
-  // only print the calculated output once in a while
-  if (counter++ <= PRINT_EVERY_N_UPDATES) {
-    return;
-  }
-  // reset the counter
-  counter = 0;
-
-#if defined(AHRS_DEBUG_OUTPUT)
-  Serial.print("Raw: ");
-  Serial.print(accel.acceleration.x, 4); Serial.print(", ");
-  Serial.print(accel.acceleration.y, 4); Serial.print(", ");
-  Serial.print(accel.acceleration.z, 4); Serial.print(", ");
-  Serial.print(gx, 4); Serial.print(", ");
-  Serial.print(gy, 4); Serial.print(", ");
-  Serial.print(gz, 4); Serial.print(", ");
-  Serial.print(mag.magnetic.x, 4); Serial.print(", ");
-  Serial.print(mag.magnetic.y, 4); Serial.print(", ");
-  Serial.print(mag.magnetic.z, 4); Serial.println("");
-#endif
-
-  // print the heading, pitch and roll
-  roll = filter.getRoll();
-  pitch = filter.getPitch();
-  heading = filter.getYaw();
-  Serial.print("Orientation: ");
-  Serial.print(heading);
-  Serial.print(", ");
-  Serial.print(pitch);
-  Serial.print(", ");
-  Serial.println(roll);
-
-  float qw, qx, qy, qz;
-  filter.getQuaternion(&qw, &qx, &qy, &qz);
-  Serial.print("Quaternion: ");
-  Serial.print(qw, 4);
-  Serial.print(", ");
-  Serial.print(qx, 4);
-  Serial.print(", ");
-  Serial.print(qy, 4);
-  Serial.print(", ");
-  Serial.println(qz, 4);  
-  
-#if defined(AHRS_DEBUG_OUTPUT)
-  Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms");
-#endif
-}

examples/calibration/LSM6DS_LIS3MDL.h

@@ -1,35 +0,0 @@
-#include <Adafruit_LIS3MDL.h>
-Adafruit_LIS3MDL lis3mdl;
-
-// Can change this to be LSM6DS33 or whatever ya like
-// For (older) Feather Sense with LSM6DS33, use this:
-#include <Adafruit_LSM6DS33.h>
-Adafruit_LSM6DS33 lsm6ds;
-// For (newer) Feather Sense with LSM6DS3TR-C, use this:
-// #include <Adafruit_LSM6DS3TRC.h>
-// Adafruit_LSM6DS3TRC lsm6ds;
-
-bool init_sensors(void) {
-  if (!lsm6ds.begin_I2C() || !lis3mdl.begin_I2C()) {
-    return false;
-  }
-  accelerometer = lsm6ds.getAccelerometerSensor();
-  gyroscope = lsm6ds.getGyroSensor();
-  magnetometer = &lis3mdl;
-
-  return true;
-}
-
-void setup_sensors(void) {
-  // set lowest range
-  lsm6ds.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
-  lsm6ds.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
-  lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS);
-
-  // set slightly above refresh rate
-  lsm6ds.setAccelDataRate(LSM6DS_RATE_104_HZ);
-  lsm6ds.setGyroDataRate(LSM6DS_RATE_104_HZ);
-  lis3mdl.setDataRate(LIS3MDL_DATARATE_1000_HZ);
-  lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE);
-  lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
-}

examples/calibration/LSM9DS.h

@@ -1,30 +0,0 @@
-// #include <Adafruit_LSM9DS1.h>
-//  Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
-
-// Or if you have the older LSM9DS0
-#include <Adafruit_LSM9DS0.h>
-Adafruit_LSM9DS0 lsm9ds = Adafruit_LSM9DS0();
-
-bool init_sensors(void) {
-  if (!lsm9ds.begin()) {
-    return false;
-  }
-  accelerometer = &lsm9ds.getAccel();
-  gyroscope = &lsm9ds.getGyro();
-  magnetometer = &lsm9ds.getMag();
-
-  return true;
-}
-
-void setup_sensors(void) {
-  // set lowest range
-#ifdef __LSM9DS0_H__
-  lsm9ds.setupAccel(lsm9ds.LSM9DS0_ACCELRANGE_2G);
-  lsm9ds.setupMag(lsm9ds.LSM9DS0_MAGGAIN_4GAUSS);
-  lsm9ds.setupGyro(lsm9ds.LSM9DS0_GYROSCALE_245DPS);
-#else
-  lsm9ds.setupAccel(lsm9ds.LSM9DS1_ACCELRANGE_2G);
-  lsm9ds.setupMag(lsm9ds.LSM9DS1_MAGGAIN_4GAUSS);
-  lsm9ds.setupGyro(lsm9ds.LSM9DS1_GYROSCALE_245DPS);
-#endif
-}

examples/calibration/NXP_FXOS_FXAS.h

@@ -1,18 +0,0 @@
-#include <Adafruit_FXAS21002C.h>
-#include <Adafruit_FXOS8700.h>
-
-Adafruit_FXOS8700 fxos = Adafruit_FXOS8700(0x8700A, 0x8700B);
-Adafruit_FXAS21002C fxas = Adafruit_FXAS21002C(0x0021002C);
-
-bool init_sensors(void) {
-  if (!fxos.begin() || !fxas.begin()) {
-    return false;
-  }
-  accelerometer = fxos.getAccelerometerSensor();
-  gyroscope = &fxas;
-  magnetometer = fxos.getMagnetometerSensor();
-
-  return true;
-}
-
-void setup_sensors(void) {}

examples/calibration/calibration.ino

@@ -1,225 +0,0 @@
-/***************************************************************************
-  This is an example for the Adafruit AHRS library
-  It will look for a supported magnetometer and output
-  PJRC Motion Sensor Calibration Tool-compatible serial data
-
-  PJRC & Adafruit invest time and resources providing this open source code,
-  please support PJRC and open-source hardware by purchasing products
-  from PJRC!
-
-  Written by PJRC, adapted by Limor Fried for Adafruit Industries.
- ***************************************************************************/
-
-#include <Adafruit_Sensor_Calibration.h>
-
-Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
-
-// uncomment one combo 9-DoF!
-#include "LSM6DS_LIS3MDL.h"  // see the the LSM6DS_LIS3MDL file in this project to change board to LSM6DS33, LSM6DS3U, LSM6DSOX, etc
-//#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
-//#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout
-
-// select either EEPROM or SPI FLASH storage:
-#ifdef ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM
-  Adafruit_Sensor_Calibration_EEPROM cal;
-#else
-  Adafruit_Sensor_Calibration_SDFat cal;
-#endif
-
-sensors_event_t mag_event, gyro_event, accel_event;
-
-int loopcount = 0;
-
-void setup(void) {
-  Serial.begin(115200);
-  while (!Serial) delay(10);     // will pause Zero, Leonardo, etc until serial console opens
-  
-  Serial.println(F("Adafruit AHRS - IMU Calibration!"));
-
-  Serial.println("Calibration filesys test");
-  if (!cal.begin()) {
-    Serial.println("Failed to initialize calibration helper");
-    while (1) yield();
-  }
-  if (! cal.loadCalibration()) {
-    Serial.println("No calibration loaded/found... will start with defaults");
-  } else {
-    Serial.println("Loaded existing calibration");
-  }
-
-  if (!init_sensors()) {
-    Serial.println("Failed to find sensors");
-    while (1) delay(10);
-  }
-  accelerometer->printSensorDetails();
-  gyroscope->printSensorDetails();
-  magnetometer->printSensorDetails();
-
-  setup_sensors();
-  
-  Wire.setClock(400000); // 400KHz
-}
-
-void loop() {
-  magnetometer->getEvent(&mag_event);
-  gyroscope->getEvent(&gyro_event);
-  accelerometer->getEvent(&accel_event);
-  
-  // 'Raw' values to match expectation of MotionCal
-  Serial.print("Raw:");
-  Serial.print(int(accel_event.acceleration.x*8192/9.8)); Serial.print(",");
-  Serial.print(int(accel_event.acceleration.y*8192/9.8)); Serial.print(",");
-  Serial.print(int(accel_event.acceleration.z*8192/9.8)); Serial.print(",");
-  Serial.print(int(gyro_event.gyro.x*SENSORS_RADS_TO_DPS*16)); Serial.print(",");
-  Serial.print(int(gyro_event.gyro.y*SENSORS_RADS_TO_DPS*16)); Serial.print(",");
-  Serial.print(int(gyro_event.gyro.z*SENSORS_RADS_TO_DPS*16)); Serial.print(",");
-  Serial.print(int(mag_event.magnetic.x*10)); Serial.print(",");
-  Serial.print(int(mag_event.magnetic.y*10)); Serial.print(",");
-  Serial.print(int(mag_event.magnetic.z*10)); Serial.println("");
-
-  // unified data
-  Serial.print("Uni:");
-  Serial.print(accel_event.acceleration.x); Serial.print(",");
-  Serial.print(accel_event.acceleration.y); Serial.print(",");
-  Serial.print(accel_event.acceleration.z); Serial.print(",");
-  Serial.print(gyro_event.gyro.x, 4); Serial.print(",");
-  Serial.print(gyro_event.gyro.y, 4); Serial.print(",");
-  Serial.print(gyro_event.gyro.z, 4); Serial.print(",");
-  Serial.print(mag_event.magnetic.x); Serial.print(",");
-  Serial.print(mag_event.magnetic.y); Serial.print(",");
-  Serial.print(mag_event.magnetic.z); Serial.println("");
-  loopcount++;
-  receiveCalibration();
-
-  // occasionally print calibration
-  if (loopcount == 50 || loopcount > 100) {
-    Serial.print("Cal1:");
-    for (int i=0; i<3; i++) {
-      Serial.print(cal.accel_zerog[i], 3); 
-      Serial.print(",");
-    }
-    for (int i=0; i<3; i++) {
-      Serial.print(cal.gyro_zerorate[i], 3);
-      Serial.print(",");
-    }  
-    for (int i=0; i<3; i++) {
-      Serial.print(cal.mag_hardiron[i], 3); 
-      Serial.print(",");
-    }  
-    Serial.println(cal.mag_field, 3);
-    loopcount++;
-  }
-  if (loopcount >= 100) {
-    Serial.print("Cal2:");
-    for (int i=0; i<9; i++) {
-      Serial.print(cal.mag_softiron[i], 4); 
-      if (i < 8) Serial.print(',');
-    }
-    Serial.println();
-    loopcount = 0;
-  }
- 
-  delay(10); 
-}
-
-/********************************************************/
-
-byte caldata[68]; // buffer to receive magnetic calibration data
-byte calcount=0;
-
-void receiveCalibration() {
-  uint16_t crc;
-  byte b, i;
-
-  while (Serial.available()) {
-    b = Serial.read();
-    if (calcount == 0 && b != 117) {
-      // first byte must be 117
-      return;
-    }
-    if (calcount == 1 && b != 84) {
-      // second byte must be 84
-      calcount = 0;
-      return;
-    }
-    // store this byte
-    caldata[calcount++] = b;
-    if (calcount < 68) {
-      // full calibration message is 68 bytes
-      return;
-    }
-    // verify the crc16 check
-    crc = 0xFFFF;
-    for (i=0; i < 68; i++) {
-      crc = crc16_update(crc, caldata[i]);
-    }
-    if (crc == 0) {
-      // data looks good, use it
-      float offsets[16];
-      memcpy(offsets, caldata+2, 16*4);
-      cal.accel_zerog[0] = offsets[0];
-      cal.accel_zerog[1] = offsets[1];
-      cal.accel_zerog[2] = offsets[2];
-      
-      cal.gyro_zerorate[0] = offsets[3];
-      cal.gyro_zerorate[1] = offsets[4];
-      cal.gyro_zerorate[2] = offsets[5];
-      
-      cal.mag_hardiron[0] = offsets[6];
-      cal.mag_hardiron[1] = offsets[7];
-      cal.mag_hardiron[2] = offsets[8];
-
-      cal.mag_field = offsets[9];
-      
-      cal.mag_softiron[0] = offsets[10];
-      cal.mag_softiron[1] = offsets[13];
-      cal.mag_softiron[2] = offsets[14];
-      cal.mag_softiron[3] = offsets[13];
-      cal.mag_softiron[4] = offsets[11];
-      cal.mag_softiron[5] = offsets[15];
-      cal.mag_softiron[6] = offsets[14];
-      cal.mag_softiron[7] = offsets[15];
-      cal.mag_softiron[8] = offsets[12];
-
-      if (! cal.saveCalibration()) {
-        Serial.println("**WARNING** Couldn't save calibration");
-      } else {
-        Serial.println("Wrote calibration");    
-      }
-      cal.printSavedCalibration();
-      calcount = 0;
-      return;
-    }
-    // look for the 117,84 in the data, before discarding
-    for (i=2; i < 67; i++) {
-      if (caldata[i] == 117 && caldata[i+1] == 84) {
-        // found possible start within data
-        calcount = 68 - i;
-        memmove(caldata, caldata + i, calcount);
-        return;
-      }
-    }
-    // look for 117 in last byte
-    if (caldata[67] == 117) {
-      caldata[0] = 117;
-      calcount = 1;
-    } else {
-      calcount = 0;
-    }
-  }
-}
-
-
-uint16_t crc16_update(uint16_t crc, uint8_t a)
-{
-  int i;
-  crc ^= a;
-  for (i = 0; i < 8; i++) {
-    if (crc & 1) {
-      crc = (crc >> 1) ^ 0xA001;
-    } else {
-      crc = (crc >> 1);
-    }
-  }
-  return crc;
-}

library.properties

@@ -1,10 +0,0 @@
-name=Adafruit AHRS
-version=2.4.0
-author=Adafruit
-maintainer=Adafruit <info@adafruit.com>
-sentence=AHRS (Altitude and Heading Reference System) for various Adafruit motion sensors
-paragraph=Includes motion calibration example sketches, as well as calibration orientation output using Mahony, Madgwick, NXP Fusion, etc fusion filters
-category=Sensors
-url=https://github.com/adafruit/Adafruit_AHRS
-architectures=*
-depends=Adafruit Unified Sensor, Adafruit LSM6DS, Adafruit LIS3MDL, Adafruit FXOS8700, Adafruit FXAS21002C, Adafruit LSM9DS1 Library, Adafruit LSM9DS0 Library, Adafruit BMP085 Unified, Adafruit BluefruitLE nRF51, SdFat - Adafruit Fork, ArduinoJson, Adafruit SPIFlash, Adafruit Sensor Calibration, Adafruit LSM303 Accel, Adafruit LSM303DLH Mag

processing/bunnyrotate/bunnyrotate.pde

@@ -27,7 +27,7 @@ GCheckbox printSerialCheckbox;
 
 void setup()
 {
-  size(400, 500, OPENGL);
+  size(400, 550, OPENGL);
   frameRate(30);
   model = new OBJModel(this);
   model.load("bunny.obj");
@@ -79,7 +79,7 @@ void draw()
   pointLight(255, 255, 255,    0,   0, -500);
   
   // Displace objects from 0,0
-  translate(200, 350, 0);
+  translate(200, 300, 0);
   
   // Rotate shapes around the X/Y/Z axis (values in radians, 0..Pi*2)
   rotateX(radians(roll));

processing/bunnyrotate_ahrs_fusion_usb/bunnyrotate_ahrs_fusion_usb.pde

@@ -1,165 +0,0 @@
-import processing.serial.*;
-import java.awt.datatransfer.*;
-import java.awt.Toolkit;
-import processing.opengl.*;
-import saito.objloader.*;
-import g4p_controls.*;
-
-float roll  = 0.0F;
-float pitch = 0.0F;
-float yaw   = 0.0F;
-float temp  = 0.0F;
-float alt   = 0.0F;
-
-OBJModel model;
-
-// Serial port state.
-Serial       port;
-String       buffer = "";
-final String serialConfigFile = "serialconfig.txt";
-boolean      printSerial = false;
-
-// UI controls.
-GPanel    configPanel;
-GDropList serialList;
-GLabel    serialLabel;
-GCheckbox printSerialCheckbox;
-
-void setup()
-{
-  size(400, 500, OPENGL);
-  frameRate(30);
-  model = new OBJModel(this);
-  model.load("bunny.obj");
-  model.scale(20);
-  
-  // Serial port setup.
-  // Grab list of serial ports and choose one that was persisted earlier or default to the first port.
-  int selectedPort = 0;
-  String[] availablePorts = Serial.list();
-  if (availablePorts == null) {
-    println("ERROR: No serial ports available!");
-    exit();
-  }
-  String[] serialConfig = loadStrings(serialConfigFile);
-  if (serialConfig != null && serialConfig.length > 0) {
-    String savedPort = serialConfig[0];
-    // Check if saved port is in available ports.
-    for (int i = 0; i < availablePorts.length; ++i) {
-      if (availablePorts[i].equals(savedPort)) {
-        selectedPort = i;
-      } 
-    }
-  }
-  // Build serial config UI.
-  configPanel = new GPanel(this, 10, 10, width-20, 90, "Configuration (click to hide/show)");
-  serialLabel = new GLabel(this,  0, 20, 80, 25, "Serial port:");
-  configPanel.addControl(serialLabel);
-  serialList = new GDropList(this, 90, 20, 200, 200, 6);
-  serialList.setItems(availablePorts, selectedPort);
-  configPanel.addControl(serialList);
-  printSerialCheckbox = new GCheckbox(this, 5, 50, 200, 20, "Print serial data");
-  printSerialCheckbox.setSelected(printSerial);
-  configPanel.addControl(printSerialCheckbox);
-  // Set serial port.
-  setSerialPort(serialList.getSelectedText());
-}
- 
-void draw()
-{
-  background(0,0, 0);
-
-  // Set a new co-ordinate space
-  pushMatrix();
-
-  // Simple 3 point lighting for dramatic effect.
-  // Slightly red light in upper right, slightly blue light in upper left, and white light from behind.
-  pointLight(255, 200, 200,  400, 400,  500);
-  pointLight(200, 200, 255, -400, 400,  500);
-  pointLight(255, 255, 255,    0,   0, -500);
-  
-  // Displace objects from 0,0
-  translate(200, 350, 0);
-  
-  // Rotate shapes around the X/Y/Z axis (values in radians, 0..Pi*2)
-  rotateX(radians(roll));
-  rotateZ(radians(pitch));
-  rotateY(radians(yaw));
-
-  pushMatrix();
-  noStroke();
-  model.draw();
-  popMatrix();
-  popMatrix();
-  //print("draw");
-}
-
-void serialEvent(Serial p) 
-{
-  String incoming = p.readString();
-  if (printSerial) {
-    println(incoming);
-  }
-  
-  if ((incoming.length() > 8))
-  {
-    String[] list = split(incoming, " ");
-    if ( (list.length > 0) && (list[2].equals("Orientation:")) ) 
-    {
-      roll  = float(list[5]);
-      pitch = float(list[4]);
-      yaw   = float(list[3]);
-      buffer = incoming;
-    }
-    if ( (list.length > 0) && (list[2].equals("Alt:")) ) 
-    {
-      alt  = float(list[3]);
-      buffer = incoming;
-    }
-    if ( (list.length > 0) && (list[2].equals("Temp:")) ) 
-    {
-      temp  = float(list[3]);
-      buffer = incoming;
-    }
-  }
-}
-
-// Set serial port to desired value.
-void setSerialPort(String portName) {
-  // Close the port if it's currently open.
-  if (port != null) {
-    port.stop();
-  }
-  try {
-    // Open port.
-    port = new Serial(this, portName, 115200);
-    port.bufferUntil('\n');
-    // Persist port in configuration.
-    saveStrings(serialConfigFile, new String[] { portName });
-  }
-  catch (RuntimeException ex) {
-    // Swallow error if port can't be opened, keep port closed.
-    port = null; 
-  }
-}
-
-// UI event handlers
-
-void handlePanelEvents(GPanel panel, GEvent event) {
-  // Panel events, do nothing.
-}
-
-void handleDropListEvents(GDropList list, GEvent event) { 
-  // Drop list events, check if new serial port is selected.
-  if (list == serialList) {
-    setSerialPort(serialList.getSelectedText()); 
-  }
-}
-
-void handleToggleControlEvents(GToggleControl checkbox, GEvent event) { 
-  // Checkbox toggle events, check if print events is toggled.
-  if (checkbox == printSerialCheckbox) {
-    printSerial = printSerialCheckbox.isSelected(); 
-  }
-}
-

processing/bunnyrotate_ahrs_fusion_usb/data/bunny.mtl

@@ -1,13 +0,0 @@
-# 3ds Max Wavefront OBJ Exporter v0.94b - (c)2007 guruware
-# File Created: 04.07.2010 10:41:39
-
-newmtl Body
-Ns 32
-d 1
-Tr 1
-Tf 1 1 1
-illum 2
-Ka 0.0000 0.0000 0.0000
-Kd 0.7412 0.4784 0.4765
-Ks 0.3500 0.3500 0.6500
-

src/Adafruit_AHRS.h

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

@@ -1,305 +0,0 @@
-//=============================================================================================
-// Madgwick.c
-//=============================================================================================
-//
-// Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-//
-// From the x-io website "Open-source resources available on this website are
-// provided under the GNU General Public Licence unless an alternative licence
-// is provided in source."
-//
-// Date			Author          Notes
-// 29/09/2011	SOH Madgwick    Initial release
-// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
-// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
-//
-//=============================================================================================
-
-//-------------------------------------------------------------------------------------------
-// Header files
-
-#include "Adafruit_AHRS_Madgwick.h"
-#include <math.h>
-
-//-------------------------------------------------------------------------------------------
-// Definitions
-
-#define sampleFreqDef 512.0f // sample frequency in Hz
-#define betaDef 0.1f         // 2 * proportional gain
-
-//============================================================================================
-// Functions
-
-//-------------------------------------------------------------------------------------------
-// AHRS algorithm update
-
-Adafruit_Madgwick::Adafruit_Madgwick() : Adafruit_Madgwick(betaDef) {}
-
-Adafruit_Madgwick::Adafruit_Madgwick(float gain) {
-  beta = gain;
-  q0 = 1.0f;
-  q1 = 0.0f;
-  q2 = 0.0f;
-  q3 = 0.0f;
-  invSampleFreq = 1.0f / sampleFreqDef;
-  anglesComputed = false;
-}
-
-void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
-                               float az, float mx, float my, float mz,
-                               float dt) {
-  float recipNorm;
-  float s0, s1, s2, s3;
-  float qDot1, qDot2, qDot3, qDot4;
-  float hx, hy;
-  float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
-      _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
-      q2q2, q2q3, q3q3;
-
-  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in
-  // magnetometer normalisation)
-  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-    updateIMU(gx, gy, gz, ax, ay, az, dt);
-    return;
-  }
-
-  // Convert gyroscope degrees/sec to radians/sec
-  gx *= 0.0174533f;
-  gy *= 0.0174533f;
-  gz *= 0.0174533f;
-
-  // Rate of change of quaternion from gyroscope
-  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
-  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
-  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
-  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
-
-  // Compute feedback only if accelerometer measurement valid (avoids NaN in
-  // accelerometer normalisation)
-  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-    // Normalise accelerometer measurement
-    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-    ax *= recipNorm;
-    ay *= recipNorm;
-    az *= recipNorm;
-
-    // Normalise magnetometer measurement
-    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-    mx *= recipNorm;
-    my *= recipNorm;
-    mz *= recipNorm;
-
-    // Auxiliary variables to avoid repeated arithmetic
-    _2q0mx = 2.0f * q0 * mx;
-    _2q0my = 2.0f * q0 * my;
-    _2q0mz = 2.0f * q0 * mz;
-    _2q1mx = 2.0f * q1 * mx;
-    _2q0 = 2.0f * q0;
-    _2q1 = 2.0f * q1;
-    _2q2 = 2.0f * q2;
-    _2q3 = 2.0f * q3;
-    _2q0q2 = 2.0f * q0 * q2;
-    _2q2q3 = 2.0f * q2 * q3;
-    q0q0 = q0 * q0;
-    q0q1 = q0 * q1;
-    q0q2 = q0 * q2;
-    q0q3 = q0 * q3;
-    q1q1 = q1 * q1;
-    q1q2 = q1 * q2;
-    q1q3 = q1 * q3;
-    q2q2 = q2 * q2;
-    q2q3 = q2 * q3;
-    q3q3 = q3 * q3;
-
-    // Reference direction of Earth's magnetic field
-    hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 +
-         _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
-    hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 +
-         my * q2q2 + _2q2 * mz * q3 - my * q3q3;
-    _2bx = sqrtf(hx * hx + hy * hy);
-    _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 +
-           _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
-    _4bx = 2.0f * _2bx;
-    _4bz = 2.0f * _2bz;
-
-    // Gradient decent algorithm corrective step
-    s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
-         _2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
-         _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
-         (-_2bx * q3 + _2bz * q1) *
-             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
-         _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-    s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
-         _2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
-         4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
-         _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
-         (_2bx * q2 + _2bz * q0) *
-             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
-         (_2bx * q3 - _4bz * q1) *
-             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-    s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
-         _2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
-         4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
-         (-_4bx * q2 - _2bz * q0) *
-             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
-         (_2bx * q1 + _2bz * q3) *
-             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
-         (_2bx * q0 - _4bz * q2) *
-             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-    s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
-         _2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
-         (-_4bx * q3 + _2bz * q1) *
-             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
-         (-_2bx * q0 + _2bz * q2) *
-             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
-         _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
-    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
-                        s3 * s3); // normalise step magnitude
-    s0 *= recipNorm;
-    s1 *= recipNorm;
-    s2 *= recipNorm;
-    s3 *= recipNorm;
-
-    // Apply feedback step
-    qDot1 -= beta * s0;
-    qDot2 -= beta * s1;
-    qDot3 -= beta * s2;
-    qDot4 -= beta * s3;
-  }
-
-  // Integrate rate of change of quaternion to yield quaternion
-  q0 += qDot1 * dt;
-  q1 += qDot2 * dt;
-  q2 += qDot3 * dt;
-  q3 += qDot4 * dt;
-
-  // Normalise quaternion
-  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-  q0 *= recipNorm;
-  q1 *= recipNorm;
-  q2 *= recipNorm;
-  q3 *= recipNorm;
-  anglesComputed = 0;
-}
-
-//-------------------------------------------------------------------------------------------
-// IMU algorithm update
-
-void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
-                                  float ay, float az, float dt) {
-  float recipNorm;
-  float s0, s1, s2, s3;
-  float qDot1, qDot2, qDot3, qDot4;
-  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
-      q3q3;
-
-  // Convert gyroscope degrees/sec to radians/sec
-  gx *= 0.0174533f;
-  gy *= 0.0174533f;
-  gz *= 0.0174533f;
-
-  // Rate of change of quaternion from gyroscope
-  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
-  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
-  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
-  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
-
-  // Compute feedback only if accelerometer measurement valid (avoids NaN in
-  // accelerometer normalisation)
-  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-    // Normalise accelerometer measurement
-    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-    ax *= recipNorm;
-    ay *= recipNorm;
-    az *= recipNorm;
-
-    // Auxiliary variables to avoid repeated arithmetic
-    _2q0 = 2.0f * q0;
-    _2q1 = 2.0f * q1;
-    _2q2 = 2.0f * q2;
-    _2q3 = 2.0f * q3;
-    _4q0 = 4.0f * q0;
-    _4q1 = 4.0f * q1;
-    _4q2 = 4.0f * q2;
-    _8q1 = 8.0f * q1;
-    _8q2 = 8.0f * q2;
-    q0q0 = q0 * q0;
-    q1q1 = q1 * q1;
-    q2q2 = q2 * q2;
-    q3q3 = q3 * q3;
-
-    // Gradient decent algorithm corrective step
-    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
-    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 +
-         _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
-    s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 +
-         _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
-    s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
-    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
-                        s3 * s3); // normalise step magnitude
-    s0 *= recipNorm;
-    s1 *= recipNorm;
-    s2 *= recipNorm;
-    s3 *= recipNorm;
-
-    // Apply feedback step
-    qDot1 -= beta * s0;
-    qDot2 -= beta * s1;
-    qDot3 -= beta * s2;
-    qDot4 -= beta * s3;
-  }
-
-  // Integrate rate of change of quaternion to yield quaternion
-  q0 += qDot1 * dt;
-  q1 += qDot2 * dt;
-  q2 += qDot3 * dt;
-  q3 += qDot4 * dt;
-
-  // Normalise quaternion
-  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-  q0 *= recipNorm;
-  q1 *= recipNorm;
-  q2 *= recipNorm;
-  q3 *= recipNorm;
-  anglesComputed = 0;
-}
-
-void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
-                               float az, float mx, float my, float mz) {
-  update(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq);
-}
-void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
-                                  float ay, float az) {
-  updateIMU(gx, gy, gz, ax, ay, az, invSampleFreq);
-};
-
-//-------------------------------------------------------------------------------------------
-// Fast inverse square-root
-// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-
-float Adafruit_Madgwick::invSqrt(float x) {
-  float halfx = 0.5f * x;
-  union {
-    float f;
-    long i;
-  } conv = {x};
-  conv.i = 0x5f3759df - (conv.i >> 1);
-  conv.f *= 1.5f - (halfx * conv.f * conv.f);
-  conv.f *= 1.5f - (halfx * conv.f * conv.f);
-  return conv.f;
-}
-
-//-------------------------------------------------------------------------------------------
-
-void Adafruit_Madgwick::computeAngles() {
-  roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
-  pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
-  yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
-  grav[0] = 2.0f * (q1 * q3 - q0 * q2);
-  grav[1] = 2.0f * (q0 * q1 + q2 * q3);
-  grav[2] = 2.0f * (q0 * q0 - 0.5f + q3 * q3);
-  anglesComputed = 1;
-}

src/Adafruit_AHRS_Madgwick.h

@@ -1,108 +0,0 @@
-//=============================================================================================
-// Madgwick.h
-//=============================================================================================
-//
-// Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-//
-// From the x-io website "Open-source resources available on this website are
-// provided under the GNU General Public Licence unless an alternative licence
-// is provided in source."
-//
-// Date			Author          Notes
-// 29/09/2011	SOH Madgwick    Initial release
-// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
-//
-//=============================================================================================
-#ifndef __Adafruit_Madgwick_h__
-#define __Adafruit_Madgwick_h__
-
-#include "Adafruit_AHRS_FusionInterface.h"
-#include <math.h>
-
-//--------------------------------------------------------------------------------------------
-// Variable declaration
-class Adafruit_Madgwick : public Adafruit_AHRS_FusionInterface {
-private:
-  static float invSqrt(float x);
-  float beta; // algorithm gain
-  float q0;
-  float q1;
-  float q2;
-  float q3; // quaternion of sensor frame relative to auxiliary frame
-  float invSampleFreq;
-  float roll, pitch, yaw;
-  float grav[3];
-  bool anglesComputed = false;
-  void computeAngles();
-
-  //-------------------------------------------------------------------------------------------
-  // Function declarations
-public:
-  Adafruit_Madgwick();
-  Adafruit_Madgwick(float gain);
-  void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
-  void update(float gx, float gy, float gz, float ax, float ay, float az,
-              float mx, float my, float mz);
-  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
-  void update(float gx, float gy, float gz, float ax, float ay, float az,
-              float mx, float my, float mz, float dt);
-  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az,
-                 float dt);
-  // float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 *
-  // q0 + 2.0f * q3 * q3 - 1.0f);}; float getRoll(){return -1.0f * asinf(2.0f *
-  // q1 * q3 + 2.0f * q0 * q2);}; float getYaw(){return atan2f(2.0f * q1 * q2
-  // - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
-  float getBeta() { return beta; }
-  void setBeta(float beta) { this->beta = beta; }
-  float getRoll() {
-    if (!anglesComputed)
-      computeAngles();
-    return roll * 57.29578f;
-  }
-  float getPitch() {
-    if (!anglesComputed)
-      computeAngles();
-    return pitch * 57.29578f;
-  }
-  float getYaw() {
-    if (!anglesComputed)
-      computeAngles();
-    return yaw * 57.29578f + 180.0f;
-  }
-  float getRollRadians() {
-    if (!anglesComputed)
-      computeAngles();
-    return roll;
-  }
-  float getPitchRadians() {
-    if (!anglesComputed)
-      computeAngles();
-    return pitch;
-  }
-  float getYawRadians() {
-    if (!anglesComputed)
-      computeAngles();
-    return yaw;
-  }
-  void getQuaternion(float *w, float *x, float *y, float *z) {
-    *w = q0;
-    *x = q1;
-    *y = q2;
-    *z = q3;
-  }
-  void setQuaternion(float w, float x, float y, float z) {
-    q0 = w;
-    q1 = x;
-    q2 = y;
-    q3 = z;
-  }
-  void getGravityVector(float *x, float *y, float *z) {
-    if (!anglesComputed)
-      computeAngles();
-    *x = grav[0];
-    *y = grav[1];
-    *z = grav[2];
-  }
-};
-#endif

src/Adafruit_AHRS_Mahony.cpp

@@ -1,288 +0,0 @@
-//=============================================================================================
-// Adafruit_Mahony.c
-//=============================================================================================
-//
-// Madgwick's implementation of Mayhony's AHRS algorithm.
-// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-//
-// From the x-io website "Open-source resources available on this website are
-// provided under the GNU General Public Licence unless an alternative licence
-// is provided in source."
-//
-// Date			Author			Notes
-// 29/09/2011	SOH Madgwick    Initial release
-// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
-//
-// Algorithm paper:
-// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
-//
-//=============================================================================================
-
-//-------------------------------------------------------------------------------------------
-// Header files
-
-#include "Adafruit_AHRS_Mahony.h"
-#include <math.h>
-
-//-------------------------------------------------------------------------------------------
-// Definitions
-
-#define DEFAULT_SAMPLE_FREQ 512.0f // sample frequency in Hz
-#define twoKpDef (2.0f * 0.5f)     // 2 * proportional gain
-#define twoKiDef (2.0f * 0.0f)     // 2 * integral gain
-
-//============================================================================================
-// Functions
-
-//-------------------------------------------------------------------------------------------
-// AHRS algorithm update
-
-Adafruit_Mahony::Adafruit_Mahony() : Adafruit_Mahony(twoKpDef, twoKiDef) {}
-
-Adafruit_Mahony::Adafruit_Mahony(float prop_gain, float int_gain) {
-  twoKp = prop_gain; // 2 * proportional gain (Kp)
-  twoKi = int_gain;  // 2 * integral gain (Ki)
-  q0 = 1.0f;
-  q1 = 0.0f;
-  q2 = 0.0f;
-  q3 = 0.0f;
-  integralFBx = 0.0f;
-  integralFBy = 0.0f;
-  integralFBz = 0.0f;
-  anglesComputed = false;
-  invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
-}
-
-void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
-                             float az, float mx, float my, float mz, float dt) {
-  float recipNorm;
-  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
-  float hx, hy, bx, bz;
-  float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
-  float halfex, halfey, halfez;
-  float qa, qb, qc;
-
-  // Use IMU algorithm if magnetometer measurement invalid
-  // (avoids NaN in magnetometer normalisation)
-  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-    updateIMU(gx, gy, gz, ax, ay, az);
-    return;
-  }
-
-  // Convert gyroscope degrees/sec to radians/sec
-  gx *= 0.0174533f;
-  gy *= 0.0174533f;
-  gz *= 0.0174533f;
-
-  // Compute feedback only if accelerometer measurement valid
-  // (avoids NaN in accelerometer normalisation)
-  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-    // Normalise accelerometer measurement
-    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-    ax *= recipNorm;
-    ay *= recipNorm;
-    az *= recipNorm;
-
-    // Normalise magnetometer measurement
-    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-    mx *= recipNorm;
-    my *= recipNorm;
-    mz *= recipNorm;
-
-    // Auxiliary variables to avoid repeated arithmetic
-    q0q0 = q0 * q0;
-    q0q1 = q0 * q1;
-    q0q2 = q0 * q2;
-    q0q3 = q0 * q3;
-    q1q1 = q1 * q1;
-    q1q2 = q1 * q2;
-    q1q3 = q1 * q3;
-    q2q2 = q2 * q2;
-    q2q3 = q2 * q3;
-    q3q3 = q3 * q3;
-
-    // Reference direction of Earth's magnetic field
-    hx = 2.0f *
-         (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
-    hy = 2.0f *
-         (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
-    bx = sqrtf(hx * hx + hy * hy);
-    bz = 2.0f *
-         (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
-
-    // Estimated direction of gravity and magnetic field
-    halfvx = q1q3 - q0q2;
-    halfvy = q0q1 + q2q3;
-    halfvz = q0q0 - 0.5f + q3q3;
-    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
-    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
-    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
-
-    // Error is sum of cross product between estimated direction
-    // and measured direction of field vectors
-    halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
-    halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
-    halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
-
-    // Compute and apply integral feedback if enabled
-    if (twoKi > 0.0f) {
-      // integral error scaled by Ki
-      integralFBx += twoKi * halfex * dt;
-      integralFBy += twoKi * halfey * dt;
-      integralFBz += twoKi * halfez * dt;
-      gx += integralFBx; // apply integral feedback
-      gy += integralFBy;
-      gz += integralFBz;
-    } else {
-      integralFBx = 0.0f; // prevent integral windup
-      integralFBy = 0.0f;
-      integralFBz = 0.0f;
-    }
-
-    // Apply proportional feedback
-    gx += twoKp * halfex;
-    gy += twoKp * halfey;
-    gz += twoKp * halfez;
-  }
-
-  // Integrate rate of change of quaternion
-  gx *= (0.5f * dt); // pre-multiply common factors
-  gy *= (0.5f * dt);
-  gz *= (0.5f * dt);
-  qa = q0;
-  qb = q1;
-  qc = q2;
-  q0 += (-qb * gx - qc * gy - q3 * gz);
-  q1 += (qa * gx + qc * gz - q3 * gy);
-  q2 += (qa * gy - qb * gz + q3 * gx);
-  q3 += (qa * gz + qb * gy - qc * gx);
-
-  // Normalise quaternion
-  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-  q0 *= recipNorm;
-  q1 *= recipNorm;
-  q2 *= recipNorm;
-  q3 *= recipNorm;
-  anglesComputed = 0;
-}
-
-//-------------------------------------------------------------------------------------------
-// IMU algorithm update
-
-void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
-                                float ay, float az, float dt) {
-  float recipNorm;
-  float halfvx, halfvy, halfvz;
-  float halfex, halfey, halfez;
-  float qa, qb, qc;
-
-  // Convert gyroscope degrees/sec to radians/sec
-  gx *= 0.0174533f;
-  gy *= 0.0174533f;
-  gz *= 0.0174533f;
-
-  // Compute feedback only if accelerometer measurement valid
-  // (avoids NaN in accelerometer normalisation)
-  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-    // Normalise accelerometer measurement
-    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-    ax *= recipNorm;
-    ay *= recipNorm;
-    az *= recipNorm;
-
-    // Estimated direction of gravity
-    halfvx = q1 * q3 - q0 * q2;
-    halfvy = q0 * q1 + q2 * q3;
-    halfvz = q0 * q0 - 0.5f + q3 * q3;
-
-    // Error is sum of cross product between estimated
-    // and measured direction of gravity
-    halfex = (ay * halfvz - az * halfvy);
-    halfey = (az * halfvx - ax * halfvz);
-    halfez = (ax * halfvy - ay * halfvx);
-
-    // Compute and apply integral feedback if enabled
-    if (twoKi > 0.0f) {
-      // integral error scaled by Ki
-      integralFBx += twoKi * halfex * dt;
-      integralFBy += twoKi * halfey * dt;
-      integralFBz += twoKi * halfez * dt;
-      gx += integralFBx; // apply integral feedback
-      gy += integralFBy;
-      gz += integralFBz;
-    } else {
-      integralFBx = 0.0f; // prevent integral windup
-      integralFBy = 0.0f;
-      integralFBz = 0.0f;
-    }
-
-    // Apply proportional feedback
-    gx += twoKp * halfex;
-    gy += twoKp * halfey;
-    gz += twoKp * halfez;
-  }
-
-  // Integrate rate of change of quaternion
-  gx *= (0.5f * dt); // pre-multiply common factors
-  gy *= (0.5f * dt);
-  gz *= (0.5f * dt);
-  qa = q0;
-  qb = q1;
-  qc = q2;
-  q0 += (-qb * gx - qc * gy - q3 * gz);
-  q1 += (qa * gx + qc * gz - q3 * gy);
-  q2 += (qa * gy - qb * gz + q3 * gx);
-  q3 += (qa * gz + qb * gy - qc * gx);
-
-  // Normalise quaternion
-  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-  q0 *= recipNorm;
-  q1 *= recipNorm;
-  q2 *= recipNorm;
-  q3 *= recipNorm;
-  anglesComputed = 0;
-}
-
-void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
-                             float az, float mx, float my, float mz) {
-  update(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq);
-}
-
-void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
-                                float ay, float az) {
-  updateIMU(gx, gy, gz, ax, ay, az, invSampleFreq);
-};
-
-//-------------------------------------------------------------------------------------------
-// Fast inverse square-root
-// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-
-float Adafruit_Mahony::invSqrt(float x) {
-  float halfx = 0.5f * x;
-  union {
-    float f;
-    long i;
-  } conv = {x};
-  conv.i = 0x5f3759df - (conv.i >> 1);
-  conv.f *= 1.5f - (halfx * conv.f * conv.f);
-  conv.f *= 1.5f - (halfx * conv.f * conv.f);
-  return conv.f;
-}
-
-//-------------------------------------------------------------------------------------------
-
-void Adafruit_Mahony::computeAngles() {
-  roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
-  pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
-  yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
-  grav[0] = 2.0f * (q1 * q3 - q0 * q2);
-  grav[1] = 2.0f * (q0 * q1 + q2 * q3);
-  grav[2] = 2.0f * (q0 * q0 - 0.5f + q3 * q3);
-  anglesComputed = 1;
-}
-
-//============================================================================================
-// END OF CODE
-//============================================================================================

src/Adafruit_AHRS_Mahony.h

@@ -1,106 +0,0 @@
-//=============================================================================================
-// Adafruit_AHRS_Mahony.h
-//=============================================================================================
-//
-// Madgwick's implementation of Mayhony's AHRS algorithm.
-// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-//
-// Date			Author			Notes
-// 29/09/2011	SOH Madgwick    Initial release
-// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
-//
-//=============================================================================================
-#ifndef __Adafruit_Mahony_h__
-#define __Adafruit_Mahony_h__
-
-#include "Adafruit_AHRS_FusionInterface.h"
-#include <math.h>
-
-//--------------------------------------------------------------------------------------------
-// Variable declaration
-
-class Adafruit_Mahony : public Adafruit_AHRS_FusionInterface {
-private:
-  float twoKp; // 2 * proportional gain (Kp)
-  float twoKi; // 2 * integral gain (Ki)
-  float q0, q1, q2,
-      q3; // quaternion of sensor frame relative to auxiliary frame
-  float integralFBx, integralFBy,
-      integralFBz; // integral error terms scaled by Ki
-  float invSampleFreq;
-  float roll, pitch, yaw;
-  float grav[3];
-  bool anglesComputed = false;
-  static float invSqrt(float x);
-  void computeAngles();
-
-  //-------------------------------------------------------------------------------------------
-  // Function declarations
-
-public:
-  Adafruit_Mahony();
-  Adafruit_Mahony(float prop_gain, float int_gain);
-  void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
-  void update(float gx, float gy, float gz, float ax, float ay, float az,
-              float mx, float my, float mz);
-  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
-  void update(float gx, float gy, float gz, float ax, float ay, float az,
-              float mx, float my, float mz, float dt);
-  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az,
-                 float dt);
-  float getKp() { return twoKp / 2.0f; }
-  void setKp(float Kp) { twoKp = 2.0f * Kp; }
-  float getKi() { return twoKi / 2.0f; }
-  void setKi(float Ki) { twoKi = 2.0f * Ki; }
-  float getRoll() {
-    if (!anglesComputed)
-      computeAngles();
-    return roll * 57.29578f;
-  }
-  float getPitch() {
-    if (!anglesComputed)
-      computeAngles();
-    return pitch * 57.29578f;
-  }
-  float getYaw() {
-    if (!anglesComputed)
-      computeAngles();
-    return yaw * 57.29578f + 180.0f;
-  }
-  float getRollRadians() {
-    if (!anglesComputed)
-      computeAngles();
-    return roll;
-  }
-  float getPitchRadians() {
-    if (!anglesComputed)
-      computeAngles();
-    return pitch;
-  }
-  float getYawRadians() {
-    if (!anglesComputed)
-      computeAngles();
-    return yaw;
-  }
-  void getQuaternion(float *w, float *x, float *y, float *z) {
-    *w = q0;
-    *x = q1;
-    *y = q2;
-    *z = q3;
-  }
-  void setQuaternion(float w, float x, float y, float z) {
-    q0 = w;
-    q1 = x;
-    q2 = y;
-    q3 = z;
-  }
-  void getGravityVector(float *x, float *y, float *z) {
-    if (!anglesComputed)
-      computeAngles();
-    *x = grav[0];
-    *y = grav[1];
-    *z = grav[2];
-  }
-};
-
-#endif

src/Adafruit_AHRS_NXPFusion.h

@@ -1,217 +0,0 @@
-/*!
- * @file Adafruit_AHRS_NXPFusion.h
- *
- * @section license License
- *
- * This is a modification of
- * https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/blob/master/Sources/tasks.h
- * by PJRC / Paul Stoffregen https://github.com/PaulStoffregen/NXPMotionSense
- *
- * Copyright (c) 2014, Freescale Semiconductor, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of Freescale Semiconductor, Inc. nor the
- *       names of its contributors may be used to endorse or promote products
- *       derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
- * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __Adafruit_Nxp_Fusion_h_
-#define __Adafruit_Nxp_Fusion_h_
-
-#include "Adafruit_AHRS_FusionInterface.h"
-#include <Arduino.h>
-
-/*!
- * @brief Kalman/NXP Fusion algorithm.
- */
-class Adafruit_NXPSensorFusion : public Adafruit_AHRS_FusionInterface {
-public:
-  /**************************************************************************/
-  /*!
-   * @brief Initializes the 9DOF Kalman filter.
-   *
-   * @param sampleFrequency The sensor sample rate in herz(samples per second).
-   */
-  /**************************************************************************/
-  void begin(float sampleFrequency = 100.0f);
-
-  /**************************************************************************/
-  /*!
-   * @brief Updates the filter with new gyroscope, accelerometer, and
-   * magnetometer data. For roll, pitch, and yaw the accelerometer values can be
-   * either m/s^2 or g, but for linear acceleration they have to be in g.
-   *
-   * 9DOF orientation function implemented using a 12 element Kalman filter
-   *
-   * void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV,
-   * const AccelSensor_t *Accel, const MagSensor_t *Mag,
-   * const GyroSensor_t *Gyro, const MagCalibration_t *MagCal)
-   *
-   * @param gx The gyroscope x axis. In DPS.
-   * @param gy The gyroscope y axis. In DPS.
-   * @param gz The gyroscope z axis. In DPS.
-   * @param ax The accelerometer x axis. In g.
-   * @param ay The accelerometer y axis. In g.
-   * @param az The accelerometer z axis. In g.
-   * @param mx The magnetometer x axis. In uT.
-   * @param my The magnetometer y axis. In uT.
-   * @param mz The magnetometer z axis. In uT.
-   */
-  /**************************************************************************/
-  void update(float gx, float gy, float gz, float ax, float ay, float az,
-              float mx, float my, float mz);
-
-  float getRoll() { return PhiPl; }
-  float getPitch() { return ThePl; }
-  float getYaw() { return PsiPl; }
-
-  void getQuaternion(float *w, float *x, float *y, float *z) {
-    *w = qPl.q0;
-    *x = qPl.q1;
-    *y = qPl.q2;
-    *z = qPl.q3;
-  }
-
-  void setQuaternion(float w, float x, float y, float z) {
-    qPl.q0 = w;
-    qPl.q1 = x;
-    qPl.q2 = y;
-    qPl.q3 = z;
-  }
-  /**************************************************************************/
-  /*!
-   * @brief Get the linear acceleration part of the acceleration value given to
-   * update.
-   *
-   * @param x The pointer to write the linear acceleration x axis to. In g.
-   * @param y The pointer to write the linear acceleration y axis to. In g.
-   * @param z The pointer to write the linear acceleration z axis to. In g.
-   */
-  /**************************************************************************/
-  void getLinearAcceleration(float *x, float *y, float *z) const {
-    *x = aSePl[0];
-    *y = aSePl[1];
-    *z = aSePl[2];
-  }
-
-  /**************************************************************************/
-  /*!
-   * @brief Get the gravity vector from the gyroscope values.
-   *
-   * @param x A float pointer to write the gravity vector x component to. In g.
-   * @param y A float pointer to write the gravity vector y component to. In g.
-   * @param z A float pointer to write the gravity vector z component to. In g.
-   */
-  /**************************************************************************/
-  void getGravityVector(float *x, float *y, float *z) {
-    *x = gSeGyMi[0];
-    *y = gSeGyMi[1];
-    *z = gSeGyMi[2];
-  }
-
-  /**************************************************************************/
-  /*!
-   * @brief Get the geomagnetic vector in global frame.
-   *
-   * @param x The pointer to write the geomagnetic vector x axis to. In uT.
-   * @param y The pointer to write the geomagnetic vector y axis to. In uT.
-   * @param z The pointer to write the geomagnetic vector z axis to. In uT.
-   */
-  /**************************************************************************/
-  void getGeomagneticVector(float *x, float *y, float *z) const {
-    *x = mGl[0];
-    *y = mGl[1];
-    *z = mGl[2];
-  }
-
-  typedef struct {
-    float q0; // w
-    float q1; // x
-    float q2; // y
-    float q3; // z
-  } Quaternion_t;
-
-  // These are Madgwick & Mahony - extrinsic rotation reference (wrong!)
-  // float getPitch() {return atan2f(2.0f * qPl.q2 * qPl.q3 - 2.0f * qPl.q0 *
-  // qPl.q1, 2.0f * qPl.q0 * qPl.q0 + 2.0f * qPl.q3 * qPl.q3 - 1.0f);}; float
-  // getRoll() {return -1.0f * asinf(2.0f * qPl.q1 * qPl.q3 + 2.0f * qPl.q0 *
-  // qPl.q2);}; float getYaw() {return atan2f(2.0f * qPl.q1 * qPl.q2 - 2.0f *
-  // qPl.q0 * qPl.q3, 2.0f * qPl.q0 * qPl.q0 + 2.0f * qPl.q1 * qPl.q1 - 1.0f);};
-
-private:
-  float PhiPl; // roll (deg)
-  float ThePl; // pitch (deg)
-  float PsiPl; // yaw (deg)
-  float RhoPl; // compass (deg)
-  float ChiPl; // tilt from vertical (deg)
-  // orientation matrix, quaternion and rotation vector
-  float RPl[3][3];  // a posteriori orientation matrix
-  Quaternion_t qPl; // a posteriori orientation quaternion
-  float RVecPl[3];  // rotation vector
-  // angular velocity
-  float Omega[3]; // angular velocity (deg/s)
-  // systick timer for benchmarking
-  int32_t systick; // systick timer;
-  // end: elements common to all motion state vectors
-
-  // elements transmitted over bluetooth in kalman packet
-  float bPl[3];     // gyro offset (deg/s)
-  float ThErrPl[3]; // orientation error (deg)
-  float bErrPl[3];  // gyro offset error (deg/s)
-  // end elements transmitted in kalman packet
-
-  float dErrGlPl[3]; // magnetic disturbance error (uT, global frame)
-  float dErrSePl[3]; // magnetic disturbance error (uT, sensor frame)
-  float aErrSePl[3]; // linear acceleration error (g, sensor frame)
-  float aSeMi[3];    // linear acceleration (g, sensor frame)
-  float DeltaPl;     // inclination angle (deg)
-  float aSePl[3];    // linear acceleration (g, sensor frame)
-  float aGlPl[3];    // linear acceleration (g, global frame)
-  float gErrSeMi[3]; // difference (g, sensor frame) of gravity vector (accel)
-                     // and gravity vector (gyro)
-  float mErrSeMi[3]; // difference (uT, sensor frame) of geomagnetic vector
-                     // (magnetometer) and geomagnetic vector (gyro)
-  float gSeGyMi[3];  // gravity vector (g, sensor frame) measurement from gyro
-  float
-      mSeGyMi[3]; // geomagnetic vector (uT, sensor frame) measurement from gyro
-  float mGl[3];   // geomagnetic vector (uT, global frame)
-  float QvAA;     // accelerometer terms of Qv
-  float QvMM;     // magnetometer terms of Qv
-  float PPlus12x12[12][12]; // covariance matrix P+
-  float K12x6[12][6];       // kalman filter gain matrix K
-  float Qw12x12[12][12];    // covariance matrix Qw
-  float C6x12[6][12];       // measurement matrix C
-  float RMi[3][3];          // a priori orientation matrix
-  Quaternion_t Deltaq;      // delta quaternion
-  Quaternion_t qMi;         // a priori orientation quaternion
-  float casq;               // FCA * FCA;
-  float cdsq;               // FCD * FCD;
-  float Fastdeltat;         // sensor sampling interval (s) = 1 / SENSORFS
-  float deltat;     // kalman filter sampling interval (s) = OVERSAMPLE_RATIO /
-                    // SENSORFS
-  float deltatsq;   // fdeltat * fdeltat
-  float QwbplusQvG; // FQWB + FQVG
-  int8_t
-      FirstOrientationLock; // denotes that 9DOF orientation has locked to 6DOF
-  int8_t resetflag;         // flag to request re-initialization on next pass
-};
-
-#endif

src/Adafruit_AHRS_NXPmatrix.c

@@ -1,467 +0,0 @@
-// Copyright (c) 2014, Freescale Semiconductor, Inc.
-// All rights reserved.
-// vim: set ts=4:
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//     * Redistributions of source code must retain the above copyright
-//       notice, this list of conditions and the following disclaimer.
-//     * Redistributions in binary form must reproduce the above copyright
-//       notice, this list of conditions and the following disclaimer in the
-//       documentation and/or other materials provided with the distribution.
-//     * Neither the name of Freescale Semiconductor, Inc. nor the
-//       names of its contributors may be used to endorse or promote products
-//       derived from this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR
-// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-//
-// This file contains matrix manipulation functions.
-//
-
-#include <inttypes.h>
-#include <math.h>
-
-// compile time constants that are private to this file
-#define CORRUPTMATRIX 0.001F // column vector modulus limit for rotation matrix
-
-// vector components
-#define X 0
-#define Y 1
-#define Z 2
-
-// function sets the 3x3 matrix A to the identity matrix
-void f3x3matrixAeqI(float A[][3]) {
-  float *pAij; // pointer to A[i][j]
-  int8_t i, j; // loop counters
-
-  for (i = 0; i < 3; i++) {
-    // set pAij to &A[i][j=0]
-    pAij = A[i];
-    for (j = 0; j < 3; j++) {
-      *(pAij++) = 0.0F;
-    }
-    A[i][i] = 1.0F;
-  }
-}
-
-// function sets the matrix A to the identity matrix
-void fmatrixAeqI(float *A[], int16_t rc) {
-  // rc = rows and columns in A
-
-  float *pAij; // pointer to A[i][j]
-  int8_t i, j; // loop counters
-
-  for (i = 0; i < rc; i++) {
-    // set pAij to &A[i][j=0]
-    pAij = A[i];
-    for (j = 0; j < rc; j++) {
-      *(pAij++) = 0.0F;
-    }
-    A[i][i] = 1.0F;
-  }
-}
-
-// function sets every entry in the 3x3 matrix A to a constant scalar
-void f3x3matrixAeqScalar(float A[][3], float Scalar) {
-  float *pAij; // pointer to A[i][j]
-  int8_t i, j; // counters
-
-  for (i = 0; i < 3; i++) {
-    // set pAij to &A[i][j=0]
-    pAij = A[i];
-    for (j = 0; j < 3; j++) {
-      *(pAij++) = Scalar;
-    }
-  }
-}
-
-// function multiplies all elements of 3x3 matrix A by the specified scalar
-void f3x3matrixAeqAxScalar(float A[][3], float Scalar) {
-  float *pAij; // pointer to A[i][j]
-  int8_t i, j; // loop counters
-
-  for (i = 0; i < 3; i++) {
-    // set pAij to &A[i][j=0]
-    pAij = A[i];
-    for (j = 0; j < 3; j++) {
-      *(pAij++) *= Scalar;
-    }
-  }
-}
-
-// function negates all elements of 3x3 matrix A
-void f3x3matrixAeqMinusA(float A[][3]) {
-  float *pAij; // pointer to A[i][j]
-  int8_t i, j; // loop counters
-
-  for (i = 0; i < 3; i++) {
-    // set pAij to &A[i][j=0]
-    pAij = A[i];
-    for (j = 0; j < 3; j++) {
-      *pAij = -*pAij;
-      pAij++;
-    }
-  }
-}
-
-// function directly calculates the symmetric inverse of a symmetric 3x3 matrix
-// only the on and above diagonal terms in B are used and need to be specified
-void f3x3matrixAeqInvSymB(float A[][3], float B[][3]) {
-  float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2]
-  float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2]
-  float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2]
-  float ftmp;           // determinant and then reciprocal
-
-  // calculate useful products
-  fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
-  fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
-  fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
-
-  // set ftmp to the determinant of the input matrix B
-  ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 +
-         B[0][2] * fB01B12mB11B02;
-
-  // set A to the inverse of B for any determinant except zero
-  if (ftmp != 0.0F) {
-    ftmp = 1.0F / ftmp;
-    A[0][0] = fB11B22mB12B12 * ftmp;
-    A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
-    A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
-    A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
-    A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
-    A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
-  } else {
-    // provide the identity matrix if the determinant is zero
-    f3x3matrixAeqI(A);
-  }
-}
-
-// function calculates the determinant of a 3x3 matrix
-float f3x3matrixDetA(float A[][3]) {
-  return (A[X][X] * (A[Y][Y] * A[Z][Z] - A[Y][Z] * A[Z][Y]) +
-          A[X][Y] * (A[Y][Z] * A[Z][X] - A[Y][X] * A[Z][Z]) +
-          A[X][Z] * (A[Y][X] * A[Z][Y] - A[Y][Y] * A[Z][X]));
-}
-
-// function computes all eigenvalues and eigenvectors of a real symmetric matrix
-// A[0..n-1][0..n-1] stored in the top left of a 10x10 array A[10][10] A[][] is
-// changed on output. eigval[0..n-1] returns the eigenvalues of A[][].
-// eigvec[0..n-1][0..n-1] returns the normalized eigenvectors of A[][]
-// the eigenvectors are not sorted by value
-void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n) {
-  // maximum number of iterations to achieve convergence: in practice 6 is
-  // typical
-#define NITERATIONS 15
-
-  // various trig functions of the jacobi rotation angle phi
-  float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
-  // scratch variable to prevent over-writing during rotations
-  float ftmp;
-  // residue from remaining non-zero above diagonal terms
-  float residue;
-  // matrix row and column indices
-  int8_t ir, ic;
-  // general loop counter
-  int8_t j;
-  // timeout ctr for number of passes of the algorithm
-  int8_t ctr;
-
-  // initialize eigenvectors matrix and eigenvalues array
-  for (ir = 0; ir < n; ir++) {
-    // loop over all columns
-    for (ic = 0; ic < n; ic++) {
-      // set on diagonal and off-diagonal elements to zero
-      eigvec[ir][ic] = 0.0F;
-    }
-
-    // correct the diagonal elements to 1.0
-    eigvec[ir][ir] = 1.0F;
-
-    // initialize the array of eigenvalues to the diagonal elements of m
-    eigval[ir] = A[ir][ir];
-  }
-
-  // initialize the counter and loop until converged or NITERATIONS reached
-  ctr = 0;
-  do {
-    // compute the absolute value of the above diagonal elements as exit
-    // criterion
-    residue = 0.0F;
-    // loop over rows excluding last row
-    for (ir = 0; ir < n - 1; ir++) {
-      // loop over above diagonal columns
-      for (ic = ir + 1; ic < n; ic++) {
-        // accumulate the residual off diagonal terms which are being driven to
-        // zero
-        residue += fabsf(A[ir][ic]);
-      }
-    }
-
-    // check if we still have work to do
-    if (residue > 0.0F) {
-      // loop over all rows with the exception of the last row (since only
-      // rotating above diagonal elements)
-      for (ir = 0; ir < n - 1; ir++) {
-        // loop over columns ic (where ic is always greater than ir since above
-        // diagonal)
-        for (ic = ir + 1; ic < n; ic++) {
-          // only continue with this element if the element is non-zero
-          if (fabsf(A[ir][ic]) > 0.0F) {
-            // calculate cot(2*phi) where phi is the Jacobi rotation angle
-            cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
-
-            // calculate tan(phi) correcting sign to ensure the smaller solution
-            // is used
-            tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
-            if (cot2phi < 0.0F) {
-              tanphi = -tanphi;
-            }
-
-            // calculate the sine and cosine of the Jacobi rotation angle phi
-            cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
-            sinphi = tanphi * cosphi;
-
-            // calculate tan(phi/2)
-            tanhalfphi = sinphi / (1.0F + cosphi);
-
-            // set tmp = tan(phi) times current matrix element used in update of
-            // leading diagonal elements
-            ftmp = tanphi * A[ir][ic];
-
-            // apply the jacobi rotation to diagonal elements [ir][ir] and
-            // [ic][ic] stored in the eigenvalue array eigval[ir] = eigval[ir] -
-            // tan(phi) *  A[ir][ic]
-            eigval[ir] -= ftmp;
-            // eigval[ic] = eigval[ic] + tan(phi) * A[ir][ic]
-            eigval[ic] += ftmp;
-
-            // by definition, applying the jacobi rotation on element ir, ic
-            // results in 0.0
-            A[ir][ic] = 0.0F;
-
-            // apply the jacobi rotation to all elements of the eigenvector
-            // matrix
-            for (j = 0; j < n; j++) {
-              // store eigvec[j][ir]
-              ftmp = eigvec[j][ir];
-              // eigvec[j][ir] = eigvec[j][ir] - sin(phi) * (eigvec[j][ic] +
-              // tan(phi/2) * eigvec[j][ir])
-              eigvec[j][ir] =
-                  ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
-              // eigvec[j][ic] = eigvec[j][ic] + sin(phi) * (eigvec[j][ir] -
-              // tan(phi/2) * eigvec[j][ic])
-              eigvec[j][ic] =
-                  eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
-            }
-
-            // apply the jacobi rotation only to those elements of matrix m that
-            // can change
-            for (j = 0; j <= ir - 1; j++) {
-              // store A[j][ir]
-              ftmp = A[j][ir];
-              // A[j][ir] = A[j][ir] - sin(phi) * (A[j][ic] + tan(phi/2) *
-              // A[j][ir])
-              A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
-              // A[j][ic] = A[j][ic] + sin(phi) * (A[j][ir] - tan(phi/2) *
-              // A[j][ic])
-              A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
-            }
-            for (j = ir + 1; j <= ic - 1; j++) {
-              // store A[ir][j]
-              ftmp = A[ir][j];
-              // A[ir][j] = A[ir][j] - sin(phi) * (A[j][ic] + tan(phi/2) *
-              // A[ir][j])
-              A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
-              // A[j][ic] = A[j][ic] + sin(phi) * (A[ir][j] - tan(phi/2) *
-              // A[j][ic])
-              A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
-            }
-            for (j = ic + 1; j < n; j++) {
-              // store A[ir][j]
-              ftmp = A[ir][j];
-              // A[ir][j] = A[ir][j] - sin(phi) * (A[ic][j] + tan(phi/2) *
-              // A[ir][j])
-              A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
-              // A[ic][j] = A[ic][j] + sin(phi) * (A[ir][j] - tan(phi/2) *
-              // A[ic][j])
-              A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
-            }
-          } // end of test for matrix element already zero
-        } // end of loop over columns
-      } // end of loop over rows
-    } // end of test for non-zero residue
-  } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
-}
-
-// function uses Gauss-Jordan elimination to compute the inverse of matrix A in
-// situ on exit, A is replaced with its inverse
-void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[],
-                    int8_t iPivot[], int8_t isize) {
-  float largest;               // largest element used for pivoting
-  float scaling;               // scaling factor in pivoting
-  float recippiv;              // reciprocal of pivot element
-  float ftmp;                  // temporary variable used in swaps
-  int8_t i, j, k, l, m;        // index counters
-  int8_t iPivotRow, iPivotCol; // row and column of pivot element
-
-  // to avoid compiler warnings
-  iPivotRow = iPivotCol = 0;
-
-  // initialize the pivot array to 0
-  for (j = 0; j < isize; j++) {
-    iPivot[j] = 0;
-  }
-
-  // main loop i over the dimensions of the square matrix A
-  for (i = 0; i < isize; i++) {
-    // zero the largest element found for pivoting
-    largest = 0.0F;
-    // loop over candidate rows j
-    for (j = 0; j < isize; j++) {
-      // check if row j has been previously pivoted
-      if (iPivot[j] != 1) {
-        // loop over candidate columns k
-        for (k = 0; k < isize; k++) {
-          // check if column k has previously been pivoted
-          if (iPivot[k] == 0) {
-            // check if the pivot element is the largest found so far
-            if (fabsf(A[j][k]) >= largest) {
-              // and store this location as the current best candidate for
-              // pivoting
-              iPivotRow = j;
-              iPivotCol = k;
-              largest = (float)fabsf(A[iPivotRow][iPivotCol]);
-            }
-          } else if (iPivot[k] > 1) {
-            // zero determinant situation: exit with identity matrix
-            fmatrixAeqI(A, isize);
-            return;
-          }
-        }
-      }
-    }
-    // increment the entry in iPivot to denote it has been selected for pivoting
-    iPivot[iPivotCol]++;
-
-    // check the pivot rows iPivotRow and iPivotCol are not the same before
-    // swapping
-    if (iPivotRow != iPivotCol) {
-      // loop over columns l
-      for (l = 0; l < isize; l++) {
-        // and swap all elements of rows iPivotRow and iPivotCol
-        ftmp = A[iPivotRow][l];
-        A[iPivotRow][l] = A[iPivotCol][l];
-        A[iPivotCol][l] = ftmp;
-      }
-    }
-
-    // record that on the i-th iteration rows iPivotRow and iPivotCol were
-    // swapped
-    iRowInd[i] = iPivotRow;
-    iColInd[i] = iPivotCol;
-
-    // check for zero on-diagonal element (singular matrix) and return with
-    // identity matrix if detected
-    if (A[iPivotCol][iPivotCol] == 0.0F) {
-      // zero determinant situation: exit with identity matrix
-      fmatrixAeqI(A, isize);
-      return;
-    }
-
-    // calculate the reciprocal of the pivot element knowing it's non-zero
-    recippiv = 1.0F / A[iPivotCol][iPivotCol];
-    // by definition, the diagonal element normalizes to 1
-    A[iPivotCol][iPivotCol] = 1.0F;
-    // multiply all of row iPivotCol by the reciprocal of the pivot element
-    // including the diagonal element the diagonal element
-    // A[iPivotCol][iPivotCol] now has value equal to the reciprocal of its
-    // previous value
-    for (l = 0; l < isize; l++) {
-      A[iPivotCol][l] *= recippiv;
-    }
-    // loop over all rows m of A
-    for (m = 0; m < isize; m++) {
-      if (m != iPivotCol) {
-        // scaling factor for this row m is in column iPivotCol
-        scaling = A[m][iPivotCol];
-        // zero this element
-        A[m][iPivotCol] = 0.0F;
-        // loop over all columns l of A and perform elimination
-        for (l = 0; l < isize; l++) {
-          A[m][l] -= A[iPivotCol][l] * scaling;
-        }
-      }
-    }
-  } // end of loop i over the matrix dimensions
-
-  // finally, loop in inverse order to apply the missing column swaps
-  for (l = isize - 1; l >= 0; l--) {
-    // set i and j to the two columns to be swapped
-    i = iRowInd[l];
-    j = iColInd[l];
-
-    // check that the two columns i and j to be swapped are not the same
-    if (i != j) {
-      // loop over all rows k to swap columns i and j of A
-      for (k = 0; k < isize; k++) {
-        ftmp = A[k][i];
-        A[k][i] = A[k][j];
-        A[k][j] = ftmp;
-      }
-    }
-  }
-}
-
-// function re-orthonormalizes a 3x3 rotation matrix
-void fmatrixAeqRenormRotA(float A[][3]) {
-  float ftmp; // scratch variable
-
-  // normalize the X column of the low pass filtered orientation matrix
-  ftmp = sqrtf(A[X][X] * A[X][X] + A[Y][X] * A[Y][X] + A[Z][X] * A[Z][X]);
-  if (ftmp > CORRUPTMATRIX) {
-    // normalize the x column vector
-    ftmp = 1.0F / ftmp;
-    A[X][X] *= ftmp;
-    A[Y][X] *= ftmp;
-    A[Z][X] *= ftmp;
-  } else {
-    // set x column vector to {1, 0, 0}
-    A[X][X] = 1.0F;
-    A[Y][X] = A[Z][X] = 0.0F;
-  }
-
-  // force the y column vector to be orthogonal to x using y = y-(x.y)x
-  ftmp = A[X][X] * A[X][Y] + A[Y][X] * A[Y][Y] + A[Z][X] * A[Z][Y];
-  A[X][Y] -= ftmp * A[X][X];
-  A[Y][Y] -= ftmp * A[Y][X];
-  A[Z][Y] -= ftmp * A[Z][X];
-
-  // normalize the y column vector
-  ftmp = sqrtf(A[X][Y] * A[X][Y] + A[Y][Y] * A[Y][Y] + A[Z][Y] * A[Z][Y]);
-  if (ftmp > CORRUPTMATRIX) {
-    // normalize the y column vector
-    ftmp = 1.0F / ftmp;
-    A[X][Y] *= ftmp;
-    A[Y][Y] *= ftmp;
-    A[Z][Y] *= ftmp;
-  } else {
-    // set y column vector to {0, 1, 0}
-    A[Y][Y] = 1.0F;
-    A[X][Y] = A[Z][Y] = 0.0F;
-  }
-
-  // finally set the z column vector to x vector cross y vector (automatically
-  // normalized)
-  A[X][Z] = A[Y][X] * A[Z][Y] - A[Z][X] * A[Y][Y];
-  A[Y][Z] = A[Z][X] * A[X][Y] - A[X][X] * A[Z][Y];
-  A[Z][Z] = A[X][X] * A[Y][Y] - A[Y][X] * A[X][Y];
-}

src/Adafruit_Simple_AHRS.cpp

@@ -1,101 +0,0 @@
-/*!
- * @file Adafruit_Simple_AHRS.cpp
- */
-
-#include "Adafruit_Simple_AHRS.h"
-
-/**************************************************************************/
-/*!
- * @brief Create a simple AHRS from a device with multiple sensors.
- */
-/**************************************************************************/
-Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor *accelerometer,
-                                           Adafruit_Sensor *magnetometer)
-    : _accel(accelerometer), _mag(magnetometer) {}
-
-/**************************************************************************/
-/*!
- * @brief Create a simple AHRS from a device with multiple sensors.
- */
-/**************************************************************************/
-Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor_Set &sensors)
-    : _accel(sensors.getSensor(SENSOR_TYPE_ACCELEROMETER)),
-      _mag(sensors.getSensor(SENSOR_TYPE_MAGNETIC_FIELD)) {}
-
-/**************************************************************************/
-/*!
- * @brief Compute orientation based on accelerometer and magnetometer data.
- */
-/**************************************************************************/
-bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t *orientation) {
-  // Validate input and available sensors.
-  if (orientation == NULL || _accel == NULL || _mag == NULL)
-    return false;
-
-  // Grab an acceleromter and magnetometer reading.
-  sensors_event_t accel_event;
-  _accel->getEvent(&accel_event);
-  sensors_event_t mag_event;
-  _mag->getEvent(&mag_event);
-
-  float const PI_F = 3.14159265F;
-
-  // roll: Rotation around the X-axis. -180 <= roll <= 180
-  // a positive roll angle is defined to be a clockwise rotation about the
-  // positive X-axis
-  //
-  //                    y
-  //      roll = atan2(---)
-  //                    z
-  //
-  // where:  y, z are returned value from accelerometer sensor
-  orientation->roll =
-      (float)atan2(accel_event.acceleration.y, accel_event.acceleration.z);
-
-  // pitch: Rotation around the Y-axis. -180 <= roll <= 180
-  // a positive pitch angle is defined to be a clockwise rotation about the
-  // positive Y-axis
-  //
-  //                                 -x
-  //      pitch = atan(-------------------------------)
-  //                    y * sin(roll) + z * cos(roll)
-  //
-  // where:  x, y, z are returned value from accelerometer sensor
-  if (accel_event.acceleration.y * sin(orientation->roll) +
-          accel_event.acceleration.z * cos(orientation->roll) ==
-      0)
-    orientation->pitch =
-        accel_event.acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
-  else
-    orientation->pitch =
-        (float)atan(-accel_event.acceleration.x /
-                    (accel_event.acceleration.y * sin(orientation->roll) +
-                     accel_event.acceleration.z * cos(orientation->roll)));
-
-  // heading: Rotation around the Z-axis. -180 <= roll <= 180
-  // a positive heading angle is defined to be a clockwise rotation about the
-  // positive Z-axis
-  //
-  //                                       z * sin(roll) - y * cos(roll)
-  //   heading =
-  //   atan2(--------------------------------------------------------------------------)
-  //                    x * cos(pitch) + y * sin(pitch) * sin(roll) + z *
-  //                    sin(pitch) * cos(roll))
-  //
-  // where:  x, y, z are returned value from magnetometer sensor
-  orientation->heading =
-      (float)atan2(mag_event.magnetic.z * sin(orientation->roll) -
-                       mag_event.magnetic.y * cos(orientation->roll),
-                   mag_event.magnetic.x * cos(orientation->pitch) +
-                       mag_event.magnetic.y * sin(orientation->pitch) *
-                           sin(orientation->roll) +
-                       mag_event.magnetic.z * sin(orientation->pitch) *
-                           cos(orientation->roll));
-
-  // Convert angular data to degree
-  orientation->roll = orientation->roll * 180 / PI_F;
-  orientation->pitch = orientation->pitch * 180 / PI_F;
-  orientation->heading = orientation->heading * 180 / PI_F;
-
-  return true;
-}

src/Adafruit_Simple_AHRS.h

@@ -1,50 +0,0 @@
-/*!
- * @file Adafruit_Simple_AHRS.cpp
- */
-#ifndef __ADAFRUIT_SIMPLE_AHRS_H__
-#define __ADAFRUIT_SIMPLE_AHRS_H__
-
-#include "Adafruit_Sensor_Set.h"
-#include <Adafruit_Sensor.h>
-
-/*!
- * @brief Simple sensor fusion AHRS using an accelerometer and magnetometer.
- */
-class Adafruit_Simple_AHRS {
-public:
-  /**************************************************************************/
-  /*!
-   * @brief Create a simple AHRS from a device with multiple sensors.
-   *
-   * @param accelerometer The accelerometer to use for this sensor fusion.
-   * @param magnetometer The magnetometer to use for this sensor fusion.
-   */
-  /**************************************************************************/
-  Adafruit_Simple_AHRS(Adafruit_Sensor *accelerometer,
-                       Adafruit_Sensor *magnetometer);
-
-  /**************************************************************************/
-  /*!
-   * @brief Create a simple AHRS from a device with multiple sensors.
-   *
-   * @param sensors A set of sensors containing the accelerometer and
-   * magnetometer for this sensor fusion.
-   */
-  /**************************************************************************/
-  Adafruit_Simple_AHRS(Adafruit_Sensor_Set &sensors);
-
-  /**************************************************************************/
-  /*!
-   * @brief Compute orientation based on accelerometer and magnetometer data.
-   *
-   * @return Whether the orientation was computed.
-   */
-  /**************************************************************************/
-  bool getOrientation(sensors_vec_t *orientation);
-
-private:
-  Adafruit_Sensor *_accel;
-  Adafruit_Sensor *_mag;
-};
-
-#endif