1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
|
2 | // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
|
3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
4 | //
|
5 | // Changelog:
|
6 | // 2019-07-08 - Added Auto Calibration and offset generator
|
7 | // - and altered FIFO retrieval sequence to avoid using blocking code
|
8 | // 2016-04-18 - Eliminated a potential infinite loop
|
9 | // 2013-05-08 - added seamless Fastwire support
|
10 | // - added note about gyro calibration
|
11 | // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
|
12 | // 2012-06-20 - improved FIFO overflow handling and simplified read process
|
13 | // 2012-06-19 - completely rearranged DMP initialization code and simplification
|
14 | // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
|
15 | // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
|
16 | // 2012-06-05 - add gravity-compensated initial reference frame acceleration output
|
17 | // - add 3D math helper file to DMP6 example sketch
|
18 | // - add Euler output and Yaw/Pitch/Roll output formats
|
19 | // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
|
20 | // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
|
21 | // 2012-05-30 - basic DMP initialization working
|
22 |
|
23 | /* ============================================
|
24 | I2Cdev device library code is placed under the MIT license
|
25 | Copyright (c) 2012 Jeff Rowberg
|
26 | Permission is hereby granted, free of charge, to any person obtaining a copy
|
27 | of this software and associated documentation files (the "Software"), to deal
|
28 | in the Software without restriction, including without limitation the rights
|
29 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
30 | copies of the Software, and to permit persons to whom the Software is
|
31 | furnished to do so, subject to the following conditions:
|
32 | The above copyright notice and this permission notice shall be included in
|
33 | all copies or substantial portions of the Software.
|
34 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
35 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
36 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
37 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
38 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
39 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
40 | THE SOFTWARE.
|
41 | ===============================================
|
42 | */
|
43 |
|
44 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
45 | // for both classes must be in the include path of your project
|
46 | #include "I2Cdev.h"
|
47 |
|
48 | #include "MPU6050_6Axis_MotionApps20.h"
|
49 | //#include "MPU6050.h" // not necessary if using MotionApps include file
|
50 |
|
51 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
52 | // is used in I2Cdev.h
|
53 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
54 | #include "Wire.h"
|
55 | #endif
|
56 |
|
57 | // class default I2C address is 0x68
|
58 | // specific I2C addresses may be passed as a parameter here
|
59 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
|
60 | // AD0 high = 0x69
|
61 | MPU6050 mpu;
|
62 | //MPU6050 mpu(0x69); // <-- use for AD0 high
|
63 |
|
64 | /* =========================================================================
|
65 | NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
|
66 | depends on the MPU-6050's INT pin being connected to the Arduino's
|
67 | external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
|
68 | digital I/O pin 2.
|
69 | * ========================================================================= */
|
70 |
|
71 | /* =========================================================================
|
72 | NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
|
73 | when using Serial.write(buf, len). The Teapot output uses this method.
|
74 | The solution requires a modification to the Arduino USBAPI.h file, which
|
75 | is fortunately simple, but annoying. This will be fixed in the next IDE
|
76 | release. For more info, see these links:
|
77 | http://arduino.cc/forum/index.php/topic,109987.0.html
|
78 | http://code.google.com/p/arduino/issues/detail?id=958
|
79 | * ========================================================================= */
|
80 |
|
81 |
|
82 |
|
83 | // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
84 | // quaternion components in a [w, x, y, z] format (not best for parsing
|
85 | // on a remote host such as Processing or something though)
|
86 | //#define OUTPUT_READABLE_QUATERNION
|
87 |
|
88 | // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
89 | // (in degrees) calculated from the quaternions coming from the FIFO.
|
90 | // Note that Euler angles suffer from gimbal lock (for more info, see
|
91 | // http://en.wikipedia.org/wiki/Gimbal_lock)
|
92 | //#define OUTPUT_READABLE_EULER
|
93 |
|
94 | // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
95 | // pitch/roll angles (in degrees) calculated from the quaternions coming
|
96 | // from the FIFO. Note this also requires gravity vector calculations.
|
97 | // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
98 | // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
99 | #define OUTPUT_READABLE_YAWPITCHROLL
|
100 |
|
101 | // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
102 | // components with gravity removed. This acceleration reference frame is
|
103 | // not compensated for orientation, so +X is always +X according to the
|
104 | // sensor, just without the effects of gravity. If you want acceleration
|
105 | // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
106 | //#define OUTPUT_READABLE_REALACCEL
|
107 |
|
108 | // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
109 | // components with gravity removed and adjusted for the world frame of
|
110 | // reference (yaw is relative to initial orientation, since no magnetometer
|
111 | // is present in this case). Could be quite handy in some cases.
|
112 | //#define OUTPUT_READABLE_WORLDACCEL
|
113 |
|
114 | // uncomment "OUTPUT_TEAPOT" if you want output that matches the
|
115 | // format used for the InvenSense teapot demo
|
116 | //#define OUTPUT_TEAPOT
|
117 |
|
118 |
|
119 |
|
120 | #define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
|
121 | #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
|
122 | bool blinkState = false;
|
123 |
|
124 | // MPU control/status vars
|
125 | bool dmpReady = false; // set true if DMP init was successful
|
126 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
127 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
|
128 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
129 | uint16_t fifoCount; // count of all bytes currently in FIFO
|
130 | uint8_t fifoBuffer[64]; // FIFO storage buffer
|
131 |
|
132 | // orientation/motion vars
|
133 | Quaternion q; // [w, x, y, z] quaternion container
|
134 | VectorInt16 aa; // [x, y, z] accel sensor measurements
|
135 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
136 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
137 | VectorFloat gravity; // [x, y, z] gravity vector
|
138 | float euler[3]; // [psi, theta, phi] Euler angle container
|
139 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
140 |
|
141 | // packet structure for InvenSense teapot demo
|
142 | uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
|
143 |
|
144 |
|
145 |
|
146 | // ================================================================
|
147 | // === INTERRUPT DETECTION ROUTINE ===
|
148 | // ================================================================
|
149 |
|
150 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
|
151 | void dmpDataReady() {
|
152 | mpuInterrupt = true;
|
153 | }
|
154 |
|
155 |
|
156 |
|
157 | // ================================================================
|
158 | // === INITIAL SETUP ===
|
159 | // ================================================================
|
160 |
|
161 | void setup() {
|
162 | // join I2C bus (I2Cdev library doesn't do this automatically)
|
163 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
164 | Wire.begin();
|
165 | Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
166 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
167 | Fastwire::setup(400, true);
|
168 | #endif
|
169 |
|
170 | // initialize serial communication
|
171 | // (115200 chosen because it is required for Teapot Demo output, but it's
|
172 | // really up to you depending on your project)
|
173 | Serial.begin(115200);
|
174 | while (!Serial); // wait for Leonardo enumeration, others continue immediately
|
175 |
|
176 | // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
|
177 | // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
|
178 | // the baud timing being too misaligned with processor ticks. You must use
|
179 | // 38400 or slower in these cases, or use some kind of external separate
|
180 | // crystal solution for the UART timer.
|
181 |
|
182 | // initialize device
|
183 | Serial.println(F("Initializing I2C devices..."));
|
184 | mpu.initialize();
|
185 | pinMode(INTERRUPT_PIN, INPUT);
|
186 |
|
187 | // verify connection
|
188 | Serial.println(F("Testing device connections..."));
|
189 | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
190 |
|
191 | // wait for ready
|
192 | Serial.println(F("\nSend any character to begin DMP programming and demo: "));
|
193 | while (Serial.available() && Serial.read()); // empty buffer
|
194 | while (!Serial.available()); // wait for data
|
195 | while (Serial.available() && Serial.read()); // empty buffer again
|
196 |
|
197 | // load and configure the DMP
|
198 | Serial.println(F("Initializing DMP..."));
|
199 | devStatus = mpu.dmpInitialize();
|
200 |
|
201 | // supply your own gyro offsets here, scaled for min sensitivity
|
202 | mpu.setXGyroOffset(220);
|
203 | mpu.setYGyroOffset(76);
|
204 | mpu.setZGyroOffset(-85);
|
205 | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
|
206 |
|
207 | // make sure it worked (returns 0 if so)
|
208 | if (devStatus == 0) {
|
209 | // Calibration Time: generate offsets and calibrate our MPU6050
|
210 | mpu.CalibrateAccel(6);
|
211 | mpu.CalibrateGyro(6);
|
212 | mpu.PrintActiveOffsets();
|
213 | // turn on the DMP, now that it's ready
|
214 | Serial.println(F("Enabling DMP..."));
|
215 | mpu.setDMPEnabled(true);
|
216 |
|
217 | // enable Arduino interrupt detection
|
218 | Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
|
219 | Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
|
220 | Serial.println(F(")..."));
|
221 | attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
|
222 | mpuIntStatus = mpu.getIntStatus();
|
223 |
|
224 | // set our DMP Ready flag so the main loop() function knows it's okay to use it
|
225 | Serial.println(F("DMP ready! Waiting for first interrupt..."));
|
226 | dmpReady = true;
|
227 |
|
228 | // get expected DMP packet size for later comparison
|
229 | packetSize = mpu.dmpGetFIFOPacketSize();
|
230 | } else {
|
231 | // ERROR!
|
232 | // 1 = initial memory load failed
|
233 | // 2 = DMP configuration updates failed
|
234 | // (if it's going to break, usually the code will be 1)
|
235 | Serial.print(F("DMP Initialization failed (code "));
|
236 | Serial.print(devStatus);
|
237 | Serial.println(F(")"));
|
238 | }
|
239 |
|
240 | // configure LED for output
|
241 | pinMode(LED_PIN, OUTPUT);
|
242 | }
|
243 |
|
244 |
|
245 |
|
246 | // ================================================================
|
247 | // === MAIN PROGRAM LOOP ===
|
248 | // ================================================================
|
249 |
|
250 | void loop() {
|
251 | // if programming failed, don't try to do anything
|
252 | if (!dmpReady) return;
|
253 | // read a packet from FIFO
|
254 | if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
|
255 | #ifdef OUTPUT_READABLE_QUATERNION
|
256 | // display quaternion values in easy matrix form: w x y z
|
257 | mpu.dmpGetQuaternion(&q, fifoBuffer);
|
258 | Serial.print("quat\t");
|
259 | Serial.print(q.w);
|
260 | Serial.print("\t");
|
261 | Serial.print(q.x);
|
262 | Serial.print("\t");
|
263 | Serial.print(q.y);
|
264 | Serial.print("\t");
|
265 | Serial.println(q.z);
|
266 | #endif
|
267 |
|
268 | #ifdef OUTPUT_READABLE_EULER
|
269 | // display Euler angles in degrees
|
270 | mpu.dmpGetQuaternion(&q, fifoBuffer);
|
271 | mpu.dmpGetEuler(euler, &q);
|
272 | Serial.print("euler\t");
|
273 | Serial.print(euler[0] * 180/M_PI);
|
274 | Serial.print("\t");
|
275 | Serial.print(euler[1] * 180/M_PI);
|
276 | Serial.print("\t");
|
277 | Serial.println(euler[2] * 180/M_PI);
|
278 | #endif
|
279 |
|
280 | #ifdef OUTPUT_READABLE_YAWPITCHROLL
|
281 | // display Euler angles in degrees
|
282 | mpu.dmpGetQuaternion(&q, fifoBuffer);
|
283 | mpu.dmpGetGravity(&gravity, &q);
|
284 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
285 | Serial.print("ypr\t");
|
286 | Serial.print(ypr[0] * 180/M_PI);
|
287 | Serial.print("\t");
|
288 | Serial.print(ypr[1] * 180/M_PI);
|
289 | Serial.print("\t");
|
290 | Serial.println(ypr[2] * 180/M_PI);
|
291 | #endif
|
292 |
|
293 | #ifdef OUTPUT_READABLE_REALACCEL
|
294 | // display real acceleration, adjusted to remove gravity
|
295 | mpu.dmpGetQuaternion(&q, fifoBuffer);
|
296 | mpu.dmpGetAccel(&aa, fifoBuffer);
|
297 | mpu.dmpGetGravity(&gravity, &q);
|
298 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
299 | Serial.print("areal\t");
|
300 | Serial.print(aaReal.x);
|
301 | Serial.print("\t");
|
302 | Serial.print(aaReal.y);
|
303 | Serial.print("\t");
|
304 | Serial.println(aaReal.z);
|
305 | #endif
|
306 |
|
307 | #ifdef OUTPUT_READABLE_WORLDACCEL
|
308 | // display initial world-frame acceleration, adjusted to remove gravity
|
309 | // and rotated based on known orientation from quaternion
|
310 | mpu.dmpGetQuaternion(&q, fifoBuffer);
|
311 | mpu.dmpGetAccel(&aa, fifoBuffer);
|
312 | mpu.dmpGetGravity(&gravity, &q);
|
313 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
314 | mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
315 | Serial.print("aworld\t");
|
316 | Serial.print(aaWorld.x);
|
317 | Serial.print("\t");
|
318 | Serial.print(aaWorld.y);
|
319 | Serial.print("\t");
|
320 | Serial.println(aaWorld.z);
|
321 | #endif
|
322 |
|
323 | #ifdef OUTPUT_TEAPOT
|
324 | // display quaternion values in InvenSense Teapot demo format:
|
325 | teapotPacket[2] = fifoBuffer[0];
|
326 | teapotPacket[3] = fifoBuffer[1];
|
327 | teapotPacket[4] = fifoBuffer[4];
|
328 | teapotPacket[5] = fifoBuffer[5];
|
329 | teapotPacket[6] = fifoBuffer[8];
|
330 | teapotPacket[7] = fifoBuffer[9];
|
331 | teapotPacket[8] = fifoBuffer[12];
|
332 | teapotPacket[9] = fifoBuffer[13];
|
333 | Serial.write(teapotPacket, 14);
|
334 | teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
|
335 | #endif
|
336 |
|
337 | // blink LED to indicate activity
|
338 | blinkState = !blinkState;
|
339 | digitalWrite(LED_PIN, blinkState);
|
340 | }
|
341 | }
|