Skip to content

Commit 0fc0e2f

Browse files
PaulStoffregenPaulStoffregen
committed
Merge pull request #8 from PaulStoffregen/master
Many fixes and improvements
2 parents bb47312 + e9f5f9c commit 0fc0e2f

File tree

4 files changed

+264
-35
lines changed

4 files changed

+264
-35
lines changed

‎examples/Visualize101/Visualize101.ino

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#include <CurieIMU.h>
2+
#include <MadgwickAHRS.h>
3+
4+
Madgwick filter;
5+
unsigned long microsPerReading, microsPrevious;
6+
float accelScale, gyroScale;
7+
8+
void setup() {
9+
Serial.begin(9600);
10+
11+
// start the IMU and filter
12+
CurieIMU.begin();
13+
CurieIMU.setGyroRate(25);
14+
CurieIMU.setAccelerometerRate(25);
15+
filter.begin(25);
16+
17+
// Set the accelerometer range to 2G
18+
CurieIMU.setAccelerometerRange(2);
19+
// Set the gyroscope range to 250 degrees/second
20+
CurieIMU.setGyroRange(250);
21+
22+
// initialize variables to pace updates to correct rate
23+
microsPerReading = 1000000 / 25;
24+
microsPrevious = micros();
25+
}
26+
27+
void loop() {
28+
int aix, aiy, aiz;
29+
int gix, giy, giz;
30+
float ax, ay, az;
31+
float gx, gy, gz;
32+
float roll, pitch, heading;
33+
unsigned long microsNow;
34+
35+
// check if it's time to read data and update the filter
36+
microsNow = micros();
37+
if (microsNow - microsPrevious >= microsPerReading) {
38+
39+
// read raw data from CurieIMU
40+
CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);
41+
42+
// convert from raw data to gravity and degrees/second units
43+
ax = convertRawAcceleration(aix);
44+
ay = convertRawAcceleration(aiy);
45+
az = convertRawAcceleration(aiz);
46+
gx = convertRawGyro(gix);
47+
gy = convertRawGyro(giy);
48+
gz = convertRawGyro(giz);
49+
50+
// update the filter, which computes orientation
51+
filter.updateIMU(gx, gy, gz, ax, ay, az);
52+
53+
// print the heading, pitch and roll
54+
roll = filter.getRoll();
55+
pitch = filter.getPitch();
56+
heading = filter.getYaw();
57+
Serial.print("Orientation: ");
58+
Serial.print(heading);
59+
Serial.print(" ");
60+
Serial.print(pitch);
61+
Serial.print(" ");
62+
Serial.println(roll);
63+
64+
// increment previous time, so we keep proper pace
65+
microsPrevious = microsPrevious + microsPerReading;
66+
}
67+
}
68+
69+
float convertRawAcceleration(int aRaw) {
70+
// since we are using 2G range
71+
// -2g maps to a raw value of -32768
72+
// +2g maps to a raw value of 32767
73+
74+
float a = (aRaw * 2.0) / 32768.0;
75+
return a;
76+
}
77+
78+
float convertRawGyro(int gRaw) {
79+
// since we are using 250 degrees/seconds range
80+
// -250 maps to a raw value of -32768
81+
// +250 maps to a raw value of 32767
82+
83+
float g = (gRaw * 250.0) / 32768.0;
84+
return g;
85+
}

‎extras/Visualizer/Visualizer.pde

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
import processing.serial.*;
2+
Serial myPort;
3+
4+
float yaw = 0.0;
5+
float pitch = 0.0;
6+
float roll = 0.0;
7+
8+
void setup()
9+
{
10+
size(600, 500, P3D);
11+
12+
// if you have only ONE serial port active
13+
myPort = new Serial(this, Serial.list()[0], 9600); // if you have only ONE serial port active
14+
15+
// if you know the serial port name
16+
//myPort = new Serial(this, "COM5:", 9600); // Windows
17+
//myPort = new Serial(this, "/dev/ttyACM0", 9600); // Linux
18+
//myPort = new Serial(this, "/dev/cu.usbmodem1217321", 9600); // Mac
19+
20+
textSize(16); // set text size
21+
textMode(SHAPE); // set text mode to shape
22+
}
23+
24+
void draw()
25+
{
26+
serialEvent(); // read and parse incoming serial message
27+
background(255); // set background to white
28+
lights();
29+
30+
translate(width/2, height/2); // set position to centre
31+
32+
pushMatrix(); // begin object
33+
34+
float c1 = cos(radians(roll));
35+
float s1 = sin(radians(roll));
36+
float c2 = cos(radians(pitch));
37+
float s2 = sin(radians(pitch));
38+
float c3 = cos(radians(yaw));
39+
float s3 = sin(radians(yaw));
40+
applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
41+
-s2, c1*c2, c2*s1, 0,
42+
c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
43+
0, 0, 0, 1);
44+
45+
drawArduino();
46+
47+
popMatrix(); // end of object
48+
49+
// Print values to console
50+
print(roll);
51+
print("\t");
52+
print(pitch);
53+
print("\t");
54+
print(yaw);
55+
println();
56+
}
57+
58+
void serialEvent()
59+
{
60+
int newLine = 13; // new line character in ASCII
61+
String message;
62+
do {
63+
message = myPort.readStringUntil(newLine); // read from port until new line
64+
if (message != null) {
65+
String[] list = split(trim(message), " ");
66+
if (list.length >= 4 && list[0].equals("Orientation:")) {
67+
yaw = float(list[1]); // convert to float yaw
68+
pitch = float(list[2]); // convert to float pitch
69+
roll = float(list[3]); // convert to float roll
70+
}
71+
}
72+
} while (message != null);
73+
}
74+
75+
void drawArduino()
76+
{
77+
/* function contains shape(s) that are rotated with the IMU */
78+
stroke(0, 90, 90); // set outline colour to darker teal
79+
fill(0, 130, 130); // set fill colour to lighter teal
80+
box(300, 10, 200); // draw Arduino board base shape
81+
82+
stroke(0); // set outline colour to black
83+
fill(80); // set fill colour to dark grey
84+
85+
translate(60, -10, 90); // set position to edge of Arduino box
86+
box(170, 20, 10); // draw pin header as box
87+
88+
translate(-20, 0, -180); // set position to other edge of Arduino box
89+
box(210, 20, 10); // draw other pin header as box
90+
}
91+

‎src/MadgwickAHRS.cpp

Lines changed: 49 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
1-
//=====================================================================================================
1+
//=============================================================================================
22
// MadgwickAHRS.c
3-
//=====================================================================================================
3+
//=============================================================================================
44
//
55
// Implementation of Madgwick's IMU and AHRS algorithms.
66
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
@@ -14,23 +14,25 @@
1414
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
1515
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
1616
//
17-
//=====================================================================================================
17+
//=============================================================================================
1818

19-
//---------------------------------------------------------------------------------------------------
19+
//-------------------------------------------------------------------------------------------
2020
// Header files
2121

2222
#include "MadgwickAHRS.h"
2323
#include <math.h>
2424

25-
//---------------------------------------------------------------------------------------------------
25+
//-------------------------------------------------------------------------------------------
2626
// Definitions
2727

28+
#define sampleFreqDef 512.0f // sample frequency in Hz
29+
#define betaDef 0.1f // 2 * proportional gain
2830

2931

30-
//====================================================================================================
32+
//============================================================================================
3133
// Functions
3234

33-
//---------------------------------------------------------------------------------------------------
35+
//-------------------------------------------------------------------------------------------
3436
// AHRS algorithm update
3537

3638
Madgwick::Madgwick() {
@@ -39,6 +41,8 @@ Madgwick::Madgwick() {
3941
q1 = 0.0f;
4042
q2 = 0.0f;
4143
q3 = 0.0f;
44+
invSampleFreq = 1.0f / sampleFreqDef;
45+
anglesComputed = 0;
4246
}
4347

4448
void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
@@ -54,6 +58,11 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
5458
return;
5559
}
5660

61+
// Convert gyroscope degrees/sec to radians/sec
62+
gx *= 0.0174533f;
63+
gy *= 0.0174533f;
64+
gz *= 0.0174533f;
65+
5766
// Rate of change of quaternion from gyroscope
5867
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
5968
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
@@ -67,7 +76,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
6776
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
6877
ax *= recipNorm;
6978
ay *= recipNorm;
70-
az *= recipNorm;
79+
az *= recipNorm;
7180

7281
// Normalise magnetometer measurement
7382
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
@@ -100,7 +109,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
100109
// Reference direction of Earth's magnetic field
101110
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
102111
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
103-
_2bx = sqrt(hx * hx + hy * hy);
112+
_2bx = sqrtf(hx * hx + hy * hy);
104113
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
105114
_4bx = 2.0f * _2bx;
106115
_4bz = 2.0f * _2bz;
@@ -124,20 +133,21 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
124133
}
125134

126135
// Integrate rate of change of quaternion to yield quaternion
127-
q0 += qDot1 * (1.0f / sampleFreq);
128-
q1 += qDot2 * (1.0f / sampleFreq);
129-
q2 += qDot3 * (1.0f / sampleFreq);
130-
q3 += qDot4 * (1.0f / sampleFreq);
136+
q0 += qDot1 * invSampleFreq;
137+
q1 += qDot2 * invSampleFreq;
138+
q2 += qDot3 * invSampleFreq;
139+
q3 += qDot4 * invSampleFreq;
131140

132141
// Normalise quaternion
133142
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
134143
q0 *= recipNorm;
135144
q1 *= recipNorm;
136145
q2 *= recipNorm;
137146
q3 *= recipNorm;
147+
anglesComputed = 0;
138148
}
139149

140-
//---------------------------------------------------------------------------------------------------
150+
//-------------------------------------------------------------------------------------------
141151
// IMU algorithm update
142152

143153
void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
@@ -146,6 +156,11 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float
146156
float qDot1, qDot2, qDot3, qDot4;
147157
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
148158

159+
// Convert gyroscope degrees/sec to radians/sec
160+
gx *= 0.0174533f;
161+
gy *= 0.0174533f;
162+
gz *= 0.0174533f;
163+
149164
// Rate of change of quaternion from gyroscope
150165
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
151166
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
@@ -195,20 +210,21 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float
195210
}
196211

197212
// Integrate rate of change of quaternion to yield quaternion
198-
q0 += qDot1 * (1.0f / sampleFreq);
199-
q1 += qDot2 * (1.0f / sampleFreq);
200-
q2 += qDot3 * (1.0f / sampleFreq);
201-
q3 += qDot4 * (1.0f / sampleFreq);
213+
q0 += qDot1 * invSampleFreq;
214+
q1 += qDot2 * invSampleFreq;
215+
q2 += qDot3 * invSampleFreq;
216+
q3 += qDot4 * invSampleFreq;
202217

203218
// Normalise quaternion
204219
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
205220
q0 *= recipNorm;
206221
q1 *= recipNorm;
207222
q2 *= recipNorm;
208223
q3 *= recipNorm;
224+
anglesComputed = 0;
209225
}
210226

211-
//---------------------------------------------------------------------------------------------------
227+
//-------------------------------------------------------------------------------------------
212228
// Fast inverse square-root
213229
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
214230

@@ -219,9 +235,20 @@ float Madgwick::invSqrt(float x) {
219235
i = 0x5f3759df - (i>>1);
220236
y = *(float*)&i;
221237
y = y * (1.5f - (halfx * y * y));
238+
y = y * (1.5f - (halfx * y * y));
222239
return y;
223240
}
224241

225-
//====================================================================================================
226-
// END OF CODE
227-
//====================================================================================================
242+
//-------------------------------------------------------------------------------------------
243+
244+
void Madgwick::computeAngles()
245+
{
246+
float x = 2.0f * (q1*q3 - q0*q2);
247+
float y = 2.0f * (q0*q1 + q2*q3);
248+
float z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
249+
pitch = atan2f(x, sqrtf(y*y + z*z));
250+
roll = atan2f(y, sqrtf(x*x + z*z));
251+
yaw = atan2f(2.0f*q1*q2 - 2.0f*q0*q3, 2.0f*q0*q0 + 2.0f*q1*q1 - 1.0f);
252+
anglesComputed = 1;
253+
}
254+

0 commit comments

Comments
 (0)