From 5e541cd726de7e25f5c2ff6053e6230e45210360 Mon Sep 17 00:00:00 2001 From: lewis Date: Tue, 13 Dec 2022 22:37:57 +0800 Subject: [PATCH] Added MadgwickAHRS --- lib/MadgwickAHRS/.codespellrc | 7 + lib/MadgwickAHRS/.github/dependabot.yml | 12 + .../.github/workflows/check-arduino.yml | 28 ++ .../.github/workflows/compile-examples.yml | 65 +++++ .../.github/workflows/report-size-deltas.yml | 24 ++ .../.github/workflows/spell-check.yml | 22 ++ .../.github/workflows/sync-labels.yml | 138 ++++++++++ lib/MadgwickAHRS/README.adoc | 37 +++ .../examples/Visualize101/Visualize101.ino | 85 ++++++ .../extras/Visualizer/Visualizer.pde | 91 +++++++ lib/MadgwickAHRS/keywords.txt | 25 ++ lib/MadgwickAHRS/library.properties | 9 + lib/MadgwickAHRS/src/MadgwickAHRS.cpp | 251 ++++++++++++++++++ lib/MadgwickAHRS/src/MadgwickAHRS.h | 74 ++++++ 14 files changed, 868 insertions(+) create mode 100644 lib/MadgwickAHRS/.codespellrc create mode 100644 lib/MadgwickAHRS/.github/dependabot.yml create mode 100644 lib/MadgwickAHRS/.github/workflows/check-arduino.yml create mode 100644 lib/MadgwickAHRS/.github/workflows/compile-examples.yml create mode 100644 lib/MadgwickAHRS/.github/workflows/report-size-deltas.yml create mode 100644 lib/MadgwickAHRS/.github/workflows/spell-check.yml create mode 100644 lib/MadgwickAHRS/.github/workflows/sync-labels.yml create mode 100644 lib/MadgwickAHRS/README.adoc create mode 100644 lib/MadgwickAHRS/examples/Visualize101/Visualize101.ino create mode 100644 lib/MadgwickAHRS/extras/Visualizer/Visualizer.pde create mode 100644 lib/MadgwickAHRS/keywords.txt create mode 100644 lib/MadgwickAHRS/library.properties create mode 100644 lib/MadgwickAHRS/src/MadgwickAHRS.cpp create mode 100644 lib/MadgwickAHRS/src/MadgwickAHRS.h diff --git a/lib/MadgwickAHRS/.codespellrc b/lib/MadgwickAHRS/.codespellrc new file mode 100644 index 0000000..101edae --- /dev/null +++ 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 diff --git a/lib/MadgwickAHRS/.github/dependabot.yml b/lib/MadgwickAHRS/.github/dependabot.yml new file mode 100644 index 0000000..fa738ec --- /dev/null +++ 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" diff --git a/lib/MadgwickAHRS/.github/workflows/check-arduino.yml b/lib/MadgwickAHRS/.github/workflows/check-arduino.yml new file mode 100644 index 0000000..3e0d26c --- /dev/null +++ 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 diff --git a/lib/MadgwickAHRS/.github/workflows/compile-examples.yml b/lib/MadgwickAHRS/.github/workflows/compile-examples.yml new file mode 100644 index 0000000..269bbf5 --- /dev/null +++ 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 }} diff --git a/lib/MadgwickAHRS/.github/workflows/report-size-deltas.yml b/lib/MadgwickAHRS/.github/workflows/report-size-deltas.yml new file mode 100644 index 0000000..652be5d --- /dev/null +++ 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 diff --git a/lib/MadgwickAHRS/.github/workflows/spell-check.yml b/lib/MadgwickAHRS/.github/workflows/spell-check.yml new file mode 100644 index 0000000..3f6b03f --- /dev/null +++ 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 diff --git a/lib/MadgwickAHRS/.github/workflows/sync-labels.yml b/lib/MadgwickAHRS/.github/workflows/sync-labels.yml new file mode 100644 index 0000000..94938f3 --- /dev/null +++ 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 }} diff --git a/lib/MadgwickAHRS/README.adoc b/lib/MadgwickAHRS/README.adoc new file mode 100644 index 0000000..774aff4 --- /dev/null +++ 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 diff --git a/lib/MadgwickAHRS/examples/Visualize101/Visualize101.ino b/lib/MadgwickAHRS/examples/Visualize101/Visualize101.ino new file mode 100644 index 0000000..b3f8c79 --- /dev/null +++ b/lib/MadgwickAHRS/examples/Visualize101/Visualize101.ino @@ -0,0 +1,85 @@ +#include +#include + +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; +} diff --git a/lib/MadgwickAHRS/extras/Visualizer/Visualizer.pde b/lib/MadgwickAHRS/extras/Visualizer/Visualizer.pde new file mode 100644 index 0000000..4915d3b --- /dev/null +++ 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 +} + diff --git a/lib/MadgwickAHRS/keywords.txt b/lib/MadgwickAHRS/keywords.txt new file mode 100644 index 0000000..9ad144c --- /dev/null +++ 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) +####################################### + diff --git a/lib/MadgwickAHRS/library.properties b/lib/MadgwickAHRS/library.properties new file mode 100644 index 0000000..57e03dd --- /dev/null +++ b/lib/MadgwickAHRS/library.properties @@ -0,0 +1,9 @@ +name=Madgwick +version=1.2.0 +author=Arduino +maintainer=Arduino +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=* diff --git a/lib/MadgwickAHRS/src/MadgwickAHRS.cpp b/lib/MadgwickAHRS/src/MadgwickAHRS.cpp new file mode 100644 index 0000000..4a941c1 --- /dev/null +++ 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 + +//------------------------------------------------------------------------------------------- +// 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; +} + diff --git a/lib/MadgwickAHRS/src/MadgwickAHRS.h b/lib/MadgwickAHRS/src/MadgwickAHRS.h new file mode 100644 index 0000000..6a82f7d --- /dev/null +++ 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 + +//-------------------------------------------------------------------------------------------- +// 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 +