Tests Yaw angle
Dependencies:
MPU6050IMU
mbed
main.cpp@2:8b8ea2b74666, 2015-02-20 (annotated)
- Committer:
- Reimerguns
- Date:
- Fri Feb 20 20:04:37 2015 +0000
- Revision:
- 2:8b8ea2b74666
- Parent:
-
0:98f7c7d55051
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|
| Reimerguns |
2:8b8ea2b74666
|
1
|
// SDA --- PTE25
|
| Reimerguns |
2:8b8ea2b74666
|
2
|
// SCL --- PTE24
|
| joshwilkins2013 |
0:98f7c7d55051
|
3
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
4
|
#include "mbed.h"
|
| joshwilkins2013 |
0:98f7c7d55051
|
5
|
#include "MPU6050.h"
|
| joshwilkins2013 |
0:98f7c7d55051
|
6
|
|
| Reimerguns |
2:8b8ea2b74666
|
7
|
float dt,Yaw,gyroZ=0, lgyroZ = 0;
|
| joshwilkins2013 |
0:98f7c7d55051
|
8
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
9
|
MPU6050 mpu6050;
|
| joshwilkins2013 |
0:98f7c7d55051
|
10
|
Timer t;
|
| joshwilkins2013 |
0:98f7c7d55051
|
11
|
Serial pc(USBTX, USBRX); // tx, rx
|
| joshwilkins2013 |
0:98f7c7d55051
|
12
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
13
|
int main(){
|
| joshwilkins2013 |
0:98f7c7d55051
|
14
|
pc.baud(9600);
|
| joshwilkins2013 |
0:98f7c7d55051
|
15
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
16
|
//Set up I2C
|
| joshwilkins2013 |
0:98f7c7d55051
|
17
|
i2c.frequency(400000); // use fast (400 kHz) I2C
|
| joshwilkins2013 |
0:98f7c7d55051
|
18
|
t.start();
|
| joshwilkins2013 |
0:98f7c7d55051
|
19
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
20
|
// Read the WHO_AM_I register, this is a good test of communication
|
| joshwilkins2013 |
0:98f7c7d55051
|
21
|
uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
|
| joshwilkins2013 |
0:98f7c7d55051
|
22
|
pc.printf("I AM 0x%x\n\r", whoami);
|
| joshwilkins2013 |
0:98f7c7d55051
|
23
|
pc.printf("I SHOULD BE 0x68\n\r");
|
| joshwilkins2013 |
0:98f7c7d55051
|
24
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
25
|
if (whoami == 0x68){
|
| joshwilkins2013 |
0:98f7c7d55051
|
26
|
pc.printf("MPU6050 is online...");
|
| joshwilkins2013 |
0:98f7c7d55051
|
27
|
wait(1);
|
| joshwilkins2013 |
0:98f7c7d55051
|
28
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
29
|
mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
|
| joshwilkins2013 |
0:98f7c7d55051
|
30
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
31
|
if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
|
| joshwilkins2013 |
0:98f7c7d55051
|
32
|
mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
|
| joshwilkins2013 |
0:98f7c7d55051
|
33
|
mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
|
| joshwilkins2013 |
0:98f7c7d55051
|
34
|
mpu6050.initMPU6050();
|
| joshwilkins2013 |
0:98f7c7d55051
|
35
|
pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
|
| joshwilkins2013 |
0:98f7c7d55051
|
36
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
37
|
pc.printf("\nMPU6050 passed self test... Initializing");
|
| joshwilkins2013 |
0:98f7c7d55051
|
38
|
wait(2);
|
| joshwilkins2013 |
0:98f7c7d55051
|
39
|
} else pc.printf("\nDevice did not the pass self-test!\n\r");
|
| joshwilkins2013 |
0:98f7c7d55051
|
40
|
} else {
|
| joshwilkins2013 |
0:98f7c7d55051
|
41
|
pc.printf("Could not connect to MPU6050: \n\r");
|
| joshwilkins2013 |
0:98f7c7d55051
|
42
|
pc.printf("%#x \n", whoami);
|
| joshwilkins2013 |
0:98f7c7d55051
|
43
|
while(1) ; // Loop forever if communication doesn't happen
|
| joshwilkins2013 |
0:98f7c7d55051
|
44
|
}
|
| joshwilkins2013 |
0:98f7c7d55051
|
45
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
46
|
while(1) {
|
| joshwilkins2013 |
0:98f7c7d55051
|
47
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
48
|
// If data ready bit set, all data registers have new data
|
| joshwilkins2013 |
0:98f7c7d55051
|
49
|
if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
|
| joshwilkins2013 |
0:98f7c7d55051
|
50
|
mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
|
| joshwilkins2013 |
0:98f7c7d55051
|
51
|
mpu6050.getGres();
|
| Reimerguns |
2:8b8ea2b74666
|
52
|
gyroZ = (float)gyroCount[2]*gRes; // - gyroBias[2];
|
| joshwilkins2013 |
0:98f7c7d55051
|
53
|
}
|
| joshwilkins2013 |
0:98f7c7d55051
|
54
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
55
|
Now = t.read_us();
|
| Reimerguns |
2:8b8ea2b74666
|
56
|
dt = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
|
| joshwilkins2013 |
0:98f7c7d55051
|
57
|
lastUpdate = Now;
|
| Reimerguns |
2:8b8ea2b74666
|
58
|
|
| Reimerguns |
2:8b8ea2b74666
|
59
|
Yaw += ((lgyroZ+gyroZ)/2)*dt;
|
| Reimerguns |
2:8b8ea2b74666
|
60
|
lgyroZ = gyroZ;
|
| Reimerguns |
2:8b8ea2b74666
|
61
|
pc.printf("Yaw: %f\n\r", Yaw);
|
| joshwilkins2013 |
0:98f7c7d55051
|
62
|
}
|
| joshwilkins2013 |
0:98f7c7d55051
|
63
|
|
| joshwilkins2013 |
0:98f7c7d55051
|
64
|
} |
The Professional Perfectionists
