.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 +0,0 @@
-serialconfig.txt
-.pio
-html

.nojekyll

@@ -0,0 +1 @@
+

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_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;
-}

examples/deprecated/ahrs_10dof/ahrs_10dof.ino

@@ -1,68 +0,0 @@
-#include <Wire.h>
-#include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_Accel.h>
-#include <Adafruit_LSM303DLH_Mag.h>
-#include <Adafruit_BMP085_U.h>
-#include <Adafruit_Simple_AHRS.h>
-
-// Create sensor instances.
-Adafruit_LSM303_Accel_Unified  accel(30301);
-Adafruit_LSM303DLH_Mag_Unified mag(30302);
-Adafruit_BMP085_Unified        bmp(18001);
-
-// 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 10 DOF Board AHRS Example")); Serial.println("");
-  
-  // Initialize the sensors.
-  accel.begin();
-  mag.begin();
-  bmp.begin();
-}
-
-void loop(void)
-{
-  sensors_vec_t   orientation;
-
-  // Use the simple AHRS function to get the current orientation.
-  if (ahrs.getOrientation(&orientation))
-  {
-    /* 'orientation' should have valid .roll and .pitch fields */
-    Serial.print(F("Orientation: "));
-    Serial.print(orientation.roll);
-    Serial.print(F(" "));
-    Serial.print(orientation.pitch);
-    Serial.print(F(" "));
-    Serial.print(orientation.heading);
-    Serial.println(F(""));
-  }
-
-  // Calculate the altitude using the barometric pressure sensor
-  sensors_event_t bmp_event;
-  bmp.getEvent(&bmp_event);
-  if (bmp_event.pressure)
-  {
-    /* Get ambient temperature in C */
-    float temperature;
-    bmp.getTemperature(&temperature);
-    /* Convert atmospheric pressure, SLP and temp to altitude */
-    Serial.print(F("Alt: "));
-    Serial.print(bmp.pressureToAltitude(seaLevelPressure,
-                                        bmp_event.pressure,
-                                        temperature)); 
-    Serial.println(F(""));
-    /* Display the temperature */
-    Serial.print(F("Temp: "));
-    Serial.print(temperature);
-    Serial.println(F(""));
-  }
-  
-  delay(100);
-}

examples/deprecated/ahrs_9dof/ahrs_9dof.ino

@@ -1,42 +0,0 @@
-#include <Wire.h>
-#include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_Accel.h>
-#include <Adafruit_LSM303DLH_Mag.h>
-#include <Adafruit_Simple_AHRS.h>
-
-// Create sensor instances.
-Adafruit_LSM303_Accel_Unified  accel(30301);
-Adafruit_LSM303DLH_Mag_Unified mag(30302);
-
-// Create simple AHRS algorithm using the above sensors.
-Adafruit_Simple_AHRS          ahrs(&accel, &mag);
-
-void setup()
-{
-  Serial.begin(115200);
-  Serial.println(F("Adafruit 9 DOF Board AHRS Example")); Serial.println("");
-  
-  // Initialize the sensors.
-  accel.begin();
-  mag.begin();
-}
-
-void loop(void)
-{
-  sensors_vec_t   orientation;
-
-  // Use the simple AHRS function to get the current orientation.
-  if (ahrs.getOrientation(&orientation))
-  {
-    /* 'orientation' should have valid .roll and .pitch fields */
-    Serial.print(F("Orientation: "));
-    Serial.print(orientation.roll);
-    Serial.print(F(" "));
-    Serial.print(orientation.pitch);
-    Serial.print(F(" "));
-    Serial.print(orientation.heading);
-    Serial.println(F(""));
-  }
-  
-  delay(100);
-}

html/dynsections.js

@@ -0,0 +1,97 @@
+function toggleVisibility(linkObj)
+{
+ var base = $(linkObj).attr('id');
+ var summary = $('#'+base+'-summary');
+ var content = $('#'+base+'-content');
+ var trigger = $('#'+base+'-trigger');
+ var src=$(trigger).attr('src');
+ if (content.is(':visible')===true) {
+   content.hide();
+   summary.show();
+   $(linkObj).addClass('closed').removeClass('opened');
+   $(trigger).attr('src',src.substring(0,src.length-8)+'closed.png');
+ } else {
+   content.show();
+   summary.hide();
+   $(linkObj).removeClass('closed').addClass('opened');
+   $(trigger).attr('src',src.substring(0,src.length-10)+'open.png');
+ } 
+ return false;
+}
+
+function updateStripes()
+{
+  $('table.directory tr').
+       removeClass('even').filter(':visible:even').addClass('even');
+}
+
+function toggleLevel(level)
+{
+  $('table.directory tr').each(function() {
+    var l = this.id.split('_').length-1;
+    var i = $('#img'+this.id.substring(3));
+    var a = $('#arr'+this.id.substring(3));
+    if (l<level+1) {
+      i.removeClass('iconfopen iconfclosed').addClass('iconfopen');
+      a.html('&#9660;');
+      $(this).show();
+    } else if (l==level+1) {
+      i.removeClass('iconfclosed iconfopen').addClass('iconfclosed');
+      a.html('&#9658;');
+      $(this).show();
+    } else {
+      $(this).hide();
+    }
+  });
+  updateStripes();
+}
+
+function toggleFolder(id)
+{
+  // the clicked row
+  var currentRow = $('#row_'+id);
+
+  // all rows after the clicked row
+  var rows = currentRow.nextAll("tr");
+
+  var re = new RegExp('^row_'+id+'\\d+_$', "i"); //only one sub
+
+  // only match elements AFTER this one (can't hide elements before)
+  var childRows = rows.filter(function() { return this.id.match(re); });
+
+  // first row is visible we are HIDING
+  if (childRows.filter(':first').is(':visible')===true) {
+    // replace down arrow by right arrow for current row
+    var currentRowSpans = currentRow.find("span");
+    currentRowSpans.filter(".iconfopen").removeClass("iconfopen").addClass("iconfclosed");
+    currentRowSpans.filter(".arrow").html('&#9658;');
+    rows.filter("[id^=row_"+id+"]").hide(); // hide all children
+  } else { // we are SHOWING
+    // replace right arrow by down arrow for current row
+    var currentRowSpans = currentRow.find("span");
+    currentRowSpans.filter(".iconfclosed").removeClass("iconfclosed").addClass("iconfopen");
+    currentRowSpans.filter(".arrow").html('&#9660;');
+    // replace down arrows by right arrows for child rows
+    var childRowsSpans = childRows.find("span");
+    childRowsSpans.filter(".iconfopen").removeClass("iconfopen").addClass("iconfclosed");
+    childRowsSpans.filter(".arrow").html('&#9658;');
+    childRows.show(); //show all children
+  }
+  updateStripes();
+}
+
+
+function toggleInherit(id)
+{
+  var rows = $('tr.inherit.'+id);
+  var img = $('tr.inherit_header.'+id+' img');
+  var src = $(img).attr('src');
+  if (rows.filter(':first').is(':visible')===true) {
+    rows.css('display','none');
+    $(img).attr('src',src.substring(0,src.length-8)+'closed.png');
+  } else {
+    rows.css('display','table-row'); // using show() causes jump in firefox
+    $(img).attr('src',src.substring(0,src.length-10)+'open.png');
+  }
+}
+

html/index.html

@@ -0,0 +1,73 @@
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.13"/>
+<meta name="viewport" content="width=device-width, initial-scale=1"/>
+<title>Adafruit AHRS Library: Main Page</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="search/search.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="search/searchdata.js"></script>
+<script type="text/javascript" src="search/search.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+  <td id="projectalign" style="padding-left: 0.5em;">
+   <div id="projectname">Adafruit AHRS Library
+   </div>
+  </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.13 -->
+<script type="text/javascript">
+var searchBox = new SearchBox("searchBox", "search",false,'Search');
+</script>
+<script type="text/javascript" src="menudata.js"></script>
+<script type="text/javascript" src="menu.js"></script>
+<script type="text/javascript">
+$(function() {
+  initMenu('',true,false,'search.php','Search');
+  $(document).ready(function() { init_search(); });
+});
+</script>
+<div id="main-nav"></div>
+</div><!-- top -->
+<!-- window showing the filter options -->
+<div id="MSearchSelectWindow"
+     onmouseover="return searchBox.OnSearchSelectShow()"
+     onmouseout="return searchBox.OnSearchSelectHide()"
+     onkeydown="return searchBox.OnSearchSelectKey(event)">
+</div>
+
+<!-- iframe showing the search results (closed by default) -->
+<div id="MSearchResultsWindow">
+<iframe src="javascript:void(0)" frameborder="0" 
+        name="MSearchResults" id="MSearchResults">
+</iframe>
+</div>
+
+<div class="header">
+  <div class="headertitle">
+<div class="title">Adafruit AHRS Library Documentation</div>  </div>
+</div><!--header-->
+<div class="contents">
+</div><!-- contents -->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by &#160;<a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.13
+</small></address>
+</body>
+</html>

html/menu.js

@@ -0,0 +1,26 @@
+function initMenu(relPath,searchEnabled,serverSide,searchPage,search) {
+  function makeTree(data,relPath) {
+    var result='';
+    if ('children' in data) {
+      result+='<ul>';
+      for (var i in data.children) {
+        result+='<li><a href="'+relPath+data.children[i].url+'">'+
+                                data.children[i].text+'</a>'+
+                                makeTree(data.children[i],relPath)+'</li>';
+      }
+      result+='</ul>';
+    }
+    return result;
+  }
+
+  $('#main-nav').append(makeTree(menudata,relPath));
+  $('#main-nav').children(':first').addClass('sm sm-dox').attr('id','main-menu');
+  if (searchEnabled) {
+    if (serverSide) {
+      $('#main-menu').append('<li style="float:right"><div id="MSearchBox" class="MSearchBoxInactive"><div class="left"><form id="FSearchBox" action="'+searchPage+'" method="get"><img id="MSearchSelect" src="'+relPath+'search/mag.png" alt=""/><input type="text" id="MSearchField" name="query" value="'+search+'" size="20" accesskey="S" onfocus="searchBox.OnSearchFieldFocus(true)" onblur="searchBox.OnSearchFieldFocus(false)"></form></div><div class="right"></div></div></li>');
+    } else {
+      $('#main-menu').append('<li style="float:right"><div id="MSearchBox" class="MSearchBoxInactive"><span class="left"><img id="MSearchSelect" src="'+relPath+'search/mag_sel.png" onmouseover="return searchBox.OnSearchSelectShow()" onmouseout="return searchBox.OnSearchSelectHide()" alt=""/><input type="text" id="MSearchField" value="'+search+'" accesskey="S" onfocus="searchBox.OnSearchFieldFocus(true)" onblur="searchBox.OnSearchFieldFocus(false)" onkeyup="searchBox.OnSearchFieldChange(event)"/></span><span class="right"><a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="'+relPath+'search/close.png" alt=""/></a></span></div></li>');
+    }
+  }
+  $('#main-menu').smartmenus();
+}

html/menudata.js

@@ -0,0 +1,2 @@
+var menudata={children:[
+{text:"Main Page",url:"index.html"}]}

html/search/nomatches.html

@@ -0,0 +1,12 @@
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html><head><title></title>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<link rel="stylesheet" type="text/css" href="search.css"/>
+<script type="text/javascript" src="search.js"></script>
+</head>
+<body class="SRPage">
+<div id="SRIndex">
+<div class="SRStatus" id="NoMatches">No Matches</div>
+</div>
+</body>
+</html>

html/search/search.css

@@ -0,0 +1,271 @@
+/*---------------- Search Box */
+
+#FSearchBox {
+    float: left;
+}
+
+#MSearchBox {
+    white-space : nowrap;
+    float: none;
+    margin-top: 8px;
+    right: 0px;
+    width: 170px;
+    height: 24px;
+    z-index: 102;
+}
+
+#MSearchBox .left
+{
+    display:block;
+    position:absolute;
+    left:10px;
+    width:20px;
+    height:19px;
+    background:url('search_l.png') no-repeat;
+    background-position:right;
+}
+
+#MSearchSelect {
+    display:block;
+    position:absolute;
+    width:20px;
+    height:19px;
+}
+
+.left #MSearchSelect {
+    left:4px;
+}
+
+.right #MSearchSelect {
+    right:5px;
+}
+
+#MSearchField {
+    display:block;
+    position:absolute;
+    height:19px;
+    background:url('search_m.png') repeat-x;
+    border:none;
+    width:115px;
+    margin-left:20px;
+    padding-left:4px;
+    color: #909090;
+    outline: none;
+    font: 9pt Arial, Verdana, sans-serif;
+    -webkit-border-radius: 0px;
+}
+
+#FSearchBox #MSearchField {
+    margin-left:15px;
+}
+
+#MSearchBox .right {
+    display:block;
+    position:absolute;
+    right:10px;
+    top:8px;
+    width:20px;
+    height:19px;
+    background:url('search_r.png') no-repeat;
+    background-position:left;
+}
+
+#MSearchClose {
+    display: none;
+    position: absolute;
+    top: 4px;
+    background : none;
+    border: none;
+    margin: 0px 4px 0px 0px;
+    padding: 0px 0px;
+    outline: none;
+}
+
+.left #MSearchClose {
+    left: 6px;
+}
+
+.right #MSearchClose {
+    right: 2px;
+}
+
+.MSearchBoxActive #MSearchField {
+    color: #000000;
+}
+
+/*---------------- Search filter selection */
+
+#MSearchSelectWindow {
+    display: none;
+    position: absolute;
+    left: 0; top: 0;
+    border: 1px solid #90A5CE;
+    background-color: #F9FAFC;
+    z-index: 10001;
+    padding-top: 4px;
+    padding-bottom: 4px;
+    -moz-border-radius: 4px;
+    -webkit-border-top-left-radius: 4px;
+    -webkit-border-top-right-radius: 4px;
+    -webkit-border-bottom-left-radius: 4px;
+    -webkit-border-bottom-right-radius: 4px;
+    -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+}
+
+.SelectItem {
+    font: 8pt Arial, Verdana, sans-serif;
+    padding-left:  2px;
+    padding-right: 12px;
+    border: 0px;
+}
+
+span.SelectionMark {
+    margin-right: 4px;
+    font-family: monospace;
+    outline-style: none;
+    text-decoration: none;
+}
+
+a.SelectItem {
+    display: block;
+    outline-style: none;
+    color: #000000; 
+    text-decoration: none;
+    padding-left:   6px;
+    padding-right: 12px;
+}
+
+a.SelectItem:focus,
+a.SelectItem:active {
+    color: #000000; 
+    outline-style: none;
+    text-decoration: none;
+}
+
+a.SelectItem:hover {
+    color: #FFFFFF;
+    background-color: #3D578C;
+    outline-style: none;
+    text-decoration: none;
+    cursor: pointer;
+    display: block;
+}
+
+/*---------------- Search results window */
+
+iframe#MSearchResults {
+    width: 60ex;
+    height: 15em;
+}
+
+#MSearchResultsWindow {
+    display: none;
+    position: absolute;
+    left: 0; top: 0;
+    border: 1px solid #000;
+    background-color: #EEF1F7;
+    z-index:10000;
+}
+
+/* ----------------------------------- */
+
+
+#SRIndex {
+    clear:both; 
+    padding-bottom: 15px;
+}
+
+.SREntry {
+    font-size: 10pt;
+    padding-left: 1ex;
+}
+
+.SRPage .SREntry {
+    font-size: 8pt;
+    padding: 1px 5px;
+}
+
+body.SRPage {
+    margin: 5px 2px;
+}
+
+.SRChildren {
+    padding-left: 3ex; padding-bottom: .5em 
+}
+
+.SRPage .SRChildren {
+    display: none;
+}
+
+.SRSymbol {
+    font-weight: bold; 
+    color: #425E97;
+    font-family: Arial, Verdana, sans-serif;
+    text-decoration: none;
+    outline: none;
+}
+
+a.SRScope {
+    display: block;
+    color: #425E97; 
+    font-family: Arial, Verdana, sans-serif;
+    text-decoration: none;
+    outline: none;
+}
+
+a.SRSymbol:focus, a.SRSymbol:active,
+a.SRScope:focus, a.SRScope:active {
+    text-decoration: underline;
+}
+
+span.SRScope {
+    padding-left: 4px;
+}
+
+.SRPage .SRStatus {
+    padding: 2px 5px;
+    font-size: 8pt;
+    font-style: italic;
+}
+
+.SRResult {
+    display: none;
+}
+
+DIV.searchresults {
+    margin-left: 10px;
+    margin-right: 10px;
+}
+
+/*---------------- External search page results */
+
+.searchresult {
+    background-color: #F0F3F8;
+}
+
+.pages b {
+   color: white;
+   padding: 5px 5px 3px 5px;
+   background-image: url("../tab_a.png");
+   background-repeat: repeat-x;
+   text-shadow: 0 1px 1px #000000;
+}
+
+.pages {
+    line-height: 17px;
+    margin-left: 4px;
+    text-decoration: none;
+}
+
+.hl {
+    font-weight: bold;
+}
+
+#searchresults {
+    margin-bottom: 20px;
+}
+
+.searchpages {
+    margin-top: 10px;
+}
+

html/search/search.js

@@ -0,0 +1,791 @@
+function convertToId(search)
+{
+  var result = '';
+  for (i=0;i<search.length;i++)
+  {
+    var c = search.charAt(i);
+    var cn = c.charCodeAt(0);
+    if (c.match(/[a-z0-9\u0080-\uFFFF]/))
+    {
+      result+=c;
+    }
+    else if (cn<16)
+    {
+      result+="_0"+cn.toString(16);
+    }
+    else
+    {
+      result+="_"+cn.toString(16);
+    }
+  }
+  return result;
+}
+
+function getXPos(item)
+{
+  var x = 0;
+  if (item.offsetWidth)
+  {
+    while (item && item!=document.body)
+    {
+      x   += item.offsetLeft;
+      item = item.offsetParent;
+    }
+  }
+  return x;
+}
+
+function getYPos(item)
+{
+  var y = 0;
+  if (item.offsetWidth)
+  {
+     while (item && item!=document.body)
+     {
+       y   += item.offsetTop;
+       item = item.offsetParent;
+     }
+  }
+  return y;
+}
+
+/* A class handling everything associated with the search panel.
+
+   Parameters:
+   name - The name of the global variable that will be
+          storing this instance.  Is needed to be able to set timeouts.
+   resultPath - path to use for external files
+*/
+function SearchBox(name, resultsPath, inFrame, label)
+{
+  if (!name || !resultsPath) {  alert("Missing parameters to SearchBox."); }
+
+  // ---------- Instance variables
+  this.name                  = name;
+  this.resultsPath           = resultsPath;
+  this.keyTimeout            = 0;
+  this.keyTimeoutLength      = 500;
+  this.closeSelectionTimeout = 300;
+  this.lastSearchValue       = "";
+  this.lastResultsPage       = "";
+  this.hideTimeout           = 0;
+  this.searchIndex           = 0;
+  this.searchActive          = false;
+  this.insideFrame           = inFrame;
+  this.searchLabel           = label;
+
+  // ----------- DOM Elements
+
+  this.DOMSearchField = function()
+  {  return document.getElementById("MSearchField");  }
+
+  this.DOMSearchSelect = function()
+  {  return document.getElementById("MSearchSelect");  }
+
+  this.DOMSearchSelectWindow = function()
+  {  return document.getElementById("MSearchSelectWindow");  }
+
+  this.DOMPopupSearchResults = function()
+  {  return document.getElementById("MSearchResults");  }
+
+  this.DOMPopupSearchResultsWindow = function()
+  {  return document.getElementById("MSearchResultsWindow");  }
+
+  this.DOMSearchClose = function()
+  {  return document.getElementById("MSearchClose"); }
+
+  this.DOMSearchBox = function()
+  {  return document.getElementById("MSearchBox");  }
+
+  // ------------ Event Handlers
+
+  // Called when focus is added or removed from the search field.
+  this.OnSearchFieldFocus = function(isActive)
+  {
+    this.Activate(isActive);
+  }
+
+  this.OnSearchSelectShow = function()
+  {
+    var searchSelectWindow = this.DOMSearchSelectWindow();
+    var searchField        = this.DOMSearchSelect();
+
+    if (this.insideFrame)
+    {
+      var left = getXPos(searchField);
+      var top  = getYPos(searchField);
+      left += searchField.offsetWidth + 6;
+      top += searchField.offsetHeight;
+
+      // show search selection popup
+      searchSelectWindow.style.display='block';
+      left -= searchSelectWindow.offsetWidth;
+      searchSelectWindow.style.left =  left + 'px';
+      searchSelectWindow.style.top  =  top  + 'px';
+    }
+    else
+    {
+      var left = getXPos(searchField);
+      var top  = getYPos(searchField);
+      top += searchField.offsetHeight;
+
+      // show search selection popup
+      searchSelectWindow.style.display='block';
+      searchSelectWindow.style.left =  left + 'px';
+      searchSelectWindow.style.top  =  top  + 'px';
+    }
+
+    // stop selection hide timer
+    if (this.hideTimeout)
+    {
+      clearTimeout(this.hideTimeout);
+      this.hideTimeout=0;
+    }
+    return false; // to avoid "image drag" default event
+  }
+
+  this.OnSearchSelectHide = function()
+  {
+    this.hideTimeout = setTimeout(this.name +".CloseSelectionWindow()",
+                                  this.closeSelectionTimeout);
+  }
+
+  // Called when the content of the search field is changed.
+  this.OnSearchFieldChange = function(evt)
+  {
+    if (this.keyTimeout) // kill running timer
+    {
+      clearTimeout(this.keyTimeout);
+      this.keyTimeout = 0;
+    }
+
+    var e  = (evt) ? evt : window.event; // for IE
+    if (e.keyCode==40 || e.keyCode==13)
+    {
+      if (e.shiftKey==1)
+      {
+        this.OnSearchSelectShow();
+        var win=this.DOMSearchSelectWindow();
+        for (i=0;i<win.childNodes.length;i++)
+        {
+          var child = win.childNodes[i]; // get span within a
+          if (child.className=='SelectItem')
+          {
+            child.focus();
+            return;
+          }
+        }
+        return;
+      }
+      else if (window.frames.MSearchResults.searchResults)
+      {
+        var elem = window.frames.MSearchResults.searchResults.NavNext(0);
+        if (elem) elem.focus();
+      }
+    }
+    else if (e.keyCode==27) // Escape out of the search field
+    {
+      this.DOMSearchField().blur();
+      this.DOMPopupSearchResultsWindow().style.display = 'none';
+      this.DOMSearchClose().style.display = 'none';
+      this.lastSearchValue = '';
+      this.Activate(false);
+      return;
+    }
+
+    // strip whitespaces
+    var searchValue = this.DOMSearchField().value.replace(/ +/g, "");
+
+    if (searchValue != this.lastSearchValue) // search value has changed
+    {
+      if (searchValue != "") // non-empty search
+      {
+        // set timer for search update
+        this.keyTimeout = setTimeout(this.name + '.Search()',
+                                     this.keyTimeoutLength);
+      }
+      else // empty search field
+      {
+        this.DOMPopupSearchResultsWindow().style.display = 'none';
+        this.DOMSearchClose().style.display = 'none';
+        this.lastSearchValue = '';
+      }
+    }
+  }
+
+  this.SelectItemCount = function(id)
+  {
+    var count=0;
+    var win=this.DOMSearchSelectWindow();
+    for (i=0;i<win.childNodes.length;i++)
+    {
+      var child = win.childNodes[i]; // get span within a
+      if (child.className=='SelectItem')
+      {
+        count++;
+      }
+    }
+    return count;
+  }
+
+  this.SelectItemSet = function(id)
+  {
+    var i,j=0;
+    var win=this.DOMSearchSelectWindow();
+    for (i=0;i<win.childNodes.length;i++)
+    {
+      var child = win.childNodes[i]; // get span within a
+      if (child.className=='SelectItem')
+      {
+        var node = child.firstChild;
+        if (j==id)
+        {
+          node.innerHTML='&#8226;';
+        }
+        else
+        {
+          node.innerHTML='&#160;';
+        }
+        j++;
+      }
+    }
+  }
+
+  // Called when an search filter selection is made.
+  // set item with index id as the active item
+  this.OnSelectItem = function(id)
+  {
+    this.searchIndex = id;
+    this.SelectItemSet(id);
+    var searchValue = this.DOMSearchField().value.replace(/ +/g, "");
+    if (searchValue!="" && this.searchActive) // something was found -> do a search
+    {
+      this.Search();
+    }
+  }
+
+  this.OnSearchSelectKey = function(evt)
+  {
+    var e = (evt) ? evt : window.event; // for IE
+    if (e.keyCode==40 && this.searchIndex<this.SelectItemCount()) // Down
+    {
+      this.searchIndex++;
+      this.OnSelectItem(this.searchIndex);
+    }
+    else if (e.keyCode==38 && this.searchIndex>0) // Up
+    {
+      this.searchIndex--;
+      this.OnSelectItem(this.searchIndex);
+    }
+    else if (e.keyCode==13 || e.keyCode==27)
+    {
+      this.OnSelectItem(this.searchIndex);
+      this.CloseSelectionWindow();
+      this.DOMSearchField().focus();
+    }
+    return false;
+  }
+
+  // --------- Actions
+
+  // Closes the results window.
+  this.CloseResultsWindow = function()
+  {
+    this.DOMPopupSearchResultsWindow().style.display = 'none';
+    this.DOMSearchClose().style.display = 'none';
+    this.Activate(false);
+  }
+
+  this.CloseSelectionWindow = function()
+  {
+    this.DOMSearchSelectWindow().style.display = 'none';
+  }
+
+  // Performs a search.
+  this.Search = function()
+  {
+    this.keyTimeout = 0;
+
+    // strip leading whitespace
+    var searchValue = this.DOMSearchField().value.replace(/^ +/, "");
+
+    var code = searchValue.toLowerCase().charCodeAt(0);
+    var idxChar = searchValue.substr(0, 1).toLowerCase();
+    if ( 0xD800 <= code && code <= 0xDBFF && searchValue > 1) // surrogate pair
+    {
+      idxChar = searchValue.substr(0, 2);
+    }
+
+    var resultsPage;
+    var resultsPageWithSearch;
+    var hasResultsPage;
+
+    var idx = indexSectionsWithContent[this.searchIndex].indexOf(idxChar);
+    if (idx!=-1)
+    {
+       var hexCode=idx.toString(16);
+       resultsPage = this.resultsPath + '/' + indexSectionNames[this.searchIndex] + '_' + hexCode + '.html';
+       resultsPageWithSearch = resultsPage+'?'+escape(searchValue);
+       hasResultsPage = true;
+    }
+    else // nothing available for this search term
+    {
+       resultsPage = this.resultsPath + '/nomatches.html';
+       resultsPageWithSearch = resultsPage;
+       hasResultsPage = false;
+    }
+
+    window.frames.MSearchResults.location = resultsPageWithSearch;
+    var domPopupSearchResultsWindow = this.DOMPopupSearchResultsWindow();
+
+    if (domPopupSearchResultsWindow.style.display!='block')
+    {
+       var domSearchBox = this.DOMSearchBox();
+       this.DOMSearchClose().style.display = 'inline';
+       if (this.insideFrame)
+       {
+         var domPopupSearchResults = this.DOMPopupSearchResults();
+         domPopupSearchResultsWindow.style.position = 'relative';
+         domPopupSearchResultsWindow.style.display  = 'block';
+         var width = document.body.clientWidth - 8; // the -8 is for IE :-(
+         domPopupSearchResultsWindow.style.width    = width + 'px';
+         domPopupSearchResults.style.width          = width + 'px';
+       }
+       else
+       {
+         var domPopupSearchResults = this.DOMPopupSearchResults();
+         var left = getXPos(domSearchBox) + 150; // domSearchBox.offsetWidth;
+         var top  = getYPos(domSearchBox) + 20;  // domSearchBox.offsetHeight + 1;
+         domPopupSearchResultsWindow.style.display = 'block';
+         left -= domPopupSearchResults.offsetWidth;
+         domPopupSearchResultsWindow.style.top     = top  + 'px';
+         domPopupSearchResultsWindow.style.left    = left + 'px';
+       }
+    }
+
+    this.lastSearchValue = searchValue;
+    this.lastResultsPage = resultsPage;
+  }
+
+  // -------- Activation Functions
+
+  // Activates or deactivates the search panel, resetting things to
+  // their default values if necessary.
+  this.Activate = function(isActive)
+  {
+    if (isActive || // open it
+        this.DOMPopupSearchResultsWindow().style.display == 'block'
+       )
+    {
+      this.DOMSearchBox().className = 'MSearchBoxActive';
+
+      var searchField = this.DOMSearchField();
+
+      if (searchField.value == this.searchLabel) // clear "Search" term upon entry
+      {
+        searchField.value = '';
+        this.searchActive = true;
+      }
+    }
+    else if (!isActive) // directly remove the panel
+    {
+      this.DOMSearchBox().className = 'MSearchBoxInactive';
+      this.DOMSearchField().value   = this.searchLabel;
+      this.searchActive             = false;
+      this.lastSearchValue          = ''
+      this.lastResultsPage          = '';
+    }
+  }
+}
+
+// -----------------------------------------------------------------------
+
+// The class that handles everything on the search results page.
+function SearchResults(name)
+{
+    // The number of matches from the last run of <Search()>.
+    this.lastMatchCount = 0;
+    this.lastKey = 0;
+    this.repeatOn = false;
+
+    // Toggles the visibility of the passed element ID.
+    this.FindChildElement = function(id)
+    {
+      var parentElement = document.getElementById(id);
+      var element = parentElement.firstChild;
+
+      while (element && element!=parentElement)
+      {
+        if (element.nodeName == 'DIV' && element.className == 'SRChildren')
+        {
+          return element;
+        }
+
+        if (element.nodeName == 'DIV' && element.hasChildNodes())
+        {
+           element = element.firstChild;
+        }
+        else if (element.nextSibling)
+        {
+           element = element.nextSibling;
+        }
+        else
+        {
+          do
+          {
+            element = element.parentNode;
+          }
+          while (element && element!=parentElement && !element.nextSibling);
+
+          if (element && element!=parentElement)
+          {
+            element = element.nextSibling;
+          }
+        }
+      }
+    }
+
+    this.Toggle = function(id)
+    {
+      var element = this.FindChildElement(id);
+      if (element)
+      {
+        if (element.style.display == 'block')
+        {
+          element.style.display = 'none';
+        }
+        else
+        {
+          element.style.display = 'block';
+        }
+      }
+    }
+
+    // Searches for the passed string.  If there is no parameter,
+    // it takes it from the URL query.
+    //
+    // Always returns true, since other documents may try to call it
+    // and that may or may not be possible.
+    this.Search = function(search)
+    {
+      if (!search) // get search word from URL
+      {
+        search = window.location.search;
+        search = search.substring(1);  // Remove the leading '?'
+        search = unescape(search);
+      }
+
+      search = search.replace(/^ +/, ""); // strip leading spaces
+      search = search.replace(/ +$/, ""); // strip trailing spaces
+      search = search.toLowerCase();
+      search = convertToId(search);
+
+      var resultRows = document.getElementsByTagName("div");
+      var matches = 0;
+
+      var i = 0;
+      while (i < resultRows.length)
+      {
+        var row = resultRows.item(i);
+        if (row.className == "SRResult")
+        {
+          var rowMatchName = row.id.toLowerCase();
+          rowMatchName = rowMatchName.replace(/^sr\d*_/, ''); // strip 'sr123_'
+
+          if (search.length<=rowMatchName.length &&
+             rowMatchName.substr(0, search.length)==search)
+          {
+            row.style.display = 'block';
+            matches++;
+          }
+          else
+          {
+            row.style.display = 'none';
+          }
+        }
+        i++;
+      }
+      document.getElementById("Searching").style.display='none';
+      if (matches == 0) // no results
+      {
+        document.getElementById("NoMatches").style.display='block';
+      }
+      else // at least one result
+      {
+        document.getElementById("NoMatches").style.display='none';
+      }
+      this.lastMatchCount = matches;
+      return true;
+    }
+
+    // return the first item with index index or higher that is visible
+    this.NavNext = function(index)
+    {
+      var focusItem;
+      while (1)
+      {
+        var focusName = 'Item'+index;
+        focusItem = document.getElementById(focusName);
+        if (focusItem && focusItem.parentNode.parentNode.style.display=='block')
+        {
+          break;
+        }
+        else if (!focusItem) // last element
+        {
+          break;
+        }
+        focusItem=null;
+        index++;
+      }
+      return focusItem;
+    }
+
+    this.NavPrev = function(index)
+    {
+      var focusItem;
+      while (1)
+      {
+        var focusName = 'Item'+index;
+        focusItem = document.getElementById(focusName);
+        if (focusItem && focusItem.parentNode.parentNode.style.display=='block')
+        {
+          break;
+        }
+        else if (!focusItem) // last element
+        {
+          break;
+        }
+        focusItem=null;
+        index--;
+      }
+      return focusItem;
+    }
+
+    this.ProcessKeys = function(e)
+    {
+      if (e.type == "keydown")
+      {
+        this.repeatOn = false;
+        this.lastKey = e.keyCode;
+      }
+      else if (e.type == "keypress")
+      {
+        if (!this.repeatOn)
+        {
+          if (this.lastKey) this.repeatOn = true;
+          return false; // ignore first keypress after keydown
+        }
+      }
+      else if (e.type == "keyup")
+      {
+        this.lastKey = 0;
+        this.repeatOn = false;
+      }
+      return this.lastKey!=0;
+    }
+
+    this.Nav = function(evt,itemIndex)
+    {
+      var e  = (evt) ? evt : window.event; // for IE
+      if (e.keyCode==13) return true;
+      if (!this.ProcessKeys(e)) return false;
+
+      if (this.lastKey==38) // Up
+      {
+        var newIndex = itemIndex-1;
+        var focusItem = this.NavPrev(newIndex);
+        if (focusItem)
+        {
+          var child = this.FindChildElement(focusItem.parentNode.parentNode.id);
+          if (child && child.style.display == 'block') // children visible
+          {
+            var n=0;
+            var tmpElem;
+            while (1) // search for last child
+            {
+              tmpElem = document.getElementById('Item'+newIndex+'_c'+n);
+              if (tmpElem)
+              {
+                focusItem = tmpElem;
+              }
+              else // found it!
+              {
+                break;
+              }
+              n++;
+            }
+          }
+        }
+        if (focusItem)
+        {
+          focusItem.focus();
+        }
+        else // return focus to search field
+        {
+           parent.document.getElementById("MSearchField").focus();
+        }
+      }
+      else if (this.lastKey==40) // Down
+      {
+        var newIndex = itemIndex+1;
+        var focusItem;
+        var item = document.getElementById('Item'+itemIndex);
+        var elem = this.FindChildElement(item.parentNode.parentNode.id);
+        if (elem && elem.style.display == 'block') // children visible
+        {
+          focusItem = document.getElementById('Item'+itemIndex+'_c0');
+        }
+        if (!focusItem) focusItem = this.NavNext(newIndex);
+        if (focusItem)  focusItem.focus();
+      }
+      else if (this.lastKey==39) // Right
+      {
+        var item = document.getElementById('Item'+itemIndex);
+        var elem = this.FindChildElement(item.parentNode.parentNode.id);
+        if (elem) elem.style.display = 'block';
+      }
+      else if (this.lastKey==37) // Left
+      {
+        var item = document.getElementById('Item'+itemIndex);
+        var elem = this.FindChildElement(item.parentNode.parentNode.id);
+        if (elem) elem.style.display = 'none';
+      }
+      else if (this.lastKey==27) // Escape
+      {
+        parent.searchBox.CloseResultsWindow();
+        parent.document.getElementById("MSearchField").focus();
+      }
+      else if (this.lastKey==13) // Enter
+      {
+        return true;
+      }
+      return false;
+    }
+
+    this.NavChild = function(evt,itemIndex,childIndex)
+    {
+      var e  = (evt) ? evt : window.event; // for IE
+      if (e.keyCode==13) return true;
+      if (!this.ProcessKeys(e)) return false;
+
+      if (this.lastKey==38) // Up
+      {
+        if (childIndex>0)
+        {
+          var newIndex = childIndex-1;
+          document.getElementById('Item'+itemIndex+'_c'+newIndex).focus();
+        }
+        else // already at first child, jump to parent
+        {
+          document.getElementById('Item'+itemIndex).focus();
+        }
+      }
+      else if (this.lastKey==40) // Down
+      {
+        var newIndex = childIndex+1;
+        var elem = document.getElementById('Item'+itemIndex+'_c'+newIndex);
+        if (!elem) // last child, jump to parent next parent
+        {
+          elem = this.NavNext(itemIndex+1);
+        }
+        if (elem)
+        {
+          elem.focus();
+        }
+      }
+      else if (this.lastKey==27) // Escape
+      {
+        parent.searchBox.CloseResultsWindow();
+        parent.document.getElementById("MSearchField").focus();
+      }
+      else if (this.lastKey==13) // Enter
+      {
+        return true;
+      }
+      return false;
+    }
+}
+
+function setKeyActions(elem,action)
+{
+  elem.setAttribute('onkeydown',action);
+  elem.setAttribute('onkeypress',action);
+  elem.setAttribute('onkeyup',action);
+}
+
+function setClassAttr(elem,attr)
+{
+  elem.setAttribute('class',attr);
+  elem.setAttribute('className',attr);
+}
+
+function createResults()
+{
+  var results = document.getElementById("SRResults");
+  for (var e=0; e<searchData.length; e++)
+  {
+    var id = searchData[e][0];
+    var srResult = document.createElement('div');
+    srResult.setAttribute('id','SR_'+id);
+    setClassAttr(srResult,'SRResult');
+    var srEntry = document.createElement('div');
+    setClassAttr(srEntry,'SREntry');
+    var srLink = document.createElement('a');
+    srLink.setAttribute('id','Item'+e);
+    setKeyActions(srLink,'return searchResults.Nav(event,'+e+')');
+    setClassAttr(srLink,'SRSymbol');
+    srLink.innerHTML = searchData[e][1][0];
+    srEntry.appendChild(srLink);
+    if (searchData[e][1].length==2) // single result
+    {
+      srLink.setAttribute('href',searchData[e][1][1][0]);
+      if (searchData[e][1][1][1])
+      {
+       srLink.setAttribute('target','_parent');
+      }
+      var srScope = document.createElement('span');
+      setClassAttr(srScope,'SRScope');
+      srScope.innerHTML = searchData[e][1][1][2];
+      srEntry.appendChild(srScope);
+    }
+    else // multiple results
+    {
+      srLink.setAttribute('href','javascript:searchResults.Toggle("SR_'+id+'")');
+      var srChildren = document.createElement('div');
+      setClassAttr(srChildren,'SRChildren');
+      for (var c=0; c<searchData[e][1].length-1; c++)
+      {
+        var srChild = document.createElement('a');
+        srChild.setAttribute('id','Item'+e+'_c'+c);
+        setKeyActions(srChild,'return searchResults.NavChild(event,'+e+','+c+')');
+        setClassAttr(srChild,'SRScope');
+        srChild.setAttribute('href',searchData[e][1][c+1][0]);
+        if (searchData[e][1][c+1][1])
+        {
+         srChild.setAttribute('target','_parent');
+        }
+        srChild.innerHTML = searchData[e][1][c+1][2];
+        srChildren.appendChild(srChild);
+      }
+      srEntry.appendChild(srChildren);
+    }
+    srResult.appendChild(srEntry);
+    results.appendChild(srResult);
+  }
+}
+
+function init_search()
+{
+  var results = document.getElementById("MSearchSelectWindow");
+  for (var key in indexSectionLabels)
+  {
+    var link = document.createElement('a');
+    link.setAttribute('class','SelectItem');
+    link.setAttribute('onclick','searchBox.OnSelectItem('+key+')');
+    link.href='javascript:void(0)';
+    link.innerHTML='<span class="SelectionMark">&#160;</span>'+indexSectionLabels[key];
+    results.appendChild(link);
+  }
+  searchBox.OnSelectItem(0);
+}
+

html/search/searchdata.js

@@ -0,0 +1,12 @@
+var indexSectionsWithContent =
+{
+};
+
+var indexSectionNames =
+{
+};
+
+var indexSectionLabels =
+{
+};
+

index.html

@@ -0,0 +1,11 @@
+<!DOCTYPE HTML>
+<html lang="en-US">
+    <head>
+       <meta charset="UTF-8">
+        <meta http-equiv="refresh" content="1;url=html/index.html">
+        <title>Page Redirection</title>
+    </head>
+    <body>
+        If you are not redirected automatically, follow the <a href="html/index.html">link to the documentation</a>
+    </body>
+</html>

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

@@ -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[0].equals("Orientation:")) ) 
-    {
-      roll  = float(list[1]);
-      pitch = float(list[2]);
-      yaw   = float(list[3]);
-      buffer = incoming;
-    }
-    if ( (list.length > 0) && (list[0].equals("Alt:")) ) 
-    {
-      alt  = float(list[1]);
-      buffer = incoming;
-    }
-    if ( (list.length > 0) && (list[0].equals("Temp:")) ) 
-    {
-      temp  = float(list[1]);
-      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/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
-

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_Sensor_Set.h

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

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