Skip to content

Commit 7f4fc8c

Browse files
hbisbyhbisby
committed
initial commit
0 parents  commit 7f4fc8c

File tree

5 files changed

+323
-0
lines changed

5 files changed

+323
-0
lines changed

‎README.adoc

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
= Madgwick Library =
2+
3+
This library wraps the official implementation of MadgwickAHRS algorithm to get orientation of an object based on accelerometer and gyroscope readings
4+
5+
== License ==
6+
7+
Copyright (c) Arduino LLC. All right reserved.
8+
9+
This library is free software; you can redistribute it and/or
10+
modify it under the terms of the GNU Lesser General Public
11+
License as published by the Free Software Foundation; either
12+
version 2.1 of the License, or (at your option) any later version.
13+
14+
This library is distributed in the hope that it will be useful,
15+
but WITHOUT ANY WARRANTY; without even the implied warranty of
16+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17+
Lesser General Public License for more details.
18+
19+
You should have received a copy of the GNU Lesser General Public
20+
License along with this library; if not, write to the Free Software
21+
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
22+
23+
24+
Implementation of Madgwick's IMU and AHRS algorithms.
25+
See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
26+
27+
Date Author Notes
28+
29/09/2011 SOH Madgwick Initial release
29+
02/10/2011 SOH Madgwick Optimised for reduced CPU load
30+
19/02/2012 SOH Madgwick Magnetometer measurement is normalised

‎keywords.txt

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#######################################
2+
# Syntax Coloring Map For WiFi
3+
#######################################
4+
5+
#######################################
6+
# Datatypes (KEYWORD1)
7+
#######################################
8+
9+
Madgwick KEYWORD1
10+
11+
#######################################
12+
# Methods and Functions (KEYWORD2)
13+
#######################################
14+
15+
update KEYWORD2
16+
updateIMU KEYWORD2
17+
getPitch KEYWORD2
18+
getYaw KEYWORD2
19+
getRoll KEYWORD2
20+
21+
22+
#######################################
23+
# Constants (LITERAL1)
24+
#######################################
25+

‎library.properties

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
name=Madgwick
2+
version=1.0
3+
author=Arduino
4+
maintainer=Arduino <info@arduino.cc>
5+
sentence=Helpers for MadgwickAHRS algorithm
6+
paragraph=This library wraps the official implementation of MadgwickAHRS algorithm to get orientation of an object based on accelerometer and gyroscope readings
7+
category=Data Processing
8+
url=http://arduino.cc/en/Reference/XXXXXX
9+
architectures=*

‎src/MadgwickAHRS.cpp

Lines changed: 215 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,215 @@
1+
//=====================================================================================================
2+
// MadgwickAHRS.c
3+
//=====================================================================================================
4+
//
5+
// Implementation of Madgwick's IMU and AHRS algorithms.
6+
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
7+
//
8+
// Date Author Notes
9+
// 29/09/2011 SOH Madgwick Initial release
10+
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
11+
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
12+
//
13+
//=====================================================================================================
14+
15+
//---------------------------------------------------------------------------------------------------
16+
// Header files
17+
18+
#include "MadgwickAHRS.h"
19+
#include <math.h>
20+
21+
//---------------------------------------------------------------------------------------------------
22+
// Definitions
23+
24+
25+
26+
//====================================================================================================
27+
// Functions
28+
29+
//---------------------------------------------------------------------------------------------------
30+
// AHRS algorithm update
31+
32+
void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
33+
float recipNorm;
34+
float s0, s1, s2, s3;
35+
float qDot1, qDot2, qDot3, qDot4;
36+
float hx, hy;
37+
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;
38+
39+
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
40+
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
41+
updateIMU(gx, gy, gz, ax, ay, az);
42+
return;
43+
}
44+
45+
// Rate of change of quaternion from gyroscope
46+
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
47+
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
48+
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
49+
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
50+
51+
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
52+
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
53+
54+
// Normalise accelerometer measurement
55+
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
56+
ax *= recipNorm;
57+
ay *= recipNorm;
58+
az *= recipNorm;
59+
60+
// Normalise magnetometer measurement
61+
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
62+
mx *= recipNorm;
63+
my *= recipNorm;
64+
mz *= recipNorm;
65+
66+
// Auxiliary variables to avoid repeated arithmetic
67+
_2q0mx = 2.0f * q0 * mx;
68+
_2q0my = 2.0f * q0 * my;
69+
_2q0mz = 2.0f * q0 * mz;
70+
_2q1mx = 2.0f * q1 * mx;
71+
_2q0 = 2.0f * q0;
72+
_2q1 = 2.0f * q1;
73+
_2q2 = 2.0f * q2;
74+
_2q3 = 2.0f * q3;
75+
_2q0q2 = 2.0f * q0 * q2;
76+
_2q2q3 = 2.0f * q2 * q3;
77+
q0q0 = q0 * q0;
78+
q0q1 = q0 * q1;
79+
q0q2 = q0 * q2;
80+
q0q3 = q0 * q3;
81+
q1q1 = q1 * q1;
82+
q1q2 = q1 * q2;
83+
q1q3 = q1 * q3;
84+
q2q2 = q2 * q2;
85+
q2q3 = q2 * q3;
86+
q3q3 = q3 * q3;
87+
88+
// Reference direction of Earth's magnetic field
89+
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
90+
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
91+
_2bx = sqrt(hx * hx + hy * hy);
92+
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
93+
_4bx = 2.0f * _2bx;
94+
_4bz = 2.0f * _2bz;
95+
96+
// Gradient decent algorithm corrective step
97+
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);
98+
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);
99+
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);
100+
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);
101+
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
102+
s0 *= recipNorm;
103+
s1 *= recipNorm;
104+
s2 *= recipNorm;
105+
s3 *= recipNorm;
106+
107+
// Apply feedback step
108+
qDot1 -= beta * s0;
109+
qDot2 -= beta * s1;
110+
qDot3 -= beta * s2;
111+
qDot4 -= beta * s3;
112+
}
113+
114+
// Integrate rate of change of quaternion to yield quaternion
115+
q0 += qDot1 * (1.0f / sampleFreq);
116+
q1 += qDot2 * (1.0f / sampleFreq);
117+
q2 += qDot3 * (1.0f / sampleFreq);
118+
q3 += qDot4 * (1.0f / sampleFreq);
119+
120+
// Normalise quaternion
121+
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
122+
q0 *= recipNorm;
123+
q1 *= recipNorm;
124+
q2 *= recipNorm;
125+
q3 *= recipNorm;
126+
}
127+
128+
//---------------------------------------------------------------------------------------------------
129+
// IMU algorithm update
130+
131+
void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
132+
float recipNorm;
133+
float s0, s1, s2, s3;
134+
float qDot1, qDot2, qDot3, qDot4;
135+
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
136+
137+
// Rate of change of quaternion from gyroscope
138+
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
139+
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
140+
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
141+
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
142+
143+
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
144+
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
145+
146+
// Normalise accelerometer measurement
147+
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
148+
ax *= recipNorm;
149+
ay *= recipNorm;
150+
az *= recipNorm;
151+
152+
// Auxiliary variables to avoid repeated arithmetic
153+
_2q0 = 2.0f * q0;
154+
_2q1 = 2.0f * q1;
155+
_2q2 = 2.0f * q2;
156+
_2q3 = 2.0f * q3;
157+
_4q0 = 4.0f * q0;
158+
_4q1 = 4.0f * q1;
159+
_4q2 = 4.0f * q2;
160+
_8q1 = 8.0f * q1;
161+
_8q2 = 8.0f * q2;
162+
q0q0 = q0 * q0;
163+
q1q1 = q1 * q1;
164+
q2q2 = q2 * q2;
165+
q3q3 = q3 * q3;
166+
167+
// Gradient decent algorithm corrective step
168+
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
169+
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
170+
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
171+
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
172+
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
173+
s0 *= recipNorm;
174+
s1 *= recipNorm;
175+
s2 *= recipNorm;
176+
s3 *= recipNorm;
177+
178+
// Apply feedback step
179+
qDot1 -= beta * s0;
180+
qDot2 -= beta * s1;
181+
qDot3 -= beta * s2;
182+
qDot4 -= beta * s3;
183+
}
184+
185+
// Integrate rate of change of quaternion to yield quaternion
186+
q0 += qDot1 * (1.0f / sampleFreq);
187+
q1 += qDot2 * (1.0f / sampleFreq);
188+
q2 += qDot3 * (1.0f / sampleFreq);
189+
q3 += qDot4 * (1.0f / sampleFreq);
190+
191+
// Normalise quaternion
192+
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
193+
q0 *= recipNorm;
194+
q1 *= recipNorm;
195+
q2 *= recipNorm;
196+
q3 *= recipNorm;
197+
}
198+
199+
//---------------------------------------------------------------------------------------------------
200+
// Fast inverse square-root
201+
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
202+
203+
float Madgwick::invSqrt(float x) {
204+
float halfx = 0.5f * x;
205+
float y = x;
206+
long i = *(long*)&y;
207+
i = 0x5f3759df - (i>>1);
208+
y = *(float*)&i;
209+
y = y * (1.5f - (halfx * y * y));
210+
return y;
211+
}
212+
213+
//====================================================================================================
214+
// END OF CODE
215+
//====================================================================================================

‎src/MadgwickAHRS.h

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
//=====================================================================================================
2+
// MadgwickAHRS.h
3+
//=====================================================================================================
4+
//
5+
// Implementation of Madgwick's IMU and AHRS algorithms.
6+
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
7+
//
8+
// Date Author Notes
9+
// 29/09/2011 SOH Madgwick Initial release
10+
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
11+
//
12+
//=====================================================================================================
13+
#ifndef MadgwickAHRS_h
14+
#define MadgwickAHRS_h
15+
#include <math.h>
16+
#define sampleFreq 512.0f // sample frequency in Hz
17+
#define betaDef 0.1f // 2 * proportional gain
18+
19+
//----------------------------------------------------------------------------------------------------
20+
// Variable declaration
21+
class Madgwick{
22+
private:
23+
float invSqrt(float x);
24+
volatile float beta = betaDef; // algorithm gain
25+
volatile float q0 = 1.0f;
26+
volatile float q1 = 0.0f;
27+
volatile float q2 = 0.0f;
28+
volatile float q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
29+
30+
//---------------------------------------------------------------------------------------------------
31+
// Function declarations
32+
public:
33+
Madgwick(void){};
34+
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
35+
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
36+
float getPitch(){return atan2(2 * q2 * q3 - 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1);};
37+
float getRoll(){return -1 * asin(2 * q1 * q3 + 2 * q0 * q2);};
38+
float getYaw(){return atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0 * q0 + 2 * q1 * q1 - 1);};
39+
};
40+
#endif
41+
42+
//=====================================================================================================
43+
// End of file
44+
//=====================================================================================================

0 commit comments

Comments
 (0)