Added support for NXP sensors and new calibration options
This commit is contained in:
microbuilder
parent
fbee108b04
commit
a8b22f70a0
15 changed files with 1643 additions and 123 deletions
|
|
@ -5,7 +5,7 @@
|
|||
#include "Adafruit_Sensor_Set.h"
|
||||
|
||||
// Simple sensor fusion AHRS using an accelerometer and magnetometer.
|
||||
class Adafruit_Simple_AHRS
|
||||
class Adafruit_Simple_AHRS
|
||||
{
|
||||
public:
|
||||
Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafruit_Sensor* magnetometer);
|
||||
|
|
@ -18,4 +18,4 @@ private:
|
|||
|
||||
};
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
|
|
|||
250
Madgwick.cpp
Normal file
250
Madgwick.cpp
Normal file
|
|
@ -0,0 +1,250 @@
|
|||
//=============================================================================================
|
||||
// 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
Normal file
79
Madgwick.h
Normal file
|
|
@ -0,0 +1,79 @@
|
|||
//=============================================================================================
|
||||
// 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
Normal file
274
Mahony.cpp
Normal file
|
|
@ -0,0 +1,274 @@
|
|||
//=============================================================================================
|
||||
// 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
Normal file
72
Mahony.h
Normal file
|
|
@ -0,0 +1,72 @@
|
|||
//=============================================================================================
|
||||
// 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
|
||||
55
examples/ahrs_calibration_ble/ArdPrintf.h
Normal file
55
examples/ahrs_calibration_ble/ArdPrintf.h
Normal file
|
|
@ -0,0 +1,55 @@
|
|||
#ifndef ARDPRINTF
|
||||
#define ARDPRINTF
|
||||
#define ARDBUFFER 20
|
||||
#include <stdarg.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
int ardprintf(char *str, ...)
|
||||
{
|
||||
int i, count=0, j=0, flag=0;
|
||||
char temp[ARDBUFFER+1];
|
||||
for(i=0; str[i]!='\0';i++) if(str[i]=='%') count++;
|
||||
|
||||
va_list argv;
|
||||
va_start(argv, count);
|
||||
for(i=0,j=0; str[i]!='\0';i++)
|
||||
{
|
||||
if(str[i]=='%')
|
||||
{
|
||||
temp[j] = '\0';
|
||||
Serial.print(temp);
|
||||
j=0;
|
||||
temp[0] = '\0';
|
||||
|
||||
switch(str[++i])
|
||||
{
|
||||
case 'd': Serial.print(va_arg(argv, int));
|
||||
break;
|
||||
case 'l': Serial.print(va_arg(argv, long));
|
||||
break;
|
||||
case 'f': Serial.print(va_arg(argv, double));
|
||||
break;
|
||||
case 'c': Serial.print((char)va_arg(argv, int));
|
||||
break;
|
||||
case 's': Serial.print(va_arg(argv, char *));
|
||||
break;
|
||||
default: ;
|
||||
};
|
||||
}
|
||||
else
|
||||
{
|
||||
temp[j] = str[i];
|
||||
j = (j+1)%ARDBUFFER;
|
||||
if(j==0)
|
||||
{
|
||||
temp[ARDBUFFER] = '\0';
|
||||
Serial.print(temp);
|
||||
temp[0]='\0';
|
||||
}
|
||||
}
|
||||
};
|
||||
Serial.println();
|
||||
return count + 1;
|
||||
}
|
||||
#undef ARDBUFFER
|
||||
#endif
|
||||
56
examples/ahrs_calibration_ble/BluefruitConfig.h
Normal file
56
examples/ahrs_calibration_ble/BluefruitConfig.h
Normal file
|
|
@ -0,0 +1,56 @@
|
|||
// COMMON SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// These settings are used in both SW UART, HW UART and SPI mode
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BUFSIZE 128 // Size of the read buffer for incoming data
|
||||
#define VERBOSE_MODE true // If set to 'true' enables debug output
|
||||
|
||||
|
||||
// SOFTWARE UART SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following macros declare the pins that will be used for 'SW' serial.
|
||||
// You should use this option if you are connecting the UART Friend to an UNO
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BLUEFRUIT_SWUART_RXD_PIN 9 // Required for software serial!
|
||||
#define BLUEFRUIT_SWUART_TXD_PIN 10 // Required for software serial!
|
||||
#define BLUEFRUIT_UART_CTS_PIN 11 // Required for software serial!
|
||||
#define BLUEFRUIT_UART_RTS_PIN -1 // Optional, set to -1 if unused
|
||||
|
||||
|
||||
// HARDWARE UART SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following macros declare the HW serial port you are using. Uncomment
|
||||
// this line if you are connecting the BLE to Leonardo/Micro or Flora
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#ifdef Serial1 // this makes it not complain on compilation if there's no Serial1
|
||||
#define BLUEFRUIT_HWSERIAL_NAME Serial1
|
||||
#endif
|
||||
|
||||
|
||||
// SHARED UART SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following sets the optional Mode pin, its recommended but not required
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused
|
||||
|
||||
|
||||
// SHARED SPI SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following macros declare the pins to use for HW and SW SPI communication.
|
||||
// SCK, MISO and MOSI should be connected to the HW SPI pins on the Uno when
|
||||
// using HW SPI. This should be used with nRF51822 based Bluefruit LE modules
|
||||
// that use SPI (Bluefruit LE SPI Friend).
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BLUEFRUIT_SPI_CS 8
|
||||
#define BLUEFRUIT_SPI_IRQ 7
|
||||
#define BLUEFRUIT_SPI_RST 6 // Optional but recommended, set to -1 if unused
|
||||
|
||||
// SOFTWARE SPI SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following macros declare the pins to use for SW SPI communication.
|
||||
// This should be used with nRF51822 based Bluefruit LE modules that use SPI
|
||||
// (Bluefruit LE SPI Friend).
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BLUEFRUIT_SPI_SCK 13
|
||||
#define BLUEFRUIT_SPI_MISO 12
|
||||
#define BLUEFRUIT_SPI_MOSI 11
|
||||
229
examples/ahrs_calibration_ble/ahrs_calibration_ble.ino
Normal file
229
examples/ahrs_calibration_ble/ahrs_calibration_ble.ino
Normal file
|
|
@ -0,0 +1,229 @@
|
|||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#if not defined (_VARIANT_ARDUINO_DUE_X_) && not defined (_VARIANT_ARDUINO_ZERO_)
|
||||
#include <SoftwareSerial.h>
|
||||
#endif
|
||||
|
||||
#define BLUEFRUIT51_ST_LSM303DLHC_L3GD20 (0)
|
||||
#define BLUEFRUIT51_ST_LSM9DS1 (1)
|
||||
#define BLUEFRUIT51_NXP_FXOS8700_FXAS21002 (2)
|
||||
|
||||
// Define your target sensor(s) here based on the list above!
|
||||
#define AHRS_VARIANT BLUEFRUIT51_ST_LSM303DLHC_L3GD20
|
||||
// #define AHRS_VARIANT BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
|
||||
#include "Adafruit_BLE.h"
|
||||
#include "Adafruit_BluefruitLE_SPI.h"
|
||||
#include "Adafruit_BluefruitLE_UART.h"
|
||||
#include "BluefruitConfig.h"
|
||||
|
||||
// Include
|
||||
#include "ArdPrintf.h"
|
||||
|
||||
// Config
|
||||
#define FACTORYRESET_ENABLE 1
|
||||
|
||||
// Include generic sensors headers
|
||||
#include <Wire.h>
|
||||
#include <Adafruit_Sensor.h>
|
||||
|
||||
// Include appropriate sensor driver(s)
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_ST_LSM303DLHC_L3GD20
|
||||
#include <Adafruit_L3GD20_U.h>
|
||||
#include <Adafruit_LSM303_U.h>
|
||||
#elif AHRS_VARIANT == BLUEFRUIT51_ST_LSM9DS1
|
||||
// ToDo!
|
||||
#elif AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
#include <Adafruit_FXAS21002C.h>
|
||||
#include <Adafruit_FXOS8700.h>
|
||||
#else
|
||||
#error "AHRS_VARIANT undefined! Please select a target sensor combination!"
|
||||
#endif
|
||||
|
||||
// Bluetooth
|
||||
// ...hardware SPI, using SCK/MOSI/MISO hardware SPI pins and then user selected CS/IRQ/RST
|
||||
Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);
|
||||
|
||||
// Create sensor instances.
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_ST_LSM303DLHC_L3GD20
|
||||
Adafruit_L3GD20_Unified gyro(20);
|
||||
Adafruit_LSM303_Accel_Unified accel(30301);
|
||||
Adafruit_LSM303_Mag_Unified mag(30302);
|
||||
#elif AHRS_VARIANT == BLUEFRUIT51_ST_LSM9DS1
|
||||
// ToDo!
|
||||
#elif AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
|
||||
Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
|
||||
#endif
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
|
||||
// Wait for the Serial Monitor to open (comment out to run without Serial Monitor)
|
||||
// while(!Serial);
|
||||
|
||||
Serial.println(F("Adafruit nDOF AHRS Calibration Example")); Serial.println("");
|
||||
|
||||
// Initialize the sensors.
|
||||
if (!gyro.begin())
|
||||
{
|
||||
/* There was a problem detecting the gyro ... check your connections */
|
||||
Serial.println("Ooops, no gyroscope detected ... Check your wiring!");
|
||||
while (1);
|
||||
}
|
||||
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
if(!accelmag.begin(ACCEL_RANGE_4G))
|
||||
{
|
||||
Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
|
||||
while(1);
|
||||
}
|
||||
#else
|
||||
if (!accel.begin())
|
||||
{
|
||||
/* There was a problem detecting the accel ... check your connections */
|
||||
Serial.println("Ooops, no accel detected ... Check your wiring!");
|
||||
while (1);
|
||||
}
|
||||
|
||||
if (!mag.begin())
|
||||
{
|
||||
/* There was a problem detecting the mag ... check your connections */
|
||||
Serial.println("Ooops, no mag detected ... Check your wiring!");
|
||||
while (1);
|
||||
}
|
||||
#endif
|
||||
|
||||
// Initialise the module
|
||||
Serial.print(F("Initialising the Bluefruit LE module: "));
|
||||
|
||||
if ( !ble.begin(VERBOSE_MODE) )
|
||||
{
|
||||
error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
|
||||
}
|
||||
Serial.println( F("OK!") );
|
||||
|
||||
// Factory Reset
|
||||
if ( FACTORYRESET_ENABLE )
|
||||
{
|
||||
/* Perform a factory reset to make sure everything is in a known state */
|
||||
Serial.println(F("Performing a factory reset: "));
|
||||
if ( ! ble.factoryReset() ) {
|
||||
error(F("Couldn't factory reset"));
|
||||
}
|
||||
}
|
||||
|
||||
/* Disable command echo from Bluefruit */
|
||||
ble.echo(false);
|
||||
|
||||
Serial.println("Requesting Bluefruit info:");
|
||||
/* Print Bluefruit information */
|
||||
ble.info();
|
||||
|
||||
/* Wait for a connection before starting the test */
|
||||
Serial.println("Waiting for a BLE connection to continue ...");
|
||||
|
||||
ble.verbose(false); // debug info is a little annoying after this point!
|
||||
|
||||
// Wait for the connection to complete
|
||||
delay(1000);
|
||||
|
||||
Serial.println(F("CONNECTED!"));
|
||||
Serial.println(F("**********"));
|
||||
|
||||
// Set module to DATA mode
|
||||
Serial.println( F("Switching to DATA mode!") );
|
||||
ble.setMode(BLUEFRUIT_MODE_DATA);
|
||||
|
||||
Serial.println(F("******************************"));
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
while ( ble.isConnected() ) {
|
||||
sensors_event_t event; // Need to read raw data, which is stored at the same time
|
||||
|
||||
// Get new data samples
|
||||
gyro.getEvent(&event);
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
accelmag.getEvent(&event);
|
||||
#else
|
||||
accel.getEvent(&event);
|
||||
mag.getEvent(&event);
|
||||
#endif
|
||||
|
||||
// Print the sensor data
|
||||
Serial.print("Raw:");
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
Serial.print(accelmag.accel_raw.x);
|
||||
Serial.print(',');
|
||||
Serial.print(accelmag.accel_raw.y);
|
||||
Serial.print(',');
|
||||
Serial.print(accelmag.accel_raw.z);
|
||||
Serial.print(',');
|
||||
#else
|
||||
Serial.print(accel.raw.x);
|
||||
Serial.print(',');
|
||||
Serial.print(accel.raw.y);
|
||||
Serial.print(',');
|
||||
Serial.print(accel.raw.z);
|
||||
Serial.print(',');
|
||||
#endif
|
||||
Serial.print(gyro.raw.x);
|
||||
Serial.print(',');
|
||||
Serial.print(gyro.raw.y);
|
||||
Serial.print(',');
|
||||
Serial.print(gyro.raw.z);
|
||||
Serial.print(',');
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
Serial.print(accelmag.mag_raw.x);
|
||||
Serial.print(',');
|
||||
Serial.print(accelmag.mag_raw.y);
|
||||
Serial.print(',');
|
||||
Serial.print(accelmag.mag_raw.z);
|
||||
Serial.println();
|
||||
#else
|
||||
Serial.print(mag.raw.x);
|
||||
Serial.print(',');
|
||||
Serial.print(mag.raw.y);
|
||||
Serial.print(',');
|
||||
Serial.print(mag.raw.z);
|
||||
Serial.println();
|
||||
#endif
|
||||
|
||||
// Send sensor data to Bluetooth
|
||||
char prefix[] = "R";
|
||||
ble.write(prefix, strlen(prefix));
|
||||
//ble.write((uint8_t *)&event, sizeof(sensors_event_t));
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
ble.write((uint8_t *)&accelmag.accel_raw.x, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&accelmag.accel_raw.y, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&accelmag.accel_raw.z, sizeof(int16_t));
|
||||
#else
|
||||
ble.write((uint8_t *)&accel.raw.x, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&accel.raw.y, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&accel.raw.z, sizeof(int16_t));
|
||||
#endif
|
||||
ble.write((uint8_t *)&gyro.raw.x, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&gyro.raw.y, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&gyro.raw.z, sizeof(int16_t));
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
ble.write((uint8_t *)&accelmag.mag_raw.x, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&accelmag.mag_raw.y, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&accelmag.mag_raw.z, sizeof(int16_t));
|
||||
#else
|
||||
ble.write((uint8_t *)&mag.raw.x, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&mag.raw.y, sizeof(int16_t));
|
||||
ble.write((uint8_t *)&mag.raw.z, sizeof(int16_t));
|
||||
#endif
|
||||
|
||||
delay(100);
|
||||
}
|
||||
}
|
||||
|
||||
// A small helper
|
||||
void error(const __FlashStringHelper*err) {
|
||||
Serial.println(err);
|
||||
while (1);
|
||||
}
|
||||
144
examples/ahrs_calibration_usb/ahrs_calibration_usb.ino
Normal file
144
examples/ahrs_calibration_usb/ahrs_calibration_usb.ino
Normal file
|
|
@ -0,0 +1,144 @@
|
|||
#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.
|
||||
//
|
||||
// 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 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);
|
||||
}
|
||||
56
examples/ahrs_fusion_ble/BluefruitConfig.h
Normal file
56
examples/ahrs_fusion_ble/BluefruitConfig.h
Normal file
|
|
@ -0,0 +1,56 @@
|
|||
// COMMON SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// These settings are used in both SW UART, HW UART and SPI mode
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BUFSIZE 128 // Size of the read buffer for incoming data
|
||||
#define VERBOSE_MODE true // If set to 'true' enables debug output
|
||||
|
||||
|
||||
// SOFTWARE UART SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following macros declare the pins that will be used for 'SW' serial.
|
||||
// You should use this option if you are connecting the UART Friend to an UNO
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BLUEFRUIT_SWUART_RXD_PIN 9 // Required for software serial!
|
||||
#define BLUEFRUIT_SWUART_TXD_PIN 10 // Required for software serial!
|
||||
#define BLUEFRUIT_UART_CTS_PIN 11 // Required for software serial!
|
||||
#define BLUEFRUIT_UART_RTS_PIN -1 // Optional, set to -1 if unused
|
||||
|
||||
|
||||
// HARDWARE UART SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following macros declare the HW serial port you are using. Uncomment
|
||||
// this line if you are connecting the BLE to Leonardo/Micro or Flora
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#ifdef Serial1 // this makes it not complain on compilation if there's no Serial1
|
||||
#define BLUEFRUIT_HWSERIAL_NAME Serial1
|
||||
#endif
|
||||
|
||||
|
||||
// SHARED UART SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following sets the optional Mode pin, its recommended but not required
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused
|
||||
|
||||
|
||||
// SHARED SPI SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following macros declare the pins to use for HW and SW SPI communication.
|
||||
// SCK, MISO and MOSI should be connected to the HW SPI pins on the Uno when
|
||||
// using HW SPI. This should be used with nRF51822 based Bluefruit LE modules
|
||||
// that use SPI (Bluefruit LE SPI Friend).
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BLUEFRUIT_SPI_CS 8
|
||||
#define BLUEFRUIT_SPI_IRQ 7
|
||||
#define BLUEFRUIT_SPI_RST 6 // Optional but recommended, set to -1 if unused
|
||||
|
||||
// SOFTWARE SPI SETTINGS
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
// The following macros declare the pins to use for SW SPI communication.
|
||||
// This should be used with nRF51822 based Bluefruit LE modules that use SPI
|
||||
// (Bluefruit LE SPI Friend).
|
||||
// ----------------------------------------------------------------------------------------------
|
||||
#define BLUEFRUIT_SPI_SCK 13
|
||||
#define BLUEFRUIT_SPI_MISO 12
|
||||
#define BLUEFRUIT_SPI_MOSI 11
|
||||
258
examples/ahrs_fusion_ble/ahrs_fusion_ble.ino
Normal file
258
examples/ahrs_fusion_ble/ahrs_fusion_ble.ino
Normal file
|
|
@ -0,0 +1,258 @@
|
|||
#include <Wire.h>
|
||||
#include <SPI.h>
|
||||
#include <Adafruit_Sensor.h>
|
||||
#include "Mahony.h"
|
||||
#include "Madgwick.h"
|
||||
#include "Adafruit_BLE.h"
|
||||
#include "Adafruit_BluefruitLE_SPI.h"
|
||||
#include "Adafruit_BluefruitLE_UART.h"
|
||||
#include "BluefruitConfig.h"
|
||||
|
||||
// Note: This sketch is a WORK IN PROGRESS
|
||||
|
||||
#define BLUEFRUIT51_ST_LSM303DLHC_L3GD20 (0)
|
||||
#define BLUEFRUIT51_ST_LSM9DS1 (1)
|
||||
#define BLUEFRUIT51_NXP_FXOS8700_FXAS21002 (2)
|
||||
|
||||
// Define your target sensor(s) here based on the list above!
|
||||
#define AHRS_VARIANT BLUEFRUIT51_ST_LSM303DLHC_L3GD20
|
||||
//#define AHRS_VARIANT BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
|
||||
// Config
|
||||
#define FACTORYRESET_ENABLE 1
|
||||
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_ST_LSM303DLHC_L3GD20
|
||||
#include <Adafruit_L3GD20_U.h>
|
||||
#include <Adafruit_LSM303_U.h>
|
||||
#elif AHRS_VARIANT == BLUEFRUIT51_ST_LSM9DS1
|
||||
// ToDo!
|
||||
#elif AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
#include <Adafruit_FXAS21002C.h>
|
||||
#include <Adafruit_FXOS8700.h>
|
||||
#else
|
||||
#error "AHRS_VARIANT undefined! Please select a target sensor combination!"
|
||||
#endif
|
||||
|
||||
// Create sensor instances.
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_ST_LSM303DLHC_L3GD20
|
||||
Adafruit_L3GD20_Unified gyro(20);
|
||||
Adafruit_LSM303_Accel_Unified accel(30301);
|
||||
Adafruit_LSM303_Mag_Unified mag(30302);
|
||||
#elif AHRS_VARIANT == BLUEFRUIT51_ST_LSM9DS1
|
||||
// ToDo!
|
||||
#elif AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
|
||||
Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
|
||||
#endif
|
||||
|
||||
// Bluetooth
|
||||
// ...hardware SPI, using SCK/MOSI/MISO hardware SPI pins and then user selected CS/IRQ/RST
|
||||
Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);
|
||||
|
||||
// Mag calibration values are calculated via ahrs_calibration.
|
||||
// These values must be determined for each baord/environment.
|
||||
// See the image in this sketch folder for the values used
|
||||
// below.
|
||||
|
||||
// Offsets applied to raw x/y/z mag values
|
||||
float mag_offsets[3] = { 2.45F, -4.55F, -26.93F };
|
||||
|
||||
// Soft iron error compensation matrix
|
||||
float mag_softiron_matrix[3][3] = { { 0.961, -0.001, 0.025 },
|
||||
{ 0.001, 0.886, 0.015 },
|
||||
{ 0.025, 0.015, 1.176 } };
|
||||
|
||||
float mag_field_strength = 44.12F;
|
||||
|
||||
// Offsets applied to compensate for gyro zero-drift error for x/y/z
|
||||
// Raw values converted to rad/s based on 250dps sensitiviy (1 lsb = 0.00875 rad/s)
|
||||
float rawToDPS = 0.00875F;
|
||||
float dpsToRad = 0.017453293F;
|
||||
float gyro_zero_offsets[3] = { 175.0F * rawToDPS * dpsToRad,
|
||||
-729.0F * rawToDPS * dpsToRad,
|
||||
101.0F * rawToDPS * dpsToRad };
|
||||
|
||||
|
||||
// Mahony is lighter weight as a filter and should be used
|
||||
// on slower systems
|
||||
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 BLE Fusion Example")); Serial.println("");
|
||||
|
||||
// Initialize the sensors.
|
||||
if(!gyro.begin())
|
||||
{
|
||||
/* There was a problem detecting the gyro ... check your connections */
|
||||
Serial.println("Ooops, no gyro detected ... Check your wiring!");
|
||||
while(1);
|
||||
}
|
||||
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
if(!accelmag.begin(ACCEL_RANGE_4G))
|
||||
{
|
||||
Serial.println("Ooops, no FXOS8700 detected ... Check your wiring!");
|
||||
while(1);
|
||||
}
|
||||
#else
|
||||
if (!accel.begin())
|
||||
{
|
||||
/* There was a problem detecting the accel ... check your connections */
|
||||
Serial.println("Ooops, no accel detected ... Check your wiring!");
|
||||
while (1);
|
||||
}
|
||||
|
||||
if (!mag.begin())
|
||||
{
|
||||
/* There was a problem detecting the mag ... check your connections */
|
||||
Serial.println("Ooops, no mag detected ... Check your wiring!");
|
||||
while (1);
|
||||
}
|
||||
#endif
|
||||
|
||||
// Filter expects 25 samples per second
|
||||
filter.begin(25);
|
||||
|
||||
|
||||
// Initialise the module
|
||||
Serial.print(F("Initialising the Bluefruit LE module: "));
|
||||
|
||||
if ( !ble.begin(VERBOSE_MODE) )
|
||||
{
|
||||
Serial.println(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
|
||||
}
|
||||
Serial.println( F("OK!") );
|
||||
|
||||
// Factory Reset
|
||||
if ( FACTORYRESET_ENABLE )
|
||||
{
|
||||
/* Perform a factory reset to make sure everything is in a known state */
|
||||
Serial.println(F("Performing a factory reset: "));
|
||||
if ( ! ble.factoryReset() ) {
|
||||
Serial.println(F("Couldn't factory reset"));
|
||||
}
|
||||
}
|
||||
|
||||
/* Disable command echo from Bluefruit */
|
||||
ble.echo(false);
|
||||
|
||||
Serial.println("Requesting Bluefruit info:");
|
||||
/* Print Bluefruit information */
|
||||
ble.info();
|
||||
|
||||
/* Wait for a connection before starting the test */
|
||||
Serial.println("Waiting for a BLE connection to continue ...");
|
||||
|
||||
ble.verbose(false); // debug info is a little annoying after this point!
|
||||
|
||||
// Wait for the connection to complete
|
||||
delay(1000);
|
||||
|
||||
Serial.println(F("CONNECTED!"));
|
||||
Serial.println(F("**********"));
|
||||
|
||||
// Set module to DATA mode
|
||||
Serial.println( F("Switching to DATA mode!") );
|
||||
ble.setMode(BLUEFRUIT_MODE_DATA);
|
||||
|
||||
Serial.println(F("******************************"));
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
sensors_event_t gyro_event;
|
||||
sensors_event_t accel_event;
|
||||
sensors_event_t mag_event;
|
||||
|
||||
while ( ble.isConnected() ) {
|
||||
|
||||
// Get new data samples
|
||||
gyro.getEvent(&gyro_event);
|
||||
#if AHRS_VARIANT == BLUEFRUIT51_NXP_FXOS8700_FXAS21002
|
||||
accelmag.getEvent(&accel_event, &mag_event);
|
||||
#else
|
||||
accel.getEvent(&accel_event);
|
||||
mag.getEvent(&mag_event);
|
||||
#endif
|
||||
|
||||
// Apply mag offset compensation (base values in uTesla)
|
||||
float x = mag_event.magnetic.x - mag_offsets[0];
|
||||
float y = mag_event.magnetic.y - mag_offsets[1];
|
||||
float z = mag_event.magnetic.z - mag_offsets[2];
|
||||
|
||||
// Apply mag soft iron error compensation
|
||||
float mx = x * mag_softiron_matrix[0][0] + y * mag_softiron_matrix[0][1] + z * mag_softiron_matrix[0][2];
|
||||
float my = x * mag_softiron_matrix[1][0] + y * mag_softiron_matrix[1][1] + z * mag_softiron_matrix[1][2];
|
||||
float mz = x * mag_softiron_matrix[2][0] + y * mag_softiron_matrix[2][1] + z * mag_softiron_matrix[2][2];
|
||||
|
||||
// Apply gyro zero-rate error compensation
|
||||
float gx = gyro_event.gyro.x - gyro_zero_offsets[0];
|
||||
float gy = gyro_event.gyro.y - gyro_zero_offsets[1];
|
||||
float gz = gyro_event.gyro.z - gyro_zero_offsets[2];
|
||||
|
||||
// The filter library expects gyro data in degrees/s, but adafruit sensor
|
||||
// uses rad/s so we need to convert them first (or adapt the filter lib
|
||||
// where they are being converted)
|
||||
gx *= 57.2958F;
|
||||
gy *= 57.2958F;
|
||||
gz *= 57.2958F;
|
||||
|
||||
// Update the filter
|
||||
filter.update(gx, gy, gz,
|
||||
accel_event.acceleration.x, accel_event.acceleration.y, accel_event.acceleration.z,
|
||||
mx, my, mz);
|
||||
|
||||
/*
|
||||
// Print the orientation filter output
|
||||
float roll = filter.getRoll();
|
||||
float pitch = filter.getPitch();
|
||||
float heading = filter.getYaw();
|
||||
Serial.print(millis());
|
||||
Serial.print(" - Orientation: ");
|
||||
Serial.print(pitch);
|
||||
Serial.print(" ");
|
||||
Serial.print(heading);
|
||||
Serial.print(" ");
|
||||
Serial.println(roll);
|
||||
|
||||
// Send sensor data to Bluetooth
|
||||
char prefix[] = "V";
|
||||
ble.write(prefix, strlen(prefix));
|
||||
ble.write((uint8_t *)&pitch, sizeof(float));
|
||||
ble.write((uint8_t *)&heading, sizeof(float));
|
||||
ble.write((uint8_t *)&roll, sizeof(float));
|
||||
*/
|
||||
|
||||
// Print the orientation filter output in quaternions.
|
||||
// This avoids the gimbal lock problem with Euler angles when you get
|
||||
// close to 180 degrees (causing the model to rotate or flip, etc.)
|
||||
float qw, qx, qy, qz;
|
||||
filter.getQuaternion(&qw, &qx, &qy, &qz);
|
||||
Serial.print(millis());
|
||||
Serial.print(" - Quat: ");
|
||||
Serial.print(qw);
|
||||
Serial.print(" ");
|
||||
Serial.print(qx);
|
||||
Serial.print(" ");
|
||||
Serial.print(qy);
|
||||
Serial.print(" ");
|
||||
Serial.println(qz);
|
||||
|
||||
// Send sensor data to Bluetooth
|
||||
char prefix[] = "V";
|
||||
ble.write(prefix, strlen(prefix));
|
||||
ble.write((uint8_t *)&qw, sizeof(float));
|
||||
ble.write((uint8_t *)&qx, sizeof(float));
|
||||
ble.write((uint8_t *)&qy, sizeof(float));
|
||||
ble.write((uint8_t *)&qz, sizeof(float));
|
||||
|
||||
delay(10);
|
||||
}
|
||||
}
|
||||
167
examples/ahrs_fusion_usb/ahrs_fusion_usb.ino
Normal file
167
examples/ahrs_fusion_usb/ahrs_fusion_usb.ino
Normal file
|
|
@ -0,0 +1,167 @@
|
|||
#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 baord/environment.
|
||||
// See the image in this sketch folder for the values used
|
||||
// below.
|
||||
|
||||
// Offsets applied to raw x/y/z mag values
|
||||
float mag_offsets[3] = { 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(70);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
Before Width: | Height: | Size: 214 KiB After Width: | Height: | Size: 214 KiB |
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
name=Adafruit AHRS
|
||||
version=1.0.2
|
||||
version=1.1.0
|
||||
author=Adafruit
|
||||
maintainer=Adafruit <info@adafruit.com>
|
||||
sentence=AHRS (Altitude and Heading Reference System) for Adafruit's 9DOF and 10DOF breakouts
|
||||
|
|
|
|||
Loading…
Reference in a new issue