-
Notifications
You must be signed in to change notification settings - Fork 275
/
MPU6050_gyro_pitch_roll_yaw.ino
65 lines (52 loc) · 1.5 KB
/
MPU6050_gyro_pitch_roll_yaw.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
/*
MPU6050 Triple Axis Gyroscope & Accelerometer. Pitch & Roll & Yaw Gyroscope Example.
Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html
GIT: https://github.com/jarzebski/Arduino-MPU6050
Web: http://www.jarzebski.pl
(c) 2014 by Korneliusz Jarzebski
*/
#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
// Timers
unsigned long timer = 0;
float timeStep = 0.01;
// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;
void setup()
{
Serial.begin(115200);
// Initialize MPU6050
while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}
// Calibrate gyroscope. The calibration must be at rest.
// If you don't want calibrate, comment this line.
mpu.calibrateGyro();
// Set threshold sensivty. Default 3.
// If you don't want use threshold, comment this line or set 0.
mpu.setThreshold(3);
}
void loop()
{
timer = millis();
// Read normalized values
Vector norm = mpu.readNormalizeGyro();
// Calculate Pitch, Roll and Yaw
pitch = pitch + norm.YAxis * timeStep;
roll = roll + norm.XAxis * timeStep;
yaw = yaw + norm.ZAxis * timeStep;
// Output raw
Serial.print(" Pitch = ");
Serial.print(pitch);
Serial.print(" Roll = ");
Serial.print(roll);
Serial.print(" Yaw = ");
Serial.println(yaw);
// Wait to full timeStep period
delay((timeStep*1000) - (millis() - timer));
}