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