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. //      2013-05-08 - added seamless Fastwire support
  7. //                 - added note about gyro calibration
  8. //      2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
  9. //      2012-06-20 - improved FIFO overflow handling and simplified read process
  10. //      2012-06-19 - completely rearranged DMP initialization code and simplification
  11. //      2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
  12. //      2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
  13. //      2012-06-05 - add gravity-compensated initial reference frame acceleration output
  14. //                 - add 3D math helper file to DMP6 example sketch
  15. //                 - add Euler output and Yaw/Pitch/Roll output formats
  16. //      2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
  17. //      2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
  18. //      2012-05-30 - basic DMP initialization working
  19.  
  20. /* ============================================
  21. I2Cdev device library code is placed under the MIT license
  22. Copyright (c) 2012 Jeff Rowberg
  23.  
  24. Permission is hereby granted, free of charge, to any person obtaining a copy
  25. of this software and associated documentation files (the "Software"), to deal
  26. in the Software without restriction, including without limitation the rights
  27. to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  28. copies of the Software, and to permit persons to whom the Software is
  29. furnished to do so, subject to the following conditions:
  30.  
  31. The above copyright notice and this permission notice shall be included in
  32. all copies or substantial portions of the Software.
  33.  
  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.  
  78.    http://arduino.cc/forum/index.php/topic,109987.0.html
  79.    http://code.google.com/p/arduino/issues/detail?id=958
  80.  * ========================================================================= */
  81.  
  82.  
  83.  
  84. // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
  85. // quaternion components in a [w, x, y, z] format (not best for parsing
  86. // on a remote host such as Processing or something though)
  87. //#define OUTPUT_READABLE_QUATERNION
  88.  
  89. // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
  90. // (in degrees) calculated from the quaternions coming from the FIFO.
  91. // Note that Euler angles suffer from gimbal lock (for more info, see
  92. // http://en.wikipedia.org/wiki/Gimbal_lock)
  93. //#define OUTPUT_READABLE_EULER
  94.  
  95. // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
  96. // pitch/roll angles (in degrees) calculated from the quaternions coming
  97. // from the FIFO. Note this also requires gravity vector calculations.
  98. // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
  99. // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
  100. #define OUTPUT_READABLE_YAWPITCHROLL
  101.  
  102. // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
  103. // components with gravity removed. This acceleration reference frame is
  104. // not compensated for orientation, so +X is always +X according to the
  105. // sensor, just without the effects of gravity. If you want acceleration
  106. // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
  107. //#define OUTPUT_READABLE_REALACCEL
  108.  
  109. // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
  110. // components with gravity removed and adjusted for the world frame of
  111. // reference (yaw is relative to initial orientation, since no magnetometer
  112. // is present in this case). Could be quite handy in some cases.
  113. //#define OUTPUT_READABLE_WORLDACCEL
  114.  
  115. // uncomment "OUTPUT_TEAPOT" if you want output that matches the
  116. // format used for the InvenSense teapot demo
  117. //#define OUTPUT_TEAPOT
  118.  
  119.  
  120.  
  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.         TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  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.  
  186.     // verify connection
  187.     Serial.println(F("Testing device connections..."));
  188.     Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  189.  
  190.     // wait for ready
  191.     Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  192.     while (Serial.available() && Serial.read()); // empty buffer
  193.     while (!Serial.available());                 // wait for data
  194.     while (Serial.available() && Serial.read()); // empty buffer again
  195.  
  196.     // load and configure the DMP
  197.     Serial.println(F("Initializing DMP..."));
  198.     devStatus = mpu.dmpInitialize();
  199.  
  200.     // supply your own gyro offsets here, scaled for min sensitivity
  201.     mpu.setXGyroOffset(220);
  202.     mpu.setYGyroOffset(76);
  203.     mpu.setZGyroOffset(-85);
  204.     mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
  205.  
  206.     // make sure it worked (returns 0 if so)
  207.     if (devStatus == 0) {
  208.         // turn on the DMP, now that it's ready
  209.         Serial.println(F("Enabling DMP..."));
  210.         mpu.setDMPEnabled(true);
  211.  
  212.         // enable Arduino interrupt detection
  213.         Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
  214.         attachInterrupt(0, dmpDataReady, RISING);
  215.         mpuIntStatus = mpu.getIntStatus();
  216.  
  217.         // set our DMP Ready flag so the main loop() function knows it's okay to use it
  218.         Serial.println(F("DMP ready! Waiting for first interrupt..."));
  219.         dmpReady = true;
  220.  
  221.         // get expected DMP packet size for later comparison
  222.         packetSize = mpu.dmpGetFIFOPacketSize();
  223.     } else {
  224.         // ERROR!
  225.         // 1 = initial memory load failed
  226.         // 2 = DMP configuration updates failed
  227.         // (if it's going to break, usually the code will be 1)
  228.         Serial.print(F("DMP Initialization failed (code "));
  229.         Serial.print(devStatus);
  230.         Serial.println(F(")"));
  231.     }
  232.  
  233.     // configure LED for output
  234.     pinMode(LED_PIN, OUTPUT);
  235. }
  236.  
  237.  
  238.  
  239. // ================================================================
  240. // ===                    MAIN PROGRAM LOOP                     ===
  241. // ================================================================
  242.  
  243. void loop() {
  244.     // if programming failed, don't try to do anything
  245.     if (!dmpReady) return;
  246.  
  247.     // wait for MPU interrupt or extra packet(s) available
  248.     while (!mpuInterrupt && fifoCount < packetSize) {
  249.         // other program behavior stuff here
  250.         // .
  251.         // .
  252.         // .
  253.         // if you are really paranoid you can frequently test in between other
  254.         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
  255.         // while() loop to immediately process the MPU data
  256.         // .
  257.         // .
  258.         // .
  259.     }
  260.  
  261.     // reset interrupt flag and get INT_STATUS byte
  262.     mpuInterrupt = false;
  263.     mpuIntStatus = mpu.getIntStatus();
  264.  
  265.     // get current FIFO count
  266.     fifoCount = mpu.getFIFOCount();
  267.  
  268.     // check for overflow (this should never happen unless our code is too inefficient)
  269.     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
  270.         // reset so we can continue cleanly
  271.         mpu.resetFIFO();
  272.         Serial.println(F("FIFO overflow!"));
  273.  
  274.     // otherwise, check for DMP data ready interrupt (this should happen frequently)
  275.     } else if (mpuIntStatus & 0x02) {
  276.         // wait for correct available data length, should be a VERY short wait
  277.         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  278.  
  279.         // read a packet from FIFO
  280.         mpu.getFIFOBytes(fifoBuffer, packetSize);
  281.  
  282.         // track FIFO count here in case there is > 1 packet available
  283.         // (this lets us immediately read more without waiting for an interrupt)
  284.         fifoCount -= packetSize;
  285.  
  286.         #ifdef OUTPUT_READABLE_QUATERNION
  287.             // display quaternion values in easy matrix form: w x y z
  288.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  289.             Serial.print("quat\t");
  290.             Serial.print(q.w);
  291.             Serial.print("\t");
  292.             Serial.print(q.x);
  293.             Serial.print("\t");
  294.             Serial.print(q.y);
  295.             Serial.print("\t");
  296.             Serial.println(q.z);
  297.         #endif
  298.  
  299.         #ifdef OUTPUT_READABLE_EULER
  300.             // display Euler angles in degrees
  301.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  302.             mpu.dmpGetEuler(euler, &q);
  303.             Serial.print("euler\t");
  304.             Serial.print(euler[0] * 180/M_PI);
  305.             Serial.print("\t");
  306.             Serial.print(euler[1] * 180/M_PI);
  307.             Serial.print("\t");
  308.             Serial.println(euler[2] * 180/M_PI);
  309.         #endif
  310.  
  311.         #ifdef OUTPUT_READABLE_YAWPITCHROLL
  312.             // display Euler angles in degrees
  313.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  314.             mpu.dmpGetGravity(&gravity, &q);
  315.             mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  316.             Serial.print("ypr\t");
  317.             Serial.print(ypr[0] * 180/M_PI);
  318.             Serial.print("\t");
  319.             Serial.print(ypr[1] * 180/M_PI);
  320.             Serial.print("\t");
  321.             Serial.println(ypr[2] * 180/M_PI);
  322.         #endif
  323.  
  324.         #ifdef OUTPUT_READABLE_REALACCEL
  325.             // display real acceleration, adjusted to remove gravity
  326.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  327.             mpu.dmpGetAccel(&aa, fifoBuffer);
  328.             mpu.dmpGetGravity(&gravity, &q);
  329.             mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  330.             Serial.print("areal\t");
  331.             Serial.print(aaReal.x);
  332.             Serial.print("\t");
  333.             Serial.print(aaReal.y);
  334.             Serial.print("\t");
  335.             Serial.println(aaReal.z);
  336.         #endif
  337.  
  338.         #ifdef OUTPUT_READABLE_WORLDACCEL
  339.             // display initial world-frame acceleration, adjusted to remove gravity
  340.             // and rotated based on known orientation from quaternion
  341.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  342.             mpu.dmpGetAccel(&aa, fifoBuffer);
  343.             mpu.dmpGetGravity(&gravity, &q);
  344.             mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  345.             mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
  346.             Serial.print("aworld\t");
  347.             Serial.print(aaWorld.x);
  348.             Serial.print("\t");
  349.             Serial.print(aaWorld.y);
  350.             Serial.print("\t");
  351.             Serial.println(aaWorld.z);
  352.         #endif
  353.  
  354.         #ifdef OUTPUT_TEAPOT
  355.             // display quaternion values in InvenSense Teapot demo format:
  356.             teapotPacket[2] = fifoBuffer[0];
  357.             teapotPacket[3] = fifoBuffer[1];
  358.             teapotPacket[4] = fifoBuffer[4];
  359.             teapotPacket[5] = fifoBuffer[5];
  360.             teapotPacket[6] = fifoBuffer[8];
  361.             teapotPacket[7] = fifoBuffer[9];
  362.             teapotPacket[8] = fifoBuffer[12];
  363.             teapotPacket[9] = fifoBuffer[13];
  364.             Serial.write(teapotPacket, 14);
  365.             teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
  366.         #endif
  367.  
  368.         // blink LED to indicate activity
  369.         blinkState = !blinkState;
  370.         digitalWrite(LED_PIN, blinkState);
  371.     }
  372. }
  373.  

Share