Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2194 | - | 1 | /* |
2 | MPU6050 lib 0x02 |
||
3 | |||
4 | copyright (c) Davide Gironi, 2012 |
||
5 | |||
6 | Released under GPLv3. |
||
7 | Please refer to LICENSE file for licensing information. |
||
8 | */ |
||
9 | |||
10 | /* |
||
11 | * This file contains all the functions needed to process 6-axis orientation by the internal chip processor |
||
12 | */ |
||
13 | |||
14 | //to enable get roll, pitch and yaw function we include the math function |
||
15 | //if some error appears we must add -lm and -lc to C linker, the linker line should become somethink like this |
||
16 | //${COMMAND} -lm ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${INPUTS} -lc |
||
17 | |||
18 | |||
19 | #include "mpu6050.h" |
||
20 | |||
21 | #if MPU6050_GETATTITUDE == 2 |
||
22 | |||
23 | #include <avr/io.h> |
||
24 | #include <avr/pgmspace.h> |
||
25 | #include <avr/interrupt.h> |
||
26 | #include <util/delay.h> |
||
27 | #include <math.h> //include libm |
||
28 | |||
29 | #define MPU6050_DMP_CODE_SIZE 1929 |
||
30 | #define MPU6050_DMP_CONFIG_SIZE 192 |
||
31 | #define MPU6050_DMP_UPDATES_SIZE 47 |
||
32 | |||
33 | volatile uint8_t mpu6050_mpuInterrupt = 0; |
||
34 | uint8_t *dmpPacketBuffer; |
||
35 | uint16_t mpu6050_fifoCount = 0; |
||
36 | uint8_t mpu6050_mpuIntStatus = 0; |
||
37 | uint8_t mpu6050_fifoBuffer[64]; |
||
38 | |||
39 | |||
40 | /* ================================================================================================ * |
||
41 | | Default MotionApps v2.0 42-byte FIFO packet structure: | |
||
42 | | | |
||
43 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
||
44 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
||
45 | | | |
||
46 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
||
47 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | |
||
48 | * ================================================================================================ */ |
||
49 | |||
50 | // this block of memory gets written to the MPU on start-up, and it seems |
||
51 | // to be volatile memory, so it has to be done each time (it only takes ~1 |
||
52 | // second though) |
||
53 | const unsigned char mpu6050_dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { |
||
54 | // bank 0, 256 bytes |
||
55 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, |
||
56 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, |
||
57 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
58 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, |
||
59 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, |
||
60 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
61 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, |
||
62 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, |
||
63 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, |
||
64 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, |
||
65 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, |
||
66 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, |
||
67 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
68 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, |
||
69 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, |
||
70 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, |
||
71 | |||
72 | // bank 1, 256 bytes |
||
73 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
74 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, |
||
75 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, |
||
76 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, |
||
77 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, |
||
78 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, |
||
79 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, |
||
80 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
81 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
82 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
83 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, |
||
84 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, |
||
85 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, |
||
86 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
87 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
88 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, |
||
89 | |||
90 | // bank 2, 256 bytes |
||
91 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
92 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, |
||
93 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, |
||
94 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
95 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
96 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
97 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
98 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
99 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
100 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
101 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
102 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, |
||
103 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
104 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
105 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
106 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
107 | |||
108 | // bank 3, 256 bytes |
||
109 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, |
||
110 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, |
||
111 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, |
||
112 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, |
||
113 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, |
||
114 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, |
||
115 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, |
||
116 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, |
||
117 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, |
||
118 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, |
||
119 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, |
||
120 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, |
||
121 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, |
||
122 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, |
||
123 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, |
||
124 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, |
||
125 | |||
126 | // bank 4, 256 bytes |
||
127 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, |
||
128 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, |
||
129 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, |
||
130 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, |
||
131 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, |
||
132 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, |
||
133 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, |
||
134 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, |
||
135 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, |
||
136 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, |
||
137 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
||
138 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, |
||
139 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
||
140 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, |
||
141 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, |
||
142 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, |
||
143 | |||
144 | // bank 5, 256 bytes |
||
145 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, |
||
146 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, |
||
147 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, |
||
148 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, |
||
149 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, |
||
150 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, |
||
151 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, |
||
152 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, |
||
153 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, |
||
154 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, |
||
155 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, |
||
156 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, |
||
157 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, |
||
158 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, |
||
159 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, |
||
160 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, |
||
161 | |||
162 | // bank 6, 256 bytes |
||
163 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, |
||
164 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, |
||
165 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, |
||
166 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, |
||
167 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, |
||
168 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, |
||
169 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, |
||
170 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, |
||
171 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, |
||
172 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, |
||
173 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, |
||
174 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, |
||
175 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, |
||
176 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, |
||
177 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, |
||
178 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, |
||
179 | |||
180 | // bank 7, 138 bytes (remainder) |
||
181 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, |
||
182 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, |
||
183 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, |
||
184 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, |
||
185 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, |
||
186 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, |
||
187 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, |
||
188 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, |
||
189 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF |
||
190 | }; |
||
191 | |||
192 | const unsigned char mpu6050_dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { |
||
193 | // BANK OFFSET LENGTH [DATA] |
||
194 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration |
||
195 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration |
||
196 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration |
||
197 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration |
||
198 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration |
||
199 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration |
||
200 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration |
||
201 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration |
||
202 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration |
||
203 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 |
||
204 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 |
||
205 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 |
||
206 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 |
||
207 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 |
||
208 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 |
||
209 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 |
||
210 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 |
||
211 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel |
||
212 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors |
||
213 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion |
||
214 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update |
||
215 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone |
||
216 | // SPECIAL 0x01 = enable interrupts |
||
217 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION |
||
218 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt |
||
219 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion |
||
220 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer |
||
221 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro |
||
222 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo |
||
223 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo |
||
224 | 0x02, 0x16, 0x02, 0x00, 0x09 // D_0_22 inv_set_fifo_rate |
||
225 | |||
226 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, |
||
227 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. |
||
228 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) |
||
229 | |||
230 | // It is important to make sure the host processor can keep up with reading and processing |
||
231 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. |
||
232 | }; |
||
233 | |||
234 | const unsigned char mpu6050_dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { |
||
235 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, |
||
236 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, |
||
237 | 0x01, 0x6A, 0x02, 0x06, 0x00, |
||
238 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
||
239 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
||
240 | 0x01, 0x62, 0x02, 0x00, 0x00, |
||
241 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 |
||
242 | }; |
||
243 | |||
244 | /* |
||
245 | * initialize mpu6050 dmp |
||
246 | */ |
||
247 | uint8_t mpu6050_dmpInitialize() { |
||
248 | //setup interrupt |
||
249 | MPU6050_DMP_INT0SETUP; |
||
250 | |||
251 | //reset |
||
252 | mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1); |
||
253 | _delay_ms(30);//wait after reset |
||
254 | |||
255 | //disable sleep mode |
||
256 | mpu6050_setSleepDisabled(); |
||
257 | |||
258 | //set memorybank to 0 |
||
259 | mpu6050_setMemoryBank(0, 0, 0); |
||
260 | |||
261 | //get X/Y/Z gyro offsets |
||
262 | int8_t xgOffset = mpu6050_getXGyroOffset(); |
||
263 | int8_t ygOffset = mpu6050_getYGyroOffset(); |
||
264 | int8_t zgOffset = mpu6050_getZGyroOffset(); |
||
265 | |||
266 | //setting slave 0 address to 0x7F |
||
267 | mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F); |
||
268 | //disabling I2C Master mode |
||
269 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0); |
||
270 | //setting slave 0 address to 0x68 (self) |
||
271 | mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68); |
||
272 | //resetting I2C Master control |
||
273 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1); |
||
274 | _delay_ms(20); |
||
275 | |||
276 | //load DMP code into memory banks |
||
277 | if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) { |
||
278 | if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) { |
||
279 | |||
280 | //set clock source |
||
281 | mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); |
||
282 | |||
283 | //set DMP and FIFO_OFLOW interrupts enabled |
||
284 | mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12); |
||
285 | |||
286 | //set sample rate |
||
287 | mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz |
||
288 | |||
289 | //set external frame sync to TEMP_OUT_L[0] |
||
290 | mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); |
||
291 | |||
292 | //set DLPF bandwidth to 42Hz |
||
293 | mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
||
294 | |||
295 | //set gyro sensitivity to +/- 2000 deg/sec |
||
296 | mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); |
||
297 | |||
298 | //set DMP configuration bytes (function unknown) |
||
299 | mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03); |
||
300 | mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00); |
||
301 | |||
302 | //clear OTP Bank flag |
||
303 | mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0); |
||
304 | |||
305 | //set X/Y/Z gyro offsets to previous values |
||
306 | //xgOffset = 0; |
||
307 | //ygOffset = 0; |
||
308 | zgOffset = 90; |
||
309 | |||
310 | mpu6050_setXGyroOffset(xgOffset); |
||
311 | mpu6050_setYGyroOffset(ygOffset); |
||
312 | mpu6050_setZGyroOffset(zgOffset); |
||
313 | |||
314 | //set X/Y/Z gyro user offsets to zero |
||
315 | mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0); |
||
316 | mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0); |
||
317 | mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 0); |
||
318 | |||
319 | //writing final memory update 1/7 (function unknown) |
||
320 | uint8_t dmpUpdate[16], j; |
||
321 | uint16_t pos = 0; |
||
322 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
323 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
324 | |||
325 | //writing final memory update 2/7 (function unknown) |
||
326 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
327 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
328 | |||
329 | //reset FIFO |
||
330 | mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1); |
||
331 | |||
332 | //reading FIFO count |
||
333 | uint8_t fifoCount = mpu6050_getFIFOCount(); |
||
334 | uint8_t fifoBuffer[128]; |
||
335 | |||
336 | //current FIFO count |
||
337 | mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer); |
||
338 | |||
339 | //setting motion detection threshold to 2 |
||
340 | mpu6050_writeByte(MPU6050_RA_MOT_THR, 2); |
||
341 | |||
342 | //setting zero-motion detection threshold to 156 |
||
343 | mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156); |
||
344 | |||
345 | //setting motion detection duration to 80 |
||
346 | mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80); |
||
347 | |||
348 | //setting zero-motion detection duration to 0 |
||
349 | mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0); |
||
350 | |||
351 | //reset FIFO |
||
352 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
||
353 | |||
354 | //enabling FIFO |
||
355 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1); |
||
356 | |||
357 | //enabling DMP |
||
358 | mpu6050_dmpEnable(); |
||
359 | |||
360 | //resetting DMP |
||
361 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1); |
||
362 | |||
363 | //waiting for FIFO count > 2 |
||
364 | while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
||
365 | |||
366 | //writing final memory update 3/7 (function unknown) |
||
367 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
368 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
369 | |||
370 | //writing final memory update 4/7 (function unknown) |
||
371 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
372 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
373 | |||
374 | //writing final memory update 5/7 (function unknown) |
||
375 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
376 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
377 | |||
378 | //reading FIFO data...")); |
||
379 | mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
||
380 | |||
381 | //reading final memory update 6/7 (function unknown) |
||
382 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
383 | mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
||
384 | |||
385 | //waiting for FIFO count > 2 |
||
386 | while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
||
387 | |||
388 | //reading FIFO data |
||
389 | mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
||
390 | |||
391 | //writing final memory update 7/7 (function unknown) |
||
392 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
||
393 | mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
||
394 | |||
395 | //disabling DMP (you turn it on later) |
||
396 | mpu6050_dmpDisable(); |
||
397 | |||
398 | //resetting FIFO and clearing INT status one last time |
||
399 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
||
400 | } else { |
||
401 | return 2; // configuration block loading failed |
||
402 | } |
||
403 | } else { |
||
404 | return 1; // main binary block loading failed |
||
405 | } |
||
406 | return 0; // success |
||
407 | } |
||
408 | |||
409 | /* |
||
410 | * enable dmp |
||
411 | */ |
||
412 | void mpu6050_dmpEnable() { |
||
413 | MPU6050_DMP_INT0ENABLE; |
||
414 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 1); |
||
415 | } |
||
416 | |||
417 | /* |
||
418 | * disable dmp |
||
419 | */ |
||
420 | void mpu6050_dmpDisable() { |
||
421 | MPU6050_DMP_INT0DISABLE; |
||
422 | mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 0); |
||
423 | } |
||
424 | |||
425 | /* |
||
426 | * get quaternion from packet |
||
427 | */ |
||
428 | void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz) { |
||
429 | if (packet == 0) packet = dmpPacketBuffer; |
||
430 | *qw = (double)((packet[0] << 8) + packet[1]) / 16384.0f; |
||
431 | *qx = (double)((packet[4] << 8) + packet[5]) / 16384.0f; |
||
432 | *qy = (double)((packet[8] << 8) + packet[9]) / 16384.0f; |
||
433 | *qz = (double)((packet[12] << 8) + packet[13]) / 16384.0f; |
||
434 | } |
||
435 | |||
436 | /* |
||
437 | * get euler angles |
||
438 | * aerospace sequence, to obtain sensor attitude: |
||
439 | * 1. rotate around sensor Z plane by yaw |
||
440 | * 2. rotate around sensor Y plane by pitch |
||
441 | * 3. rotate around sensor X plane by roll |
||
442 | */ |
||
443 | void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw) { |
||
444 | *yaw = atan2(2*qx*qy - 2*qw*qz, 2*qw*qw + 2*qx*qx - 1); |
||
445 | *pitch = -asin(2*qx*qz + 2*qw*qy); |
||
446 | *roll = atan2(2*qy*qz - 2*qw*qx, 2*qw*qw + 2*qz*qz - 1); |
||
447 | } |
||
448 | |||
449 | /* |
||
450 | * get quaternion and wait |
||
451 | */ |
||
452 | uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz) { |
||
453 | while (!mpu6050_mpuInterrupt && mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize); |
||
454 | //reset interrupt |
||
455 | mpu6050_mpuInterrupt = 0; |
||
456 | |||
457 | //check for overflow |
||
458 | mpu6050_mpuIntStatus = mpu6050_getIntStatus(); |
||
459 | mpu6050_fifoCount = mpu6050_getFIFOCount(); |
||
460 | if ((mpu6050_mpuIntStatus & 0x10) || mpu6050_fifoCount == 1024) { |
||
461 | //reset |
||
462 | mpu6050_resetFIFO(); |
||
463 | } else if (mpu6050_mpuIntStatus & 0x02) { |
||
464 | //wait for correct available data length, should be a VERY short wait |
||
465 | while (mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize) |
||
466 | mpu6050_fifoCount = mpu6050_getFIFOCount(); |
||
467 | //read a packet from FIFO |
||
468 | mpu6050_getFIFOBytes(mpu6050_fifoBuffer, MPU6050_DMP_dmpPacketSize); |
||
469 | mpu6050_fifoCount -= MPU6050_DMP_dmpPacketSize; |
||
470 | //get quaternion |
||
471 | mpu6050_getQuaternion(mpu6050_fifoBuffer, qw, qx, qy, qz); |
||
472 | return 1; |
||
473 | } |
||
474 | |||
475 | return 0; |
||
476 | } |
||
477 | |||
478 | /* |
||
479 | * on interrupt set data availabe |
||
480 | */ |
||
481 | ISR (INT0_vect) { |
||
482 | mpu6050_mpuInterrupt = 1; |
||
483 | } |
||
484 | |||
485 | #endif |