Added MadgwickAHRS

2022-12-13 22:37:57 +08:00 · 2022-12-13 22:37:57 +08:00 · 5e541cd726
commit 5e541cd726
parent 69c57a237a
14 changed files with 868 additions and 0 deletions
--- a/lib/MadgwickAHRS/.codespellrc
+++ b/lib/MadgwickAHRS/.codespellrc
@ -0,0 +1,7 @@
+# See: https://github.com/codespell-project/codespell#using-a-config-file
+[codespell]
+# In the event of a false positive, add the problematic word, in all lowercase, to a comma-separated list here:
+ignore-words-list = ,
+check-filenames =
+check-hidden =
+skip = ./.git
--- a/lib/MadgwickAHRS/.github/dependabot.yml
+++ b/lib/MadgwickAHRS/.github/dependabot.yml
@ -0,0 +1,12 @@
+# See: https://docs.github.com/en/github/administering-a-repository/configuration-options-for-dependency-updates#about-the-dependabotyml-file
+version: 2
+
+updates:
+  # Configure check for outdated GitHub Actions actions in workflows.
+  # See: https://docs.github.com/en/github/administering-a-repository/keeping-your-actions-up-to-date-with-dependabot
+  - package-ecosystem: github-actions
+    directory: / # Check the repository's workflows under /.github/workflows/
+    schedule:
+      interval: daily
+    labels:
+      - "topic: infrastructure"
--- a/lib/MadgwickAHRS/.github/workflows/check-arduino.yml
+++ b/lib/MadgwickAHRS/.github/workflows/check-arduino.yml
@ -0,0 +1,28 @@
+name: Check Arduino
+
+# See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows
+on:
+  push:
+  pull_request:
+  schedule:
+    # Run every Tuesday at 8 AM UTC to catch breakage caused by new rules added to Arduino Lint.
+    - cron: "0 8 * * TUE"
+  workflow_dispatch:
+  repository_dispatch:
+
+jobs:
+  lint:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v3
+
+      - name: Arduino Lint
+        uses: arduino/arduino-lint-action@v1
+        with:
+          compliance: specification
+          library-manager: update
+          # Always use this setting for official repositories. Remove for 3rd party projects.
+          official: true
+          project-type: library
--- a/lib/MadgwickAHRS/.github/workflows/compile-examples.yml
+++ b/lib/MadgwickAHRS/.github/workflows/compile-examples.yml
@ -0,0 +1,65 @@
+name: Compile Examples
+
+# See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows
+on:
+  push:
+    paths:
+      - ".github/workflows/compile-examples.yml"
+      - "library.properties"
+      - "examples/**"
+      - "src/**"
+  pull_request:
+    paths:
+      - ".github/workflows/compile-examples.yml"
+      - "library.properties"
+      - "examples/**"
+      - "src/**"
+  schedule:
+    # Run every Tuesday at 8 AM UTC to catch breakage caused by changes to external resources (libraries, platforms).
+    - cron: "0 8 * * TUE"
+  workflow_dispatch:
+  repository_dispatch:
+
+jobs:
+  build:
+    name: ${{ matrix.board.fqbn }}
+    runs-on: ubuntu-latest
+
+    env:
+      SKETCHES_REPORTS_PATH: sketches-reports
+
+    strategy:
+      fail-fast: false
+
+      matrix:
+        board:
+          - fqbn: Intel:arc32:arduino_101
+            platforms: |
+              - name: Intel:arc32
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v3
+
+      - name: Compile examples
+        uses: arduino/compile-sketches@v1
+        with:
+          github-token: ${{ secrets.GITHUB_TOKEN }}
+          fqbn: ${{ matrix.board.fqbn }}
+          platforms: ${{ matrix.board.platforms }}
+          libraries: |
+            # Install the library from the local path.
+            - source-path: ./
+            # Additional library dependencies can be listed here.
+            # See: https://github.com/arduino/compile-sketches#libraries
+          sketch-paths: |
+            - examples
+          enable-deltas-report: true
+          sketches-report-path: ${{ env.SKETCHES_REPORTS_PATH }}
+
+      - name: Save sketches report as workflow artifact
+        uses: actions/upload-artifact@v3
+        with:
+          if-no-files-found: error
+          path: ${{ env.SKETCHES_REPORTS_PATH }}
+          name: ${{ env.SKETCHES_REPORTS_PATH }}
--- a/lib/MadgwickAHRS/.github/workflows/report-size-deltas.yml
+++ b/lib/MadgwickAHRS/.github/workflows/report-size-deltas.yml
@ -0,0 +1,24 @@
+name: Report Size Deltas
+
+# See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows
+on:
+  push:
+    paths:
+      - ".github/workflows/report-size-deltas.yml"
+  schedule:
+    # Run at the minimum interval allowed by GitHub Actions.
+    # Note: GitHub Actions periodically has outages which result in workflow failures.
+    # In this event, the workflows will start passing again once the service recovers.
+    - cron: "*/5 * * * *"
+  workflow_dispatch:
+  repository_dispatch:
+
+jobs:
+  report:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Comment size deltas reports to PRs
+        uses: arduino/report-size-deltas@v1
+        with:
+          # The name of the workflow artifact created by the sketch compilation workflow
+          sketches-reports-source: sketches-reports
--- a/lib/MadgwickAHRS/.github/workflows/spell-check.yml
+++ b/lib/MadgwickAHRS/.github/workflows/spell-check.yml
@ -0,0 +1,22 @@
+name: Spell Check
+
+# See: https://docs.github.com/en/free-pro-team@latest/actions/reference/events-that-trigger-workflows
+on:
+  push:
+  pull_request:
+  schedule:
+    # Run every Tuesday at 8 AM UTC to catch new misspelling detections resulting from dictionary updates.
+    - cron: "0 8 * * TUE"
+  workflow_dispatch:
+  repository_dispatch:
+
+jobs:
+  spellcheck:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v3
+
+      - name: Spell check
+        uses: codespell-project/actions-codespell@master
--- a/lib/MadgwickAHRS/.github/workflows/sync-labels.yml
+++ b/lib/MadgwickAHRS/.github/workflows/sync-labels.yml
@ -0,0 +1,138 @@
+# Source: https://github.com/arduino/tooling-project-assets/blob/main/workflow-templates/sync-labels.md
+name: Sync Labels
+
+# See: https://docs.github.com/en/actions/reference/events-that-trigger-workflows
+on:
+  push:
+    paths:
+      - ".github/workflows/sync-labels.ya?ml"
+      - ".github/label-configuration-files/*.ya?ml"
+  pull_request:
+    paths:
+      - ".github/workflows/sync-labels.ya?ml"
+      - ".github/label-configuration-files/*.ya?ml"
+  schedule:
+    # Run daily at 8 AM UTC to sync with changes to shared label configurations.
+    - cron: "0 8 * * *"
+  workflow_dispatch:
+  repository_dispatch:
+
+env:
+  CONFIGURATIONS_FOLDER: .github/label-configuration-files
+  CONFIGURATIONS_ARTIFACT: label-configuration-files
+
+jobs:
+  check:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v3
+
+      - name: Download JSON schema for labels configuration file
+        id: download-schema
+        uses: carlosperate/download-file-action@v2
+        with:
+          file-url: https://raw.githubusercontent.com/arduino/tooling-project-assets/main/workflow-templates/assets/sync-labels/arduino-tooling-gh-label-configuration-schema.json
+          location: ${{ runner.temp }}/label-configuration-schema
+
+      - name: Install JSON schema validator
+        run: |
+          sudo npm install \
+            --global \
+            ajv-cli \
+            ajv-formats
+
+      - name: Validate local labels configuration
+        run: |
+          # See: https://github.com/ajv-validator/ajv-cli#readme
+          ajv validate \
+            --all-errors \
+            -c ajv-formats \
+            -s "${{ steps.download-schema.outputs.file-path }}" \
+            -d "${{ env.CONFIGURATIONS_FOLDER }}/*.{yml,yaml}"
+
+  download:
+    needs: check
+    runs-on: ubuntu-latest
+
+    strategy:
+      matrix:
+        filename:
+          # Filenames of the shared configurations to apply to the repository in addition to the local configuration.
+          # https://github.com/arduino/tooling-project-assets/blob/main/workflow-templates/assets/sync-labels
+          - universal.yml
+
+    steps:
+      - name: Download
+        uses: carlosperate/download-file-action@v2
+        with:
+          file-url: https://raw.githubusercontent.com/arduino/tooling-project-assets/main/workflow-templates/assets/sync-labels/${{ matrix.filename }}
+
+      - name: Pass configuration files to next job via workflow artifact
+        uses: actions/upload-artifact@v3
+        with:
+          path: |
+            *.yaml
+            *.yml
+          if-no-files-found: error
+          name: ${{ env.CONFIGURATIONS_ARTIFACT }}
+
+  sync:
+    needs: download
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Set environment variables
+        run: |
+          # See: https://docs.github.com/en/actions/reference/workflow-commands-for-github-actions#setting-an-environment-variable
+          echo "MERGED_CONFIGURATION_PATH=${{ runner.temp }}/labels.yml" >> "$GITHUB_ENV"
+
+      - name: Determine whether to dry run
+        id: dry-run
+        if: >
+          github.event_name == 'pull_request' ||
+          (
+            (
+              github.event_name == 'push' ||
+              github.event_name == 'workflow_dispatch'
+            ) &&
+            github.ref != format('refs/heads/{0}', github.event.repository.default_branch)
+          )
+        run: |
+          # Use of this flag in the github-label-sync command will cause it to only check the validity of the
+          # configuration.
+          echo "::set-output name=flag::--dry-run"
+
+      - name: Checkout repository
+        uses: actions/checkout@v3
+
+      - name: Download configuration files artifact
+        uses: actions/download-artifact@v3
+        with:
+          name: ${{ env.CONFIGURATIONS_ARTIFACT }}
+          path: ${{ env.CONFIGURATIONS_FOLDER }}
+
+      - name: Remove unneeded artifact
+        uses: geekyeggo/delete-artifact@v2
+        with:
+          name: ${{ env.CONFIGURATIONS_ARTIFACT }}
+
+      - name: Merge label configuration files
+        run: |
+          # Merge all configuration files
+          shopt -s extglob
+          cat "${{ env.CONFIGURATIONS_FOLDER }}"/*.@(yml|yaml) > "${{ env.MERGED_CONFIGURATION_PATH }}"
+
+      - name: Install github-label-sync
+        run: sudo npm install --global github-label-sync
+
+      - name: Sync labels
+        env:
+          GITHUB_ACCESS_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+        run: |
+          # See: https://github.com/Financial-Times/github-label-sync
+          github-label-sync \
+            --labels "${{ env.MERGED_CONFIGURATION_PATH }}" \
+            ${{ steps.dry-run.outputs.flag }} \
+            ${{ github.repository }}
--- a/lib/MadgwickAHRS/README.adoc
+++ b/lib/MadgwickAHRS/README.adoc
@ -0,0 +1,37 @@
+:repository-owner: arduino-libraries
+:repository-name: MadgwickAHRS
+
+= Madgwick Library =
+
+image:https://github.com/{repository-owner}/{repository-name}/actions/workflows/check-arduino.yml/badge.svg["Check Arduino status", link="https://github.com/{repository-owner}/{repository-name}/actions/workflows/check-arduino.yml"]
+image:https://github.com/{repository-owner}/{repository-name}/actions/workflows/compile-examples.yml/badge.svg["Compile Examples status", link="https://github.com/{repository-owner}/{repository-name}/actions/workflows/compile-examples.yml"]
+image:https://github.com/{repository-owner}/{repository-name}/actions/workflows/spell-check.yml/badge.svg["Spell Check status", link="https://github.com/{repository-owner}/{repository-name}/actions/workflows/spell-check.yml"]
+
+This library wraps the official implementation of MadgwickAHRS algorithm to get orientation of an object based on accelerometer and gyroscope readings
+
+== License ==
+
+Copyright (c) Arduino LLC. All right reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+
+
+Implementation of Madgwick's IMU and AHRS algorithms.
+See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+
+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
--- a/lib/MadgwickAHRS/examples/Visualize101/Visualize101.ino
+++ b/lib/MadgwickAHRS/examples/Visualize101/Visualize101.ino
@ -0,0 +1,85 @@
+#include <CurieIMU.h>
+#include <MadgwickAHRS.h>
+
+Madgwick filter;
+unsigned long microsPerReading, microsPrevious;
+float accelScale, gyroScale;
+
+void setup() {
+  Serial.begin(9600);
+
+  // start the IMU and filter
+  CurieIMU.begin();
+  CurieIMU.setGyroRate(25);
+  CurieIMU.setAccelerometerRate(25);
+  filter.begin(25);
+
+  // Set the accelerometer range to 2 g
+  CurieIMU.setAccelerometerRange(2);
+  // Set the gyroscope range to 250 degrees/second
+  CurieIMU.setGyroRange(250);
+
+  // initialize variables to pace updates to correct rate
+  microsPerReading = 1000000 / 25;
+  microsPrevious = micros();
+}
+
+void loop() {
+  int aix, aiy, aiz;
+  int gix, giy, giz;
+  float ax, ay, az;
+  float gx, gy, gz;
+  float roll, pitch, heading;
+  unsigned long microsNow;
+
+  // check if it's time to read data and update the filter
+  microsNow = micros();
+  if (microsNow - microsPrevious >= microsPerReading) {
+
+    // read raw data from CurieIMU
+    CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);
+
+    // convert from raw data to gravity and degrees/second units
+    ax = convertRawAcceleration(aix);
+    ay = convertRawAcceleration(aiy);
+    az = convertRawAcceleration(aiz);
+    gx = convertRawGyro(gix);
+    gy = convertRawGyro(giy);
+    gz = convertRawGyro(giz);
+
+    // update the filter, which computes orientation
+    filter.updateIMU(gx, gy, gz, ax, ay, az);
+
+    // 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);
+
+    // increment previous time, so we keep proper pace
+    microsPrevious = microsPrevious + microsPerReading;
+  }
+}
+
+float convertRawAcceleration(int aRaw) {
+  // since we are using 2 g range
+  // -2 g maps to a raw value of -32768
+  // +2 g maps to a raw value of 32767
+  
+  float a = (aRaw * 2.0) / 32768.0;
+  return a;
+}
+
+float convertRawGyro(int gRaw) {
+  // since we are using 250 degrees/seconds range
+  // -250 maps to a raw value of -32768
+  // +250 maps to a raw value of 32767
+  
+  float g = (gRaw * 250.0) / 32768.0;
+  return g;
+}
--- a/lib/MadgwickAHRS/extras/Visualizer/Visualizer.pde
+++ b/lib/MadgwickAHRS/extras/Visualizer/Visualizer.pde
@ -0,0 +1,91 @@
+import processing.serial.*;
+Serial myPort;
+
+float yaw = 0.0;
+float pitch = 0.0;
+float roll = 0.0;
+
+void setup()
+{
+  size(600, 500, P3D);
+
+  // if you have only ONE serial port active
+  myPort = new Serial(this, Serial.list()[0], 9600); // if you have only ONE serial port active
+
+  // if you know the serial port name
+  //myPort = new Serial(this, "COM5:", 9600);                    // Windows
+  //myPort = new Serial(this, "/dev/ttyACM0", 9600);             // Linux
+  //myPort = new Serial(this, "/dev/cu.usbmodem1217321", 9600);  // Mac
+
+  textSize(16); // set text size
+  textMode(SHAPE); // set text mode to shape
+}
+
+void draw()
+{
+  serialEvent();  // read and parse incoming serial message
+  background(255); // set background to white
+  lights();
+
+  translate(width/2, height/2); // set position to centre
+
+  pushMatrix(); // begin object
+
+  float c1 = cos(radians(roll));
+  float s1 = sin(radians(roll));
+  float c2 = cos(radians(pitch));
+  float s2 = sin(radians(pitch));
+  float c3 = cos(radians(yaw));
+  float s3 = sin(radians(yaw));
+  applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
+               -s2, c1*c2, c2*s1, 0,
+               c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
+               0, 0, 0, 1);
+
+  drawArduino();
+
+  popMatrix(); // end of object
+
+  // Print values to console
+  print(roll);
+  print("\t");
+  print(pitch);
+  print("\t");
+  print(yaw);
+  println();
+}
+
+void serialEvent()
+{
+  int newLine = 13; // new line character in ASCII
+  String message;
+  do {
+    message = myPort.readStringUntil(newLine); // read from port until new line
+    if (message != null) {
+      String[] list = split(trim(message), " ");
+      if (list.length >= 4 && list[0].equals("Orientation:")) {
+        yaw = float(list[1]); // convert to float yaw
+        pitch = float(list[2]); // convert to float pitch
+        roll = float(list[3]); // convert to float roll
+      }
+    }
+  } while (message != null);
+}
+
+void drawArduino()
+{
+  /* function contains shape(s) that are rotated with the IMU */
+  stroke(0, 90, 90); // set outline colour to darker teal
+  fill(0, 130, 130); // set fill colour to lighter teal
+  box(300, 10, 200); // draw Arduino board base shape
+
+  stroke(0); // set outline colour to black
+  fill(80); // set fill colour to dark grey
+
+  translate(60, -10, 90); // set position to edge of Arduino box
+  box(170, 20, 10); // draw pin header as box
+
+  translate(-20, 0, -180); // set position to other edge of Arduino box
+  box(210, 20, 10); // draw other pin header as box
+}
+
--- a/lib/MadgwickAHRS/keywords.txt
+++ b/lib/MadgwickAHRS/keywords.txt
@ -0,0 +1,25 @@
+#######################################
+# Syntax Coloring Map For MadgwickAHRS
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+Madgwick	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+update	KEYWORD2
+updateIMU	KEYWORD2
+getPitch	KEYWORD2
+getYaw	KEYWORD2
+getRoll	KEYWORD2
+
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
--- a/lib/MadgwickAHRS/library.properties
+++ b/lib/MadgwickAHRS/library.properties
@ -0,0 +1,9 @@
+name=Madgwick
+version=1.2.0
+author=Arduino
+maintainer=Arduino <info@arduino.cc>
+sentence=Helpers for MadgwickAHRS algorithm
+paragraph=This library wraps the official implementation of MadgwickAHRS algorithm to get orientation of an object based on accelerometer and gyroscope readings
+category=Data Processing
+url=https://github.com/arduino-libraries/MadgwickAHRS
+architectures=*
--- a/lib/MadgwickAHRS/src/MadgwickAHRS.cpp
+++ b/lib/MadgwickAHRS/src/MadgwickAHRS.cpp
@ -0,0 +1,251 @@
+//=============================================================================================
+// MadgwickAHRS.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 "MadgwickAHRS.h"
+#include <math.h>
+
+//-------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreqDef   512.0f          // sample frequency in Hz
+#define betaDef         0.1f            // 2 * proportional gain
+
+
+//============================================================================================
+// Functions
+
+//-------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+Madgwick::Madgwick() {
+	beta = betaDef;
+	q0 = 1.0f;
+	q1 = 0.0f;
+	q2 = 0.0f;
+	q3 = 0.0f;
+	invSampleFreq = 1.0f / sampleFreqDef;
+	anglesComputed = 0;
+}
+
+void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+	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);
+		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 * invSampleFreq;
+	q1 += qDot2 * invSampleFreq;
+	q2 += qDot3 * invSampleFreq;
+	q3 += qDot4 * invSampleFreq;
+
+	// 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 Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+	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 * invSampleFreq;
+	q1 += qDot2 * invSampleFreq;
+	q2 += qDot3 * invSampleFreq;
+	q3 += qDot4 * invSampleFreq;
+
+	// Normalise quaternion
+	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+	q0 *= recipNorm;
+	q1 *= recipNorm;
+	q2 *= recipNorm;
+	q3 *= recipNorm;
+	anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+float Madgwick::invSqrt(float x) {
+	float halfx = 0.5f * x;
+	float y = x;
+	long i = *(long*)&y;
+	i = 0x5f3759df - (i>>1);
+	y = *(float*)&i;
+	y = y * (1.5f - (halfx * y * y));
+	y = y * (1.5f - (halfx * y * y));
+	return y;
+}
+
+//-------------------------------------------------------------------------------------------
+
+void 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);
+	anglesComputed = 1;
+}
+
--- a/lib/MadgwickAHRS/src/MadgwickAHRS.h
+++ b/lib/MadgwickAHRS/src/MadgwickAHRS.h
@ -0,0 +1,74 @@
+//=============================================================================================
+// MadgwickAHRS.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 MadgwickAHRS_h
+#define MadgwickAHRS_h
+#include <math.h>
+
+//--------------------------------------------------------------------------------------------
+// Variable declaration
+class Madgwick{
+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;
+    float pitch;
+    float yaw;
+    char anglesComputed;
+    void computeAngles();
+
+//-------------------------------------------------------------------------------------------
+// Function declarations
+public:
+    Madgwick(void);
+    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
+    void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+    void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+    //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 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;
+    }
+};
+#endif
+