adafruit-mirror/Adafruit_AHRS
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

full refactor (#7)

Browse source 
* moved files and added NXP fusion

* rename and make unique names

* renamed files and made a multi-filter tester

* USB and Mahony examples are now superceded by one calibrated_orientation example

* add lsm9ds0 fusion demo to calibrated_orientation and removed standalone version
also moving really old 10dof/9dof demos

* make one unified calibrator

* add CI

* add some deps

* add more deps

* more deps

* update naming

* more deps

* skip and dep

* manually add conversion const

* more fixfix

* change default to mahony

* fix skips

* clangclang

* moreclang
This commit is contained in:
Limor "Ladyada" Fried 2020-02-02 13:26:56 -05:00 committed by GitHub
parent a01de978b6
commit e07ec313ef
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
43 changed files with 3526 additions and 1436 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

37
.github/workflows/githubci.yml vendored Normal file
View file

@ -0,0 +1,37 @@
name: Arduino Library CI
on: [pull_request, push, repository_dispatch]
jobs:
build:
runs-on: ubuntu-latest
steps:
- uses: actions/setup-python@v1
with:
python-version: '3.x'
- uses: actions/checkout@v2
- uses: actions/checkout@v2
with:
repository: adafruit/ci-arduino
path: ci
- name: pre-install
run: bash ci/actions_install.sh
# manually install calib
- name: extra libraries
run: |
git clone --quiet https://github.com/adafruit/Adafruit_Sensor_Calibration.git /home/runner/Arduino/libraries/Adafruit_Sensor_Calibration
- name: test platforms
run: python3 ci/build_platform.py main_platforms
- 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

72
Adafruit_Simple_AHRS.cpp
View file

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

21
Adafruit_Simple_AHRS.h
View file

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

250
Madgwick.cpp
View file

@ -1,250 +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 "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
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;
}

79
Madgwick.h
View file

@ -1,79 +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 __Madgwick_h__
#define __Madgwick_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;
}
void getQuaternion(float *w, float *x, float *y, float *z) {
*w = q0;
*x = q1;
*y = q2;
*z = q3;
}
};
#endif

274
Mahony.cpp
View file

@ -1,274 +0,0 @@
//=============================================================================================
// 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 "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
Mahony::Mahony()
{
twoKp = twoKpDef; // 2 * proportional gain (Kp)
twoKi = twoKiDef; // 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 = 0;
invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
}
void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
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 * invSampleFreq;
integralFBy += twoKi * halfey * invSampleFreq;
integralFBz += twoKi * halfez * invSampleFreq;
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 * invSampleFreq); // pre-multiply common factors
gy *= (0.5f * invSampleFreq);
gz *= (0.5f * invSampleFreq);
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 Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
{
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 * invSampleFreq;
integralFBy += twoKi * halfey * invSampleFreq;
integralFBz += twoKi * halfez * invSampleFreq;
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 * invSampleFreq); // pre-multiply common factors
gy *= (0.5f * invSampleFreq);
gz *= (0.5f * invSampleFreq);
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;
}
//-------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float Mahony::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 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);
anglesComputed = 1;
}
//============================================================================================
// END OF CODE
//============================================================================================

72
Mahony.h
View file

@ -1,72 +0,0 @@
//=============================================================================================
// 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 __Mahony_h__
#define __Mahony_h__
#include <math.h>
//--------------------------------------------------------------------------------------------
// Variable declaration
class Mahony {
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;
char anglesComputed;
static float invSqrt(float x);
void computeAngles();
//-------------------------------------------------------------------------------------------
// Function declarations
public:
Mahony();
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 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;
}
};
#endif

86
examples/ahrs_calibration/ahrs_calibration.ino
View file

@ -1,86 +0,0 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_LSM303_U.h>
// Create sensor instances.
Adafruit_L3GD20_Unified gyro(20);
Adafruit_LSM303_Accel_Unified accel(30301);
Adafruit_LSM303_Mag_Unified mag(30302);
// This sketch can be used to output raw sensor data in a format that
// can be understoof by MotionCal from PJRC. Download the application
// from http://www.pjrc.com/store/prop_shield.html and make note of the
// magentic offsets after rotating the sensors sufficiently.
//
// You should end up with 3 offsets for X/Y/Z, which are displayed
// in the top-right corner of the application.
//
// Make sure you have the latest versions of the Adfruit_L3GD20_U and
// Adafruit_LSM303_U libraries when running this sketch since they
// use a new .raw field added in the latest versions (Oct 10 2016)
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 10 DOF Board AHRS Calibration Example")); Serial.println("");
// Initialize the sensors.
if(!gyro.begin())
{
/* There was a problem detecting the L3GD20 ... check your connections */
Serial.println("Ooops, no L3GD20 detected ... Check your wiring!");
while(1);
}
if(!accel.begin())
{
/* There was a problem detecting the LSM303DLHC ... check your connections */
Serial.println("Ooops, no L3M303DLHC accel detected ... Check your wiring!");
while(1);
}
if(!mag.begin())
{
/* There was a problem detecting the LSM303DLHC ... check your connections */
Serial.println("Ooops, no L3M303DLHC mag detected ... Check your wiring!");
while(1);
}
}
void loop(void)
{
sensors_event_t event; // Need to read raw data, which is stored at the same time
// Get new data samples
gyro.getEvent(&event);
accel.getEvent(&event);
mag.getEvent(&event);
// Print the sensor data
Serial.print("Raw:");
Serial.print(accel.raw.x);
Serial.print(',');
Serial.print(accel.raw.y);
Serial.print(',');
Serial.print(accel.raw.z);
Serial.print(',');
Serial.print(gyro.raw.x);
Serial.print(',');
Serial.print(gyro.raw.y);
Serial.print(',');
Serial.print(gyro.raw.z);
Serial.print(',');
Serial.print(mag.raw.x);
Serial.print(',');
Serial.print(mag.raw.y);
Serial.print(',');
Serial.print(mag.raw.z);
Serial.println();
delay(50);
}

0
examples/ahrs_calibration_ble_nrf51/.uno.test.only Normal file
View file

62
examples/ahrs_calibration_ble_nrf51/ArdPrintf.h
View file

@ -1,50 +1,50 @@
#ifndef ARDPRINTF
#define ARDPRINTF
#define ARDBUFFER 20
#include <stdarg.h>
#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++;
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]=='%')
{
for (i = 0, j = 0; str[i] != '\0'; i++) {
if (str[i] == '%') {
temp[j] = '\0';
Serial.print(temp);
j=0;
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: ;
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
{
} else {
temp[j] = str[i];
j = (j+1)%ARDBUFFER;
if(j==0)
{
j = (j + 1) % ARDBUFFER;
if (j == 0) {
temp[ARDBUFFER] = '\0';
Serial.print(temp);
temp[0]='\0';
temp[0] = '\0';
}
}
};

35
examples/ahrs_calibration_ble_nrf51/BluefruitConfig.h
View file

@ -2,37 +2,34 @@
// ----------------------------------------------------------------------------------------------
// 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
#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
#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
#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
#define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused
// SHARED SPI SETTINGS
// ----------------------------------------------------------------------------------------------
@ -41,9 +38,9 @@
// 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
#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
// ----------------------------------------------------------------------------------------------
@ -51,6 +48,6 @@
// 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
#define BLUEFRUIT_SPI_SCK 13
#define BLUEFRUIT_SPI_MISO 12
#define BLUEFRUIT_SPI_MOSI 11

140
examples/ahrs_calibration_usb/ahrs_calibration_usb.ino
View file

@ -1,140 +0,0 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#define ST_LSM303DLHC_L3GD20 (0)
#define ST_LSM9DS1 (1)
#define NXP_FXOS8700_FXAS21002 (2)
// Define your target sensor(s) here based on the list above!
// #define AHRS_VARIANT ST_LSM303DLHC_L3GD20
#define AHRS_VARIANT NXP_FXOS8700_FXAS21002
// Include appropriate sensor driver(s)
#if AHRS_VARIANT == ST_LSM303DLHC_L3GD20
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_LSM303_U.h>
#elif AHRS_VARIANT == ST_LSM9DS1
// ToDo!
#elif AHRS_VARIANT == 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 == ST_LSM303DLHC_L3GD20
Adafruit_L3GD20_Unified gyro(20);
Adafruit_LSM303_Accel_Unified accel(30301);
Adafruit_LSM303_Mag_Unified mag(30302);
#elif AHRS_VARIANT == ST_LSM9DS1
// ToDo!
#elif AHRS_VARIANT == NXP_FXOS8700_FXAS21002
Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
#endif
// NOTE: THIS IS A WORK IN PROGRESS!
// This sketch can be used to output raw sensor data in a format that
// can be understoof by MotionCal from PJRC. Download the application
// from http://www.pjrc.com/store/prop_shield.html and make note of the
// magentic offsets after rotating the sensors sufficiently.
//
// You should end up with 3 offsets for X/Y/Z, which are displayed
// in the top-right corner of the application.
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 gyro detected ... Check your wiring!");
while(1);
}
#if AHRS_VARIANT == 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
}
void loop(void)
{
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 == NXP_FXOS8700_FXAS21002
accelmag.getEvent(&event);
#else
accel.getEvent(&event);
mag.getEvent(&event);
#endif
// Print the sensor data
Serial.print("Raw:");
#if AHRS_VARIANT == 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 == 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
delay(50);
}

0
examples/ahrs_fusion_ble_nrf51/.uno.test.only Normal file
View file

35
examples/ahrs_fusion_ble_nrf51/BluefruitConfig.h
View file

@ -2,37 +2,34 @@
// ----------------------------------------------------------------------------------------------
// 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
#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
#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
#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
#define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused
// SHARED SPI SETTINGS
// ----------------------------------------------------------------------------------------------
@ -41,9 +38,9 @@
// 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
#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
// ----------------------------------------------------------------------------------------------
@ -51,6 +48,6 @@
// 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
#define BLUEFRUIT_SPI_SCK 13
#define BLUEFRUIT_SPI_MISO 12
#define BLUEFRUIT_SPI_MOSI 11

11
examples/ahrs_fusion_ble_nrf51/ahrs_fusion_ble_nrf51.ino
View file

@ -1,8 +1,5 @@
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_Sensor.h>
#include "Mahony.h"
#include "Madgwick.h"
#include "Adafruit_AHRS_Mahony.h"
#include "Adafruit_AHRS_Madgwick.h"
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"
@ -75,8 +72,8 @@ float gyro_zero_offsets[3] = { 175.0F * rawToDPS * dpsToRad,
// Mahony is lighter weight as a filter and should be used
// on slower systems
Mahony filter;
//Madgwick filter;
Adafruit_Mahony filter;
//Adafruit_Madgwick filter;
void setup()
{

167
examples/ahrs_fusion_usb/ahrs_fusion_usb.ino
View file

@ -1,167 +0,0 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Mahony.h>
#include <Madgwick.h>
// Note: This sketch is a WORK IN PROGRESS
#define ST_LSM303DLHC_L3GD20 (0)
#define ST_LSM9DS1 (1)
#define NXP_FXOS8700_FXAS21002 (2)
// Define your target sensor(s) here based on the list above!
// #define AHRS_VARIANT ST_LSM303DLHC_L3GD20
#define AHRS_VARIANT NXP_FXOS8700_FXAS21002
#if AHRS_VARIANT == ST_LSM303DLHC_L3GD20
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_LSM303_U.h>
#elif AHRS_VARIANT == ST_LSM9DS1
// ToDo!
#elif AHRS_VARIANT == 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 == ST_LSM303DLHC_L3GD20
Adafruit_L3GD20_Unified gyro(20);
Adafruit_LSM303_Accel_Unified accel(30301);
Adafruit_LSM303_Mag_Unified mag(30302);
#elif AHRS_VARIANT == ST_LSM9DS1
// ToDo!
#elif AHRS_VARIANT == NXP_FXOS8700_FXAS21002
Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
#endif
// Mag calibration values are calculated via ahrs_calibration.
// These values must be determined for each board/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] = { 0.93F, -7.47F, -35.23F };
// Soft iron error compensation matrix
float mag_softiron_matrix[3][3] = { { 0.943, 0.011, 0.020 },
{ 0.022, 0.918, -0.008 },
{ 0.020, -0.008, 1.156 } };
float mag_field_strength = 50.23F;
// Offsets applied to compensate for gyro zero-drift error for x/y/z
float gyro_zero_offsets[3] = { 0.0F, 0.0F, 0.0F };
// Mahony is lighter weight as a filter and should be used
// on slower systems
Mahony filter;
//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 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 == 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 70 samples per second
// Based on a Bluefruit M0 Feather ... rate should be adjuted for other MCUs
filter.begin(10);
}
void loop(void)
{
sensors_event_t gyro_event;
sensors_event_t accel_event;
sensors_event_t mag_event;
// Get new data samples
gyro.getEvent(&gyro_event);
#if AHRS_VARIANT == 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
// Note: To avoid gimbal lock you should read quaternions not Euler
// angles, but Euler angles are used here since they are easier to
// understand looking at the raw values. See the ble fusion sketch for
// and example of working with quaternion data.
float roll = filter.getRoll();
float pitch = filter.getPitch();
float heading = filter.getYaw();
Serial.print(millis());
Serial.print(" - Orientation: ");
Serial.print(heading);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(roll);
delay(10);
}

BIN
examples/ahrs_fusion_usb/sample_config_data.png
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 214 KiB

72
examples/ahrs_lsm9ds0/ahrs_lsm9ds0.ino
View file

@ -1,72 +0,0 @@
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM9DS0.h>
#include <Adafruit_Simple_AHRS.h>
// Create LSM9DS0 board instance.
Adafruit_LSM9DS0 lsm(1000); // Use I2C, ID #1000
// Create simple AHRS algorithm using the LSM9DS0 instance's accelerometer and magnetometer.
Adafruit_Simple_AHRS ahrs(&lsm.getAccel(), &lsm.getMag());
// Function to configure the sensors on the LSM9DS0 board.
// You don't need to change anything here, but have the option to select different
// range and gain values.
void configureLSM9DS0(void)
{
// 1.) Set the accelerometer range
lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_2G);
//lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_4G);
//lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_6G);
//lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_8G);
//lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_16G);
// 2.) Set the magnetometer sensitivity
lsm.setupMag(lsm.LSM9DS0_MAGGAIN_2GAUSS);
//lsm.setupMag(lsm.LSM9DS0_MAGGAIN_4GAUSS);
//lsm.setupMag(lsm.LSM9DS0_MAGGAIN_8GAUSS);
//lsm.setupMag(lsm.LSM9DS0_MAGGAIN_12GAUSS);
// 3.) Setup the gyroscope
lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_245DPS);
//lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_500DPS);
//lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_2000DPS);
}
void setup(void)
{
Serial.begin(115200);
Serial.println(F("Adafruit LSM9DS0 9 DOF Board AHRS Example")); Serial.println("");
// Initialise the LSM9DS0 board.
if(!lsm.begin())
{
// There was a problem detecting the LSM9DS0 ... check your connections
Serial.print(F("Ooops, no LSM9DS0 detected ... Check your wiring or I2C ADDR!"));
while(1);
}
// Setup the sensor gain and integration time.
configureLSM9DS0();
}
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);
}

120
examples/ahrs_mahony/ahrs_mahony.ino
View file

@ -1,120 +0,0 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_LSM303_U.h>
#include <MahonyAHRS.h>
#include <MadgwickAHRS.h>
// Note: This sketch requires the MahonyAHRS sketch from
// https://github.com/PaulStoffregen/MahonyAHRS, or the
// MagdwickAHRS sketch from https://github.com/PaulStoffregen/MadgwickAHRS
// Note: This sketch is a WORK IN PROGRESS
// Create sensor instances.
Adafruit_L3GD20_Unified gyro(20);
Adafruit_LSM303_Accel_Unified accel(30301);
Adafruit_LSM303_Mag_Unified mag(30302);
// 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 values
float mag_offsets[3] = { -2.20F, -5.53F, -26.34F };
// Soft iron error compensation matrix
float mag_softiron_matrix[3][3] = { { 0.934, 0.005, 0.013 },
{ 0.005, 0.948, 0.012 },
{ 0.013, 0.012, 1.129 } };
float mag_field_strength = 48.41F;
// Mahony is lighter weight as a filter and should be used
// on slower systems
Mahony filter;
//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 10 DOF Board AHRS Calibration Example")); Serial.println("");
// Initialize the sensors.
if(!gyro.begin())
{
/* There was a problem detecting the L3GD20 ... check your connections */
Serial.println("Ooops, no L3GD20 detected ... Check your wiring!");
while(1);
}
if(!accel.begin())
{
/* There was a problem detecting the LSM303DLHC ... check your connections */
Serial.println("Ooops, no L3M303DLHC accel detected ... Check your wiring!");
while(1);
}
if(!mag.begin())
{
/* There was a problem detecting the LSM303DLHC ... check your connections */
Serial.println("Ooops, no L3M303DLHC mag detected ... Check your wiring!");
while(1);
}
// Filter expects 50 samples per second
filter.begin(50);
}
void loop(void)
{
sensors_event_t gyro_event;
sensors_event_t accel_event;
sensors_event_t mag_event;
// Get new data samples
gyro.getEvent(&gyro_event);
accel.getEvent(&accel_event);
mag.getEvent(&mag_event);
// 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];
// 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)
float gx = gyro_event.gyro.x * 57.2958F;
float gy = gyro_event.gyro.y * 57.2958F;
float gz = gyro_event.gyro.z * 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(heading);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(roll);
delay(10);
}

BIN
examples/ahrs_mahony/sample_config_data.png
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 214 KiB

31
examples/calibrated_orientation/LSM6DS_LIS3MDL.h Normal file
View file

@ -0,0 +1,31 @@
#include <Adafruit_LIS3MDL.h>
Adafruit_LIS3MDL lis3mdl;
// Can change this to be LSM6DSOX or whatever ya like
#include <Adafruit_LSM6DS33.h>
Adafruit_LSM6DS33 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);
}

30
examples/calibrated_orientation/LSM9DS.h Normal file
View file

@ -0,0 +1,30 @@
#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
}

20
examples/calibrated_orientation/LSM9DS1.h Normal file
View file

@ -0,0 +1,20 @@
#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);
}

18
examples/calibrated_orientation/NXP_FXOS_FXAS.h Normal file
View file

@ -0,0 +1,18 @@
#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) {}

111
examples/calibrated_orientation/calibrated_orientation.ino Normal file
View file

@ -0,0 +1,111 @@
// 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>
#define SENSORS_RADS_TO_DPS (180.0 / 3.141592653589793238463)
Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
// uncomment one combo 9-DoF!
//#include "LSM6DS_LIS3MDL.h" // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
#include "LSM9DS.h" // LSM9DS1 or LSM9DS0
//#include "NXP_FXOS_FXAS.h" // NXP 9-DoF breakout
Adafruit_Sensor_Calibration cal;
// pick your filter! slower == better quality output
//Adafruit_NXPSensorFusion filter; // slowest
//Adafruit_Madgwick filter; // faster than NXP
Adafruit_Mahony filter; // fastest/smalleset
#define FILTER_UPDATE_RATE_HZ 100
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;
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);
//Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
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);
//Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
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("");
// 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);
Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms");
}

31
examples/calibration/LSM6DS_LIS3MDL.h Normal file
View file

@ -0,0 +1,31 @@
#include <Adafruit_LIS3MDL.h>
Adafruit_LIS3MDL lis3mdl;
// Can change this to be LSM6DSOX or whatever ya like
#include <Adafruit_LSM6DSOX.h>
Adafruit_LSM6DSOX lsm6ds;
bool init_sensors(void) {
if (!lsm6ds.begin_I2C() || !lis3mdl.begin_I2C()) {
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);
}

30
examples/calibration/LSM9DS.h Normal file
View file

@ -0,0 +1,30 @@
//#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
}

18
examples/calibration/NXP_FXOS_FXAS.h Normal file
View file

@ -0,0 +1,18 @@
#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) {}

221
examples/calibration/calibration.ino Normal file
View file

@ -0,0 +1,221 @@
/***************************************************************************
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>
#define SENSORS_RADS_TO_DPS (180.0 / 3.141592653589793238463)
Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
// uncomment one combo 9-DoF!
#include "LSM6DS_LIS3MDL.h" // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
//#include "LSM9DS.h" // LSM9DS1 or LSM9DS0
//#include "NXP_FXOS_FXAS.h" // NXP 9-DoF breakout
Adafruit_Sensor_Calibration cal;
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;
}

0
examples/ahrs_10dof/ahrs_10dof.ino → examples/deprecated/ahrs_10dof/ahrs_10dof.ino
View file

0
examples/ahrs_9dof/ahrs_9dof.ino → examples/deprecated/ahrs_9dof/ahrs_9dof.ino
View file

7
library.properties
View file

@ -1,9 +1,10 @@
name=Adafruit AHRS
version=1.1.4
version=2.0.0
author=Adafruit
maintainer=Adafruit <info@adafruit.com>
sentence=AHRS (Altitude and Heading Reference System) for Adafruit's 9DOF and 10DOF breakouts
paragraph=AHRS (Altitude and Heading Reference System) for Adafruit's 9DOF and 10DOF breakouts
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 LSM303DLHC, Adafruit BMP085 Unified, Adafruit BluefruitLE nRF51, SdFat - Adafruit Fork, ArduinoJson, Adafruit SPIFlash

3
src/Adafruit_AHRS.h Normal file
View file

@ -0,0 +1,3 @@
#include <Adafruit_AHRS_Madgwick.h>
#include <Adafruit_AHRS_Mahony.h>
#include <Adafruit_AHRS_NXPFusion.h>

289
src/Adafruit_AHRS_Madgwick.cpp Normal file
View file

@ -0,0 +1,289 @@
//=============================================================================================
// 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() {
beta = betaDef;
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
invSampleFreq = 1.0f / sampleFreqDef;
anglesComputed = 0;
}
void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
float az, float mx, float my, float mz) {
float 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 Adafruit_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 Adafruit_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 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);
anglesComputed = 1;
}

87
src/Adafruit_AHRS_Madgwick.h Normal file
View file

@ -0,0 +1,87 @@
//=============================================================================================
// 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 <math.h>
//--------------------------------------------------------------------------------------------
// Variable declaration
class Adafruit_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:
Adafruit_Madgwick(void);
void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
void update(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz);
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
// 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;
}
void getQuaternion(float *w, float *x, float *y, float *z) {
*w = q0;
*x = q1;
*y = q2;
*z = q3;
}
};
#endif

272
src/Adafruit_AHRS_Mahony.cpp Normal file
View file

@ -0,0 +1,272 @@
//=============================================================================================
// 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() {
twoKp = twoKpDef; // 2 * proportional gain (Kp)
twoKi = twoKiDef; // 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 = 0;
invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
}
void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
float az, float mx, float my, float mz) {
float 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 * invSampleFreq;
integralFBy += twoKi * halfey * invSampleFreq;
integralFBz += twoKi * halfez * invSampleFreq;
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 * invSampleFreq); // pre-multiply common factors
gy *= (0.5f * invSampleFreq);
gz *= (0.5f * invSampleFreq);
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 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 * invSampleFreq;
integralFBy += twoKi * halfey * invSampleFreq;
integralFBz += twoKi * halfez * invSampleFreq;
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 * invSampleFreq); // pre-multiply common factors
gy *= (0.5f * invSampleFreq);
gz *= (0.5f * invSampleFreq);
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;
}
//-------------------------------------------------------------------------------------------
// 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;
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 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);
anglesComputed = 1;
}
//============================================================================================
// END OF CODE
//============================================================================================

81
src/Adafruit_AHRS_Mahony.h Normal file
View file

@ -0,0 +1,81 @@
//=============================================================================================
// 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 <math.h>
//--------------------------------------------------------------------------------------------
// Variable declaration
class Adafruit_Mahony {
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;
char anglesComputed;
static float invSqrt(float x);
void computeAngles();
//-------------------------------------------------------------------------------------------
// Function declarations
public:
Adafruit_Mahony();
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 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;
}
};
#endif

1487
src/Adafruit_AHRS_NXPFusion.cpp Normal file
View file

File diff suppressed because it is too large Load diff

114
src/Adafruit_AHRS_NXPFusion.h Normal file
View file

@ -0,0 +1,114 @@
#include <Arduino.h>
// 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.
//
// changed class name to avoid collision
class Adafruit_NXPSensorFusion {
public:
void begin(float sampleRate = 100.0f);
void update(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz);
float getRoll() { return PhiPl; }
float getPitch() { return ThePl; }
float getYaw() { return PsiPl; }
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
};

467
src/Adafruit_AHRS_NXPmatrix.c Normal file
View file

@ -0,0 +1,467 @@
// 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];
}

5
Adafruit_Sensor_Set.h → src/Adafruit_Sensor_Set.h
View file

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

85
src/Adafruit_Simple_AHRS.cpp Normal file
View file

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

20
src/Adafruit_Simple_AHRS.h Normal file
View file

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