MPU-6050: DMP Data from i2cdevlib

In March, I posted on experimenting with the MPU-6050 IMU chip (mounted in a GY-521 breakout board).  It seems that many people are using the MPU-6050, and I wanted to follow up with some more information, because there are better ways to access and process the combined sensor data than were demonstrated in that post.  The previous experiment compared the raw data from the 3-axis accelerometer and 3-axis gyroscope sensors to the results when the raw data are combined via a complementary filter.

For the comparison, I had adapted a program from the Arduino Playground Wiki on MPU-6050 to pull the raw accelerometer and gyroscope data from the MPU-6050,  The program calculated pitch, roll and yaw (rotation about the X, Y and Z axes, respectively, also knows as Euler Angles).  These calculations were limited by certain properties of both the accelerometer and gyroscope.  Gyroscope data has a tendency to drift with time, and accelerometer data is noisy and makes it difficult to calculate rotations over ranges greater than 180 degrees.  The formulas(*) for computing roll, $latex \phi$, and pitch, $latex \rho$, (you can’t compute yaw, $latex \theta$ from accelerometer data) from the accelerometer readings $latex (A_{X}, A_{Y}, A_{Z})$ are:

$latex \phi = \arctan{\left(\frac{A_{X}}{\sqrt{A_{Y}^{2} + A_{Z}^{2}}}\right)} $      $latex \rho= \arctan{\left(\frac{A_{Y}}{\sqrt{A_{X}^{2} + A_{Z}^{2}}}\right)} $

The range of the $latex \arctan$ function is $latex \{-\pi/2, \pi/2\}$, which determines the range of motion you can detect easily from the IMU using these functions.  Additionally, using Euler angles for the rotation calculations can lead to a problem known as Gimbal Lock.  Because Euler angle rotations are performed in sequence (e.g, roll, pitch, and then yaw), problems arise when the second rotation in the sequence approaches 90 degrees ($latex \pi/2$), because this brings first and third rotation axes into alignment with each other.  This concept is tricky to explain, but the YouTube video below does quite a good job with it:

One way to avoid the problems with Euler angles is to use an alternate method of representing rotation called quaternions.  Quaternions describe rotation in three dimensions by using four scalar values. Three of these scalars define an axis, and the fourth specifies a rotation around that axis.  I won’t go into the mathematics of quaternions which is fairly complicated, and which I’m just starting to learn myself.  The salient points of quaternions are that they avoid the problem of gimbal lock and can represent rotations over ranges of greater than 180 degrees without difficulty.  Additionally, they have some useful mathematical properties, such as allowing for straightforward interpolation between two quaternions, which can be useful in animation, and reducing round-off error when multiple rotation operations are performed in sequence.

Using the DMP for Data Processing

The MPU6050 IMU contains a DMP (Digital Motion Processor) which fuses the accelerometer and gyroscope data together to minimize the effects of errors inherent in each sensor.  The DMP computes the results in terms of quaternions, and can convert the results to Euler angles and perform other computations with the data as well. Invensense will not reveal the algorithm that they use to combine the data, but I was curious to see how the results the DMP produces compare to those I had computed with a complementary filter.

Therefore the next step was to figure out how to extract data from the DMP.  Fortunately, Jeff Rowberg has written and made public a very useful library, i2cdevlib, which does just that.  It is available for download at:  https://github.com/jrowberg/i2cdevlib.  To use it with the MPU 6050, you will need the library functions in the folder labeled Arduino/MPU6050.  To install the library, simply copy the Arduino/MPU6050 folder to the same location as your other Arduino libraries.

I used an Arduino Nano and GY-521 breakout board containing the MPU-6050 to run the demos.  This setup requires the following connections:  (Arduino<->GY-521)

5 Connections are required between the MPU-6050 and Arduino when using i2cdevlib.
5 connections are required between the MPU-6050 and Arduino when using i2cdevlib.
  • 5V<->VCC (the GY-521 contains a voltage regulator and can use 3.3V or 5V)
  • GND<->GND
  • A5<->SCL
  • A4<->SDA
  • Arduino Pin 2<->INT (used for interrupts)

The Teapot Demo

The MPU6050 library contains a folder called “Examples”, with a demo called MPU6050_DMP6.  Inside that folder are an Arduino sketch, MPU6050_DMP6.ino, to read quaternion data from the DMP and send it over the serial port, and a Processing sketch, MPUTeapot.pde, to read the data from the serial port and display the rotations graphically.

The demo ran more or less right out of the box, with a few tweaks.  The only change I made to the Arduino sketch was to reduce the Serial port communication rate from 115200 to 38400 (This is set on line 163 of the sketch) .  The 115200 rate seemed to overwhelm the Nano, but the 38400 rate worked well.

The Processing sketch is called MPUTeapot.pde, even though the figure it displays is a 3-D arrow, not a teapot.  I assume that earlier versions of this sketch showed a teapot.  I’ve seen some working demos with the teapot version online, e.g. this YouTube video:

If you are going to run this example, make sure you use the 32-bit version of Processing, since last I checked, it is the only version that works with serial communications.

To get the Processing sketch working, I had to make the following changes:

  • Changed the Serial port data rate to 38400 to match the Arduino rate.  I also specified which serial port to read the data from.  
  • The data, which comes across the Serial port in 14 byte packets was not getting properly aligned.  Each 14-byte packet starts with the ‘$’ character, so I modified the program to ignore all bytes read before the first ‘$’.
  • The Processing sketch sends a character over the serial port to the Arduino to tell it to begin sending data.  I found that the sketch was hanging here, so I had the Processing sketch send over a couple of additional characters for good measure.  It seemed to fix the problem.  You can download my slightly modified version of the Processing sketch here:  

Below is a video demonstration of the “teapot” demo sketches.  The 3-D airplane/arrow in the Processing sketch follows the rotation of the IMU without significant jitter or lag.  In addition, the demo easily performs rotations of any angle, even those greater than 180 degrees.  What was also interesting is that for the first few seconds, the figure shows significant yaw drift, but after about 8 seconds of keeping the IMU motionless, the drift stops.  I presume the DMP performs some sort of calibration to correct for the yaw drift.  The complementary filter I had used in my previous post was able to correct for gyroscope drift in pitch and roll by combining accelerometer and gyroscope data on these axes, but since accelerometers don’t provide yaw, it was not able to correct for the yaw drift.  I think that, unless you have good calibration and data fusion algorithms, there is a significant advantage to using the DMP calculated data.

I am working on a side-by-side comparison of DMP data vs. the complementary filter discussed in my previous post about the MPU-6050, and hope to be writing it up soon.

(*)  As an unrelated aside, I’ve just discovered the use of LaTeX for formatting equations within WordPress, and I’m having a lot of fun with it.

154 thoughts on “MPU-6050: DMP Data from i2cdevlib

  1. Hi, I actually have a question regarding print statements. I understand that Rowberg’s data packet comes out with a ‘$’ first, but how do his initialization statements get printed? Won’t they trip your gotFirstByte check and get ignored?

    1. Hi,
      I’m on vacation for the next few days, without easy access to the code, but off the top of my head, I believe that the code does just ignore the DMP initialization statements on the Processing side.
      Debra

  2. How much is the yaw drift of the MPU-6050 DMP output (during your experiments)?

    I’m looking for a gyro that has a minimal yaw drift, currently I’m using a L3G4200D and it has a yaw drift of 1-2 degree per minute. Of course, I’m using calibration. I cannot use a compass, because there are large motors in this small robot, and there’s no chance to measure anything else magnetic for a compass than the motors…

    So how large is the yaw drift in the MPU-6050 (degress per minute) compared to by L3G4200D (1-2 degree yaw drift per second) – Thanks 🙂

    1. I know this is an extremely late reply, sorry. I never explicitly measured the yaw drift. The yaw drift was somewhat dependent on the magnitude and angular velocity of the MPU-6050’s yaw rotation, though it was pretty small with the i2cdevlib library. I don’t have exact numbers for you though.
      Debra

  3. Super post. Well done. Im working with the the MPU6050 myself. I’ve just started streaming raw data and hope to move onto more complex stuff soon. I have written a Qt class to stream serial data from an Arduino for use in the Qt environment. The whole project is still in the nascent stages but I hope to write up my findings on my blog soon.

    Saw your post on the 3D printer as well. Great Stuff.

  4. Hi Debra!

    Thank you very much for your great posts about IMUs, I highly appreciate that you are sharing your experiences with us.

    As I’m quit new to microcontrollers and IMUs bouth your posts about this topic support my actual semester project very much and save some of my rare students-freetime 🙂

    Best wishes from Switzerland!

  5. I’ve developed the system on ATmega processor using MPU 6050.
    It works exactly the same as on the video.
    So I plased the device on the shuttle and start to swing it in roll axis.
    I couldn’observe significant change in roll angle and I must add that real angles were really big.
    I don,t understand what really happen.
    Can You explain the reason?

    1. Sorry for the belated response. I’m afraid I don’t have a good answer as to why you wouldn’t see changes in roll other than a faulty IMU. I never experienced a problem with only one axis failing to respond correctly. I hope you’ve figured it out by now.

  6. Hi there,
    I’m afraid I’m completely new to using arduinos so i’m having a few problems. I did exactly what you said (copying the MPU6050 library into the location of my other libraries) but when i try to compile it comes up with lots of errors and says
    “In file included from MPU6050.cpp:37:
    /MPU6050.h:40:20: error: I2Cdev.h: No such file or directory”
    I have tried adding the whole of the I2Cdev library to my library folder but I still get this problem. any ideas?

    Cheers

  7. i try test thats but error come alltime ardunio ide v103 uno board.
    error= MPU does not name a type
    and row have= MPU6050 mpu;
    what must do :O ??????

  8. Dear Sir,
    I was very impressed by his Empowerment Website Project and I would love to make it happen in my school. I have already bought all of the components described by her and I miss only the code to make it work. I would be very grateful if you please share this with me.
    Waiting for his kind reply,
    sending greetings.
    Gerry Mazzotta.

  9. I am sorry, I happened to put my Reply at the wrong thread. I posted it under “About” for some reason. 🙁

  10. “This post talks about where to put the toxi libraries. Hopefully it will help:
    http://stackoverflow.com/questions/4093706/add-toxi-library-to-processing

    Thanks for your fast respons, the link above helped me to put the libraries in the correct folder.
    So when running Processing and the Teapotdemo I get a nice graph of the Arrow.
    But it is frozen.
    I believe that the Communication between the Arduino UNO R3 and the MPU6050 is working. From the ArduinoIDE I can see that the program is waiting for a character to be sent.
    And when I send a character from the Serial Monitor, I can read output from the MPU on the Serial Monitor

    1. My only recommendation is to try to modify the sketch on the Processing side to see if the data is actually getting through to the Processing IDE. Try adding some code to the Processing sketch to have it display the data it is receiving. Sorry if that’s not much help, but it’s hard to know exactly where the problem is.

      1. These are two errors coming and my arrow is frozen too.

        The field PConstants.OPENGL is deprecated ——-in line 62 of your modified code
        Type String[] of the last argument to method println(Object…) doesn’t exactly match the vararg parameter type. Cast to Object[] to confirm the non-varargs invocation, or pass individual arguments of type Object for a varargs invocation. ——-in line 70 of your modified code.

        My arrow is also frozen.
        I am using processing 3 32 bit version.

        Thanks.

        1. That code is so 2013 and suitable for libraries of that time unless you modify it for your time. What hardware setup you have?

  11. Hi,
    I am relatively new to this but have been really impressed with the site. I have tried to replicate the Teapot demo however I cannot seem to get it working.

    When running the Arduino sketch my Serial Monitor returns a variation of this;

    ‘ú,žŸ®þéNšÅ”ý™—ƒ ‘

    I downloaded and installed Processing 2.0 and have also downloaded the ToxicLibs files though I haven’t been able to find the location to put them in so that Processing will recognise they are there, currently when trying to run Processing I will get the error message;

    No library found for toxi.geom
    No library found for toxi.processing
    Libraries must be installed in a folder named ‘libraries’ inside the ‘sketchbook’ folder.

    If anyone has a step by step guide or better yet a file map of where each folder and file goes that would help greatly,
    Thanks,
    Dave

  12. Hi,

    I also have this problem and have tried several different ways to get the library into folders (both sketch and core library) without success. I’m running Processing 2.1.1. I downloaded toxiclibs-complete-0020.zip and unzipped the files to processing-2.1.1/core/library folder and to a libraries folder in MPUteapot folder (sketch) and still get the same problem and messages described by Dave.

    Looks like Dave and I could use a little help with this.

    Thanks
    Larry

  13. Hi everybody,

    I just implemented the DMP portion of the i2c dev library in a piece of Software on my Beaglebone to use the DMP of the MPU6050. I am especially interestet in pitch and roll values. The values calculated via the function dmpGetYawPitchRoll() are not very stable. a slight movement results in overshots in pos/neg. directions. After a while of leaving the sensor static at the rached angle settles the value, but as soon as I move the sensor the values jump around again until I keep the sensor static and then they settle based on the actually reached position. I also tried to calculate pitch and roll (and yaw) from the quaternions directly, but with the same result.


    pIMU->getFIFOBytes(fifoBuffer, packetSize);
    pIMU->dmpGetQuaternion(&q, fifoBuffer);
    pIMU->dmpGetGravity(&gravity, &q);
    pIMU->dmpGetYawPitchRoll(ypr, &q, &gravity);

    qroll = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w – q.x*q.x – q.y*q.y + q.z*q.z);
    qpitch = -1.0 * asin(-2.0*(q.x*q.z – q.w*q.y));
    qyaw = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x – q.y*q.y – q.z*q.z);

    Whenever I move the sensor it is very very sensitive to changes. When I e.g. move the sensor from horizontal postion slightly up to an angle of 45° pitch (3-4 seconds to reach the angle). I get overshoots to +80° and -60°. After approx 2-3 seconds the value settles to the expected 45°.

    Previously I used the raw acc/gyro values provided via the i2c library and applied either a kalman or complementary filter to get pitch and roll and I thought when using the DMP I get better values due to the IMU internal fusion of data, but the raw-method looks still better (even that it does not yet satisfy me for the application I am working with, where I have a moving and acclerating object and want to measure pitch and roll independent of the object acceleration). Am I missing something in regard to the used of the DMP? I followed 1:1 the (6 axis-)demo in regard how to set up the IMU and how to read the FIFO (in a 10ms loop in my software). The sampling rate of the DMP is set to 200/(1+9)Hz = 20Hz (DMP_FIFO_RATE = 0).

    I appreciate any feedback on this! Thanks a lot in advance!

    Regards,
    Tom

  14. Hi Admin,

    I tried the link from your reply and the instructions in it did not work. The link is a little more than 3 years old and likely applied to an older version of processing. In processing 2.1.1 there is no such sub folder as libraries so I created one and added the toxi libraries to it. Opened a new sketch in processing and selected the sketch tab- import libraries and toxi was not there and in import libraries-add library toxi does not show up in the library manager.

    So when running the MPUTeapot sketch the following error occurs:

    No library found for toxi.geom
    No library found for toxi.processing
    Libraries must be installed in a folder named ‘libraries’ inside the ‘sketchbook’ folder.

    and I added a libraries folder containing the toxi libraries to the MPUTeapot sketch and get the same error. I created a new sketch that contained “import toxi.geom.*;” only as a single line and it came back with ” No library found for toxi.geom” changing the line to “import processing.serial.*;” did not create any error. The problem has to be the inability for processing 2.1.1 to recoginize and use the toxi libraries.

    I also read the note and followed the instructions that came with MPUTeapot as follows:

    // NOTE: requires ToxicLibs to be installed in order to run properly.
    // 1. Download from http://toxiclibs.org/downloads
    // 2. Extract into [userdir]/Processing/libraries
    // (location may be different on Mac/Linux)
    // 3. Run and bask in awesomeness

    and was unable to “bask in the awesomeness”.

    I will try an older version processing (if one is available) and if not then I have spent about as much time on this example as I can endure.

    1. A previous commenter said that he had solved his problem by using a previous version of processing (see Mauro’s comment where he indicated that he used Processing 1.5.1). I’m running Processing 2.0b8. I created this example a while ago and haven’t my Processing version in a while so it’s entirely possible that that is the problem. Good luck.

  15. Hi Admin,

    I tried 1.5.1 and 2.03 with the same results. I opened a blank sketch, selected sketch-import library-add library which opened Library Manager and I installed a library Ani from it. Then did a search for Ani and found it in a My documents/Processing/libraries which I had not seen before. So the Library Manager created this folder and I unzipped toxiclibs-complete-0020 into it. Opened a new sketch an in it opened selected sketch-import library and found toxi libraries and was able to import them. Created MPUTeapot folder in this Processing folder and was able to get things to run.

    So for anyone having a problem with “processing” create a folder in their user directory named Processing and a sub folder in Processing named libraries and extract the toxiclibs-complete-0020.zip downloaded file to it. Then MPUTeapot should run fine (be sure to close the Arduino serial monitor after entering a character).

  16. Nice job! I am using a MEGA 2560 and when I run the code on Processing I get this:

    WARNING: RXTX Version mismatch
    Jar version = RXTX-2.2pre1
    native lib Version = RXTX-2.2pre2

    I can see the plane, but as I move the GY-521 the plane stands still – it doesn’t move at all. Do you have any idea why is that?
    And one more question. I want to get only raw values out of the MPU6050 code without using the processing sketch, I am a newbie at Arduino and I am not sure how to change the code to get only values?

  17. I know about this code by Krodal, but from what I understand it doesn’t use the DMP engine which is the case with the MPU6050_DMP6. I need the accuracy and stability of the values given by the MPU6050_DMP6 code. I am trying to cut the Processing part of the code but I get “FIFO overflow” as the serial monitor displays values as I tilt the MPU6050. What I did was just putting this part as a comment which asks for a character to be sent by the Processing Teapot code so that the DMP would start working:

    “// wait for ready
    //Serial.println(F(“\nSend any character to begin DMP programming and demo: “));
    //while (Serial.available() && Serial.read()); // empty buffer
    //while (!Serial.available()); // wait for data
    //while (Serial.available() && Serial.read()); // empty buffer again”

    This is the only thing I change from the code and when I open the serial monitor it doesn’t wait to receive a character and proceeds to values generation. But when it does it gives out the following, and I am not sure if it is a problem or not:

    Initializing I2C devices…
    Testing device connections…
    MPU6050 connection successful
    Initializing DMP…
    Enabling DMP…
    Enabling interrupt detection (Arduino external interrupt 0)…
    DMP ready! Waiting for first interrupt…
    quat 1.00 0.01 -0.02 -0.00
    FIFO overflow!
    quat 1.00 0.04 -0.02 -0.01
    FIFO overflow!
    quat 1.00 0.05 -0.01 -0.02
    FIFO overflow!
    quat 1.00 0.06 -0.01 -0.04
    FIFO overflow!
    quat 1.00 0.07 -0.01 -0.05
    FIFO overflow!
    quat 1.00 0.07 -0.01 -0.06
    FIFO overflow!
    quat 0.99 0.07 -0.01 -0.07
    FIFO overflow!
    quat 0.99 0.07 -0.01 -0.08

    I just want the MPU6050_DMP6 code without the Processing part.

  18. Hi Angel,

    Have you changed anything in the original downloded MPU6050_DMP6 code other than uncommenting // #define OUTPUT_READABLE_QUATERNION and commenting #define OUTPUT_READABLE_YAWPITCHROLL?

    Just for fun I tried to uncomment // #define OUTPUT_READABLE_QUATERNION leaving #define OUTPUT_READABLE_YAWPITCHROLL as is to see if i would get a FIFO overflow and did not. I added a line to print fifoCount:

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    Serial.print(fifoCount);
    Serial.println(” FIFO Count”);

    and get:
    DMP ready! Waiting for first interrupt…
    42 FIFO Count
    quat 1.00 0.01 -0.01 -0.00
    ypr 0.03 1.02 0.85
    42 FIFO Count
    quat 1.00 0.01 -0.01 -0.00
    ypr 0.06 1.01 0.89
    42 FIFO Count
    quat 1.00 0.01 -0.01 -0.00
    ypr 0.09 0.99 0.96
    42 FIFO Count
    quat 1.00 0.01 -0.01 -0.00
    ypr 0.12 0.97 1.03
    42 FIFO Count
    quat 1.00 0.01 -0.01 -0.00
    ypr 0.16 0.96 1.10

    I can’t replicate the problem so not sure how to help fix it.

  19. I found where the problem lies. This is what I’ve done:
    I uncomment: // #define OUTPUT_READABLE_QUATERNION
    and comment: #define OUTPUT_READABLE_YAWPITCHROLL

    And like I mentioned earlier I am commenting line 190-194 thus removing the need of a character input for the DMP to start generating values:

    // wait for ready
    //Serial.println(F(“\nSend any character to begin DMP programming and demo: “));
    //while (Serial.available() && Serial.read()); // empty buffer
    //while (!Serial.available()); // wait for data
    //while (Serial.available() && Serial.read()); // empty buffer again

    The problem with the FIFO overflow occurs when I put a “delay (1000)” function after line 296 in the main loop for the QUATERNION values option which looks like this:

    #ifdef OUTPUT_READABLE_QUATERNION
    // display quaternion values in easy matrix form: w x y z
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    Serial.print(“quat\t”);
    Serial.print(q.w);
    Serial.print(“\t”);
    Serial.print(q.x);
    Serial.print(“\t”);
    Serial.print(q.y);
    Serial.print(“\t”);
    Serial.println(q.z);
    delay(1000); //<—— This is my addition to the code for better clarity of the values for each tilt of the MPU6050
    #endif

    Any ideas how to deal with this overflow?

    1. Hi Angel,

      I don’t know the specifics of whats causing the overflow, but my experience with this routine is that if you’re not flushing out the DMP buffer quickly enough, it will overflow. So any delay you contribute to the code will make it harder to keep up with the DMP buffer. The “delay(1000)” statement is almost certainly causing your problems, and “Serial.print” statements are also notoriously slow, so you’ll want to minimize the number of those you use as well. One thing I did to speed up the software a little was to speed up the rate of communication on the i2c bus. You can increase the rate from 100kHz to 400kHz by following the instructions here: http://forum.arduino.cc/index.php/topic,16793.0.html. I’ve tried to use this code for a couple of projects, and have found it very difficult to keep any code I add to it fast enough not to cause a buffer overflow. If you added the delay because you’re having trouble reading the values on the Arduino IDE Serial Monitor, try unclicking “Autoscroll” at the bottom of the Serial Monitor to be able to read the printed values.

      Debra

      Just to add to this comment – if you do try to speed up the rate of the i2c bus (which the MPU6050 uses), the relevant instructions are here:
      the atmega-hardware can do 400 KHz, but you have to tweak the Wire-library in file hardware/libraries/Wire/utility/twi.h.

      Near the top of the file you see :
      Code:
      #ifndef TWI_FREQ
      #define TWI_FREQ 100000L
      #endif
      If you change that to
      Code:
      #ifndef TWI_FREQ
      #define TWI_FREQ 400000L
      #endif
      The I²C bus should run at 400kHz
      But you also have to the delete the files
      hardware/libraries/Wire/Wire.o
      hardware/libraries/Wire/utility/twi.o
      because the library must be re-compiled before it uses the new speed.
      (This is done automaticaly when you open the Arduino-IDE)

      With the more recent versions of Arduino, you don’t have to delete the “.o” files. In fact I couldn’t find them, but the change seemed to work. It speeds the code up a little, though you still have to be very careful of the execution time of any code you add to be able to keep up with the DMP buffer.

  20. Hi Angel,
    I was able to duplicate the problem by implementing your code. Removing the delay fixed it so as Debra suspected that is the cause of the problem. I tried different baud rates and received FIFO overflow at 19200 and below (didn’t try higher other than 115200).
    Good luck with what you are trying to accomplish.
    Larry

  21. Thank you all for your responses.
    Hmm, my destination for the Wire library is: Arduino/libraries/Wire/utility/twi.h – it’s not in the hardware folder, I hope that’s not a problem. So I changed it to 400000L, but there were no twi.o files anywhere after I changed it through “notepad”, hopefully that’s not an issue too.
    So when I ran the code again, the FIFO was still overflowing, I reduced the delay to ‘200’ but it was still overflowing. When I completely remove the delay then the FIFO is not overflowing at all. I am just wondering if the changes I’ve made in the twi.h are in effect. Is there any way I can check if the code is running at 400KHz? And my other question is – can I make it go at 1.7MHz, would the MPU6050 have any problem with a speed of 1.7MHz?

    1. Sorry if I gave the wrong impression, but changing the i2c rate in twi.h will not solve all of your speed problems, it is just one way to help speed up the process of reading the i2c bus. I found that it gave me a little extra time to execute additional code I added. To avoid DMP buffer overflow while using this program as a base, you will need to write efficient code that can execute quickly enough not to overflow the buffer. 200 milliseconds is a very long time with respect to computer operations. I don’t know if the MPU will communicate at 1.7 MHz or not. You can certainly do the experiment. Changing that rate will still only help speed up the part of the code that communicates over the i2c bus, but not the clock rate of the Arduino. You will still have to make sure any other code you add is efficient, which is something you’ll probably have to figure out yourself.

  22. I appreciate all of the help you have given me. I really don’t want to (and actually can’t) tweak the MPU6050_DMP6 code to make it more efficient because I don’t get much of the stuff in it. I just want to add one delay function and that’s all. I am not giving up though, I will have this down and then I will share it here. Thanks again!

  23. Hi, thank you a lot for this post!
    But I have some weird problems. Data streming starts okay, and then… It sometimes freezes! and sometimes not. It happens after different intervals of time. The sensor moves during freeze or not, doesn’t matter 🙁 when I tried to run it in arduino IDE serial monitor it behaves the same way, Starts normally and then data flow freezes. or not. May be someone has similar problems?

    1. I do have the same problem!

      This response might arrive a bit late, but did you ifnd a way to overcome it??? Did you at least find the reason for it?

  24. So far.. It is arduino who freezes. After around 20 uploads I’ve figured out that everything freezes except interrupts.. =_= (led analogWrite inside interrupt function). Still not figured out a way around this.

  25. I am apreciated for this post, i am starting using processing and mpu and this post allow me to understand the basics.

    I am trying to user your codes but I am facing a problem, i get this error ‘The constructor Quaternion(float, float, float, float) is not visible. Can you help me?

  26. I made a videoclip to share some experiences.
    The Arduino UNOrev3 and the MPU5060 delivers calculated angels via the onboard DMP and also RAW data. Seems to work both in Arduino sketch and in Processing (TeapotDemo)
    Problem to solve: The graphical representation, the “airplane”, will not move around its x-y-z-axis.
    Someone who have had the same problem?

    http://www.youtube.com/watch?v=0DyojBzKurA&list=UUGfoXxpRr-1ahvnr8SaQwdg

    A link to my STUV-group on Fb: https://www.facebook.com/groups/1415442618700681/

  27. Hi Rob

    This error should go away if you put the I2Cdev and MPU6050 library files in the same folder as the sketch file (.ino).

  28. Larry,
    I’m not sure which Serial Monitor you are referring to. I uploaded the DMP-sketch to the Arduino.
    After that I start the sketch and then close the Arduiono IDE. (I assumed the sketch is idle, waiting for a “character” send from Processing to my Arduiono)
    So I have nothing running on the PC.
    Then I start Processing with the modified TeapotDemo.
    And as you can see on the video, Processing is receiving DMP-data, but the graphic is not moving around the x-y-z-axises

  29. Andersk44,

    I had mine working once using an UNO R3 , Arduino 1.0.5-r3, and Processing 2.1.1 32 bit but now I get the same problem you described. I don’t have a Facebook account and don’t plan on getting one so can’t look at your video. I get data into processing no problem but the figure does not move as you have described and I am puzzled why I can’t get this to work anymore. Once I figured out how DMP data works I moved on from the example and haven’t used it since 4 Feb. I wish I could help but I’m new to Processing and haven’t a clue on what to do.

    Larry

  30. Hi Anderssk44,
    I saw the video from the link and I was having the same problem. I believe to fix the problem you will need to comment “#define OUTPUT_READABLE_YAWPITCHROLL” and scroll down and un-comment “//#define OUTPUT_TEAPOT”. Let me know if that works for you.
    Larry

  31. Hi Larry,
    Thanks for your advice, You saved me hours of troubleshooting! Now it is working, the airplane is moving as it should.
    (but the characters rolling in the Processing window look a little bit odd, no numbers just like a foreign language).
    First a calibration phase and then I can move it smoothly in any direction.

    By the way, when I am using the Outputformat Yaw-Pitch-Roll in the sketch, it seems to me as it output in a different order like Yaw-Roll-Pitch instead in Processing. (My reference point is the position of the little mark at the upper left hand corner on the 6050-chip).
    Am I wrong?

  32. Anderssk44,
    The teapot demo is quaternion format but not sure if it is the same as the other quaternion output. I,m not at all savvy on processing but my guess is that it is taking in binary values. I only say this because the output values look very similar to .bin files that I have opened with a text editor in the past.

    I use a GY521 board with the 6050 and it has the X and Y rotation marks on it with Z perpendicular to the chip/board (matches the diagram on page 21 of MPU6050 data sheet). I verified that the output on MPU Teapot matches the diagram and the markings on my board. The yaw pitch roll output (default setting for MPU6050_DMP.ino sketch) are incorrect. Pitch and Roll are reversed and clockwise rotation (viewed in the direction of the arrow as in the diagram) produces a negative number when it should be positive.

    So long story short you are correct that the output for YPR should be YRP or more correctly Y(-R)P
    Larry

  33. how to connect two mpu6050s to arduino uno and take the yaw pitch roll data(jeff’s code) from both the sensors?

  34. Hi,I have MPU-9150.When I try this codes,I get “DMP initilazation failed(code1)” ,

    how can i fix it? by the way your project is very helpful

    have a nice day

  35. i have the same board with the same problem that your are facing did u find any solution to it also, i used the Raw code with 1st order complementry filter it works very well with roll and pitch while yaw ( z-axis ) nothing changes in the upcoming raw data…if u have any solution to it please let me know

  36. Hi!

    I really like your tutorial on how to use the MPU-6050 with a arduino nano and processing!

    I’ve followed your description but I have run into a problem…when I run the processing sketch I get the following error:

    java.lang.NullPointerException
    at processing.mode.java.runner.Runner.findException(Runner.java:926)
    at processing.mode.java.runner.Runner.reportException(Runner.java:871)
    at processing.mode.java.runner.Runner.exceptionEvent(Runner.java:797)
    at processing.mode.java.runner.Runner$2.run(Runner.java:686)

    I have not the slightes idea on how to get the sketch to work. Does you/anyone have a suggestion?

    Kind regards / Christoffer

  37. Hi,

    I would like to get the stable values for Acceleration, Euler angles and Yaw, pitch and roll.
    I connected MPU 6050 with the controller and using the same sketch as provided by Jeff Rowberg.
    I put the sensor on the breadboard.
    For acceleration I am getting different values. as in static position values must be 0,0,0 or closer to that. Same with angles it should give me something 90,90,0 or similar somehow.
    I did not change anything in the sketch.

    Could anybody please help or suggest me in this regards?
    Looking Forward anxiously.

    Kind Regards,
    Farhan

    1. Farhan,

      Are you using MPU6050_raw or MPU6050_DMP sketch? The output from the _raw sketch is noisy and you will likely have to come up with a Kalman filter for them. Gyros tend to drift especially with the rotation of the earth. You need to place the board on a solid surface such as a concrete floor (preferable). In Jeff’s code about midway down in the dmpIinitialize function he has entered offset values for his MPU6050 and yours will likely be different. I never bothered changing them as I was more interested in getting the processing sketch to work and will come back in the future to use the MPU6050 in an application. My guess is that the board needs to be placed on a solid surface and allowed to stabilize but I don’t know how Jeff picked the values for min sensitivity so you may need to ask him. You can read the values into a file and average them for the offset values. I use a “rs232_data_logger” program to save results to a file for my GPS and should work for this as it reads everything coming in on the USB.

      Good Luck
      Larry

  38. I can’t agree that MPU6050 suffers for gimbal lock. Gimbal lock appear on that euler mechanism but MPU6050 dont using it – there aren’t any mechanical parts like in euler circles mechanism. Propably yaw pitch and roll differencies are determined from accelerating around the 3 axis (instead of accelerometr which measure acceleration in direction of 3 axis) – so any gimbal lock can’t appear in gyroscope (in fact it isn’t a true gyroscop) of MPU6050.
    IMO

  39. I tried this, both using the library’s Processing file and yours. The arrow/plane drifts 5 degrees a second regardless if the gyroscope is moving or not. I’m completely new to Arduino. Do you know if it’s a problem with the MPU, or is it somewhere in the programming? I mean, it worked for you with the same code.

  40. Sometimes it actually works without drifting, but it happens at random. Additionally, at first it moves along with the actual movement of the gyroscope, but when the gyroscope is held completely still, the arrow will decelerate and slowly approach the position of the gyroscope.

    1. No, I’m sorry, I’m afraid I got distracted on a different project before completing that one. I found myself having problems completing the complementary filter computations without getting buffer overflow on the DMP. It would still be a good project, though.

    2. No, I’m afraid I found I was having difficulty completing the complementary filter calculations before getting DMP buffer overflow, then got distracted by another project. Sorry I don’t have that information.

  41. I that code, I changed the codes after #def to read the so called “world-frame accel sensor measurements”. It seems that during the few second at beginning, DMP is trying to cancel out acceleration on x and y axes and trying to make acceleration measure on z axes about 1600. Interesting~

  42. I followed the instructions to where to extract the toxiclibs libraries, and i’m still getting a bunch of errors such as “‘import’ does not name a type”.
    Any idea how to fix this problem???
    Thank you

  43. Hello, how can I skip those uncalibrated values which are displayed during the start of the Serial? Mine starts from line -4 degrees and then slowly goes to -1 or so. Still can’t get 0 degree as horizontal position. So I have to set -1 degree as the set point in Pid library in my balancing robot. I am using a single axis only

  44. does this look alright? i am seeing strange values being sent and received between android and processing

    ? 4 0
    Char: £
    £ 4 0
    Char: 
     4 0
    Char: (
    ( 4 0
    Char: ý
    ý 4 0

  45. Hi Debra
    Well done – your demonstrations and explanations are excellent. I resisted the temptation to pester you when I was struggling to get the Teapot demo going, but I got there in the end. If you have time, have a look at my blog at http://smokespark.blogspot.co.uk/2014/05/53-robotics-gyroscope-accelerometer.html where I more or less repeat the same thing. I would value your comments if I have described anything inaccurately.

    Looking forward to your side-by-side comparison.
    KC – Northern Ireland

    1. Hi Kieran,
      Great blog post! Thanks for letting me know that you were able to get all of the code to work. I like your writeup. My only minor quibble would be that you appear to reference to quaternions as an alternative to the complementary/Kalman filter calculations. You can actually use quaternions to do the angle filtering. The advantage quaternions offer is the ability to compute rotations of more than 180 degrees and also that they eliminate the problem of gimbal lock, which occurs when two of the rotational axes come into alignment. Quaternions are basically another coordinate system to be used in lieu of Euler angles.
      Also, I’m afraid I stalled out on doing the side-by-side comparison of the DMP with the complementary filter. I kept running into buffer overflow problems with the DMP while trying to get the complementary filter calculations going at the same time. I should probably take another stab at it – though maybe you’ll have better luck than I did, if you’d like to try it.
      Thanks for writing, and congratulations on the terrific blog.
      Debra

    1. Hi Jimmy,
      Sorry for the very belated response. I hope you’ve solved your problem by now. The data coming over the serial port from Arduino (that’s what you meant, not “Android”, right?) should be legible to a human reader – the accelerometer, gyro and filtered angles are written in plain text, so I’m wondering if you perhaps have an incompatible baud rate between Arduino and Processing. Otherwise, I’m not sure what the problem might be.
      Debra

  46. Hi I’m new to Arduino and I’m trying to use a 6DOF IMU (the MPU-6050) along with the i2cdevlib.
    I’ve been able to set the libraries and to execute a sketch in order to calculate the offset of the IMU starting from the raw data.
    Now I’m trying to execute the example code that i found on the github of the i2cdevlib https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino setting the output in the YPR mode (#define OUTPUT_READABLE_YAWPITCHROLL) with and without my offset parameters.
    In both sketches I found the same issue.
    The data on the serial shift from the initial position for 5-8 second showing an increment of several degrees (depends on the cases but usually there are 12-25 degrees) of the YPR angles without moving the IMU.
    The same thing happen with the Euler angles (#define OUTPUT_READABLE_EULER).
    I’ve tried also to execute the quaternion output mode (#define OUTPUT_READABLE_QUATERNION)
    which i don’t know how to read (I’ve just started learning about this) but the value I read on the serial seems to stay “stable”.

    I might solve this with a delay from the turn on which allow me to see of how much degrees the IMU “virtually moved” and then subtract that value to future reading (such as another offset) but i find this not usefull since I need the IMU to have the right angles on the turn on.

    I read on there that Farhan had the same issues, so I’m not the only one.
    What might be?

    PS: that’s a wonderfull blog! as soon as I have the IMU working properly I’m going to try an implementation of the filters as you suggest!

  47. hey everyone …i have made a code using mpu6050 and arduino uno to calculate roll and pitCH.
    Now i am shifting to mpu9150 and i am geting the magnometer values too .I want to calculate the YAW.
    Plz help me .(provide the code if possible). The code i made is given below:

    #include
    #include “Kalman.h” // Source: https://github.com/TKJElectronics/KalmanFilter

    #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead – please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

    Kalman kalmanX; // Create the Kalman instances
    Kalman kalmanY;

    /* IMU Data */
    int i,j;
    int count = 0; // for iterating first 100 values to calculate RMS values using Serial.end
    double sum_roll = 0;
    double sum_pitch = 0;
    double mean_roll = 0;
    double mean_pitch = 0;
    double update_roll;
    double update_pitch;
    int flag = 0;
    int sum=0;
    int sum1=0;
    int avg=0;int avg1=0;int ar=0;int br=0;
    double a[100];
    double b[100];
    double accX, accY, accZ;
    double gyroX, gyroY, gyroZ;
    int16_t tempRaw;

    double gyroXangle, gyroYangle; // Angle calculate using the gyro only
    double compAngleX, compAngleY; // Calculated angle using a complementary filter
    double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter

    uint32_t timer;
    uint8_t i2cData[14]; // Buffer for I2C data

    void setup() {
    Serial.begin(115200);
    Wire.begin();
    TWBR = ((F_CPU / 400000L) – 16) / 2; // Set I2C frequency to 400kHz

    i2cData[0] = 7; // Set the sample rate to 1000Hz – 8kHz/(7+1) = 1000Hz
    i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
    i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
    i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
    while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
    while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

    while (i2cRead(0x75, i2cData, 1));
    if (i2cData[0] != 0x68) { // Read “WHO_AM_I” register
    Serial.print(F(“Error reading sensor”));
    while (1);
    }

    delay(100); // Wait for sensor to stabilize

    /* Set kalman and gyro starting angle */
    while (i2cRead(0x3B, i2cData, 6));
    accX = (i2cData[0] << 8) | i2cData[1];
    accY = (i2cData[2] << 8) | i2cData[3];
    accZ = (i2cData[4] << 8) | i2cData[5];

    // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
    // atan2 outputs the value of -π to π (radians) – see http://en.wikipedia.org/wiki/Atan2
    // It is then converted from radians to degrees
    #ifdef RESTRICT_PITCH // Eq. 25 and 26
    double roll = atan2(accY, accZ) * RAD_TO_DEG;
    double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
    #else // Eq. 28 and 29
    double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
    double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
    #endif

    kalmanX.setAngle(roll); // Set starting angle
    kalmanY.setAngle(pitch);
    gyroXangle = roll;
    gyroYangle = pitch;
    compAngleX = roll;
    compAngleY = pitch;

    timer = micros();
    }

    void loop() {
    /* Update all the values */
    while (i2cRead(0x3B, i2cData, 14));
    accX = ((i2cData[0] << 8) | i2cData[1]) + 2000.0;
    accY = ((i2cData[2] << 8) | i2cData[3]) + 3319.84;
    accZ = ((i2cData[4] << 8) | i2cData[5]) + 664.48;
    tempRaw = (i2cData[6] << 8) | i2cData[7];
    gyroX = (i2cData[8] << 8) | i2cData[9];
    gyroY = (i2cData[10] << 8) | i2cData[11];
    gyroZ = (i2cData[12] << 8) | i2cData[13];

    double dt = (double)(micros() – timer) / 1000000; // Calculate delta time
    timer = micros();

    // It is then converted from radians to degrees
    #ifdef RESTRICT_PITCH // Eq. 25 and 26
    double roll = atan2(accY, accZ) * RAD_TO_DEG;
    double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
    #else // Eq. 28 and 29
    double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
    double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
    #endif
    //a[i]=roll;
    //b[i]=pitch;

    // array storing the values
    //Serial.println(a[i]);
    //Serial.println(b[i]);

    double gyroXrate = gyroX / 131.0; // Convert to deg/s
    double gyroYrate = gyroY / 131.0; // Convert to deg/s

    #ifdef RESTRICT_PITCH
    // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
    if ((roll 90) || (roll > 90 && kalAngleX 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
    #else
    // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
    if ((pitch 90) || (pitch > 90 && kalAngleY 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
    #endif

    gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
    gyroYangle += gyroYrate * dt;
    //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
    //gyroYangle += kalmanY.getRate() * dt;

    compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
    compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

    // Reset the gyro angle when it has drifted too much
    if (gyroXangle 180)
    gyroXangle = kalAngleX;
    if (gyroYangle 180)
    gyroYangle = kalAngleY;

    /* Print Data */
    #if 0 // Set to 1 to activate
    //Serial.print(accX); Serial.print(“\t”);
    //Serial.print(accY); Serial.print(“\t”);
    //Serial.print(accZ); Serial.print(“\t”);

    //Serial.print(gyroX); Serial.print(“\t”);
    //Serial.print(gyroY); Serial.print(“\t”);
    //Serial.print(gyroZ); Serial.print(“\t”);

    //Serial.print(“\t”);

    // Now calculating data for the 1st 100 values based on the roll and pitch

    #endif

    //Serial.println(roll); Serial.print(“\t”);
    //Serial.print(gyroXangle); Serial.print(“\t”);
    //Serial.print(compAngleX); Serial.print(“\t”);
    //Serial.println(kalAngleX); Serial.print(“\t”);

    //Serial.print(“\t”);

    //Serial.println(pitch); Serial.print(“\t”);
    //Serial.print(gyroYangle); Serial.print(“\t”);
    //Serial.print(compAngleY); Serial.print(“\t”);
    //Serial.println(kalAngleY); Serial.print(“\t”);
    //

    // Serial.println(“This code calculates the threshod values for the roll and pitch….”);
    // Serial.println(“It then shows the updated values besides the raw values of roll and pitch”);

    Serial.print(“\t”);
    if(count<5000)
    {
    sum_roll = sum_roll+roll*roll;
    sum_pitch = sum_pitch+pitch*pitch;
    count = count + 1;
    }
    else
    {
    mean_roll = sqrt(sum_roll/count);
    mean_pitch = sqrt(sum_pitch/count);
    Serial.println("\n\n\n");
    //Serial.println("Serial Communication has finished after 1000 values");
    Serial.println("\n\n\n\n");
    Serial.print("RMS value of the roll values is ");Serial.print(mean_roll);Serial.println("");
    Serial.print("RMS value of the pitch values is ");Serial.print(mean_pitch);Serial.println("");
    //Serial.end(); // ending the Serial communication
    }

    //Now Start the Serial Communication again to get updated values with subtracting the threshold values
    //if(count<=10)
    //{

    //Serial.println("");
    //Serial.println("Serial Communication now starts again to get updated values");
    //Serial.begin(115200);
    update_roll = abs(roll) – abs(mean_roll);
    update_pitch = abs(pitch) – abs(mean_pitch);
    Serial.print("Value (roll): ");Serial.print((update_roll));Serial.println("");
    Serial.print("Value (pitch): "); Serial.print(update_pitch);Serial.println("");
    Serial.print("Value (gyroXangle): ");Serial.print(gyroXangle); Serial.print("");
    Serial.print("Value (compXangle): ");Serial.print(compAngleX); Serial.print("");
    Serial.print("Value (KalmanXangle): ");Serial.println(kalAngleX); Serial.print("");
    Serial.println("\n");
    //count++;
    //}

    #if 0 // Set to 1 to print the temperature
    //Serial.print("\t");
    double temperature = (double)tempRaw / 340.0 + 36.53;
    Serial.print(temperature); Serial.print("\t");
    #endif

    //Serial.print("\r\n");

    }

  48. I write a dealy(1) statement into loop: while (!mpuInterrupt && fifoCount < packetSize)
    and also after this while loop.

  49. svp j’ai fait tout ce que vous avez dit mais la bibliothèque du toxi encore n’existe pas seulement du Ani je ne sais pas pourquoi . j’ai utilisé processing 2.0.3 .
    please aidez-moi

  50. Hello all!

    I am struggling with getting the origins set properly. What readings should I use to set the accel and gyro offsets?

    Thanks!

  51. Is there a way to lengthen the interupt pulse on an mpu6050, Im trying to use this on a Galileo and it seems the interupt pulse is wayyyyy to quick.

  52. Hi!

    Can anyone come up with a easy way to make the MPU portable, as in transmit data to the arduino wireless. This way the MPU could be attached to a moving object to read its movement without having wires hanging from it.

    1. Seems like it would just be easier to use the MPU with an Arduino with a small footprint, like a nano or a tiny.

  53. Does anyone know the units for the acceleration used in Jeff Rowberg code? I am getting an output of close to 2000 for 1G. I want to use the acceleration to compute velocity over a short period of time. I would like to convert the acceleration output to knots so that I could use it in parallel with a GPS.

  54. You must look if your reading comes directly from registers of the MPU. If so most probably accelerometer is set to 16g range. In that case readouts for 16g should be 32768. For 1g they
    should be 32768/16=2048. This value is very close to 2000. To calculate acceleration in g one
    should use ACCEL[g]=ACCEL_READOUT*ACCEL_RANGE/32768.

  55. hi,
    i am doing a small project which contains mpu-6050,atmega8L-8pu(micro controller) and avr studio 4(IDE) only. i am new to this sensor(mpu-6050), initially, i am trying to get temperature data from TEMP_H(0x41),TEMP_L(0x42). But i am getting constant data, there was no change in temperature. could any one please help me, how to get the correct data from mpu-6050 or suggest me changes in my code(the following code) in c language and i don’t know c++or some other languages. i am new to this concept and beginner.

    //***** nightmare code *****//

    #define F_CPU 11059200UL
    #include
    #include
    #include

    void usart_init(void);
    void usart_send(unsigned char ch);
    void usart_data(unsigned char l_d);
    void i2c_write(unsigned char data);
    void i2c_start(void);
    void i2c_stop();
    void i2c_init(void);
    unsigned char i2c_read(unsigned char islast);

    int main(void)

    {

    unsigned char w,is_last,q;
    signed int temp_H,temp_L,temp_Raw;
    unsigned int m,a,b,c,d,e,k,s_in,in;
    float temp_c;

    //******* writing data into sensor ********//

    i2c_init();
    i2c_start(); //start transfer
    i2c_write(0b01101000); // put slave address+write(i.e= ‘0’), 1101000=0x68 AD0=’0′
    i2c_write(0x6A);
    i2c_write(0x05);

    i2c_start();
    i2c_write(0b01101000);
    i2c_write(0x6B);
    i2c_write(0x80); //
    i2c_start();
    i2c_write(0b01101000);
    i2c_write(0x6C);
    i2c_write(0x80);

    i2c_start();
    i2c_write(0b01101000);
    i2c_write(0x1A);
    i2c_write(0x08);

    i2c_start();
    i2c_write(0b01101000);
    i2c_write(0x19);
    i2c_write(0x07);
    i2c_stop();

    i2c_start();
    i2c_write(0b01101000);
    i2c_write(0x38);
    i2c_write(0x01);

    _delay_ms(1);

    //***** reading data from sensor *****//

    while(1) // infinite loop

    {

    i2c_start();
    i2c_write(0b01101000);
    i2c_write(0x3A);
    i2c_start();
    i2c_write(0b11101000);
    w=i2c_read(is_last);

    if(w!=0x00)
    {

    // //
    i2c_start();
    i2c_write(0b01101000);
    i2c_write(0x41);
    i2c_start();
    i2c_write(0b11101000);
    temp_H=i2c_read(in);
    /
    }

    if(w!=0x00)

    {
    i2c_start();
    i2c_write(0b01101000);
    i2c_write(0x42);
    i2c_start();
    i2c_write(0b11101000);
    temp_L=i2c_read(in);

    }

    _delay_ms(1);

    temp_H=temp_H<<8;

    temp_Raw=temp_H+temp_L;

    s_in=temp_Raw & 32768;

    if(s_in==32768)

    {
    temp_Raw = ~(temp_Raw);
    temp_Raw = temp_Raw+1;
    temp_c = temp_Raw/340.0;
    temp_c = 36.53+temp_c;
    temp_c = temp_c*100;
    m=temp_c;
    }

    else

    {
    temp_c = temp_Raw/340.0;
    temp_c = 36.53+temp_c;
    m=temp_c;
    }

    a=m/10000;
    m=m%10000;
    a=a+48; // converting
    b=m/1000;
    m=m%1000;
    b=b+48;
    c=m/100;
    m=m%100;
    c=c+48;
    d=m/10;
    d=d+48;
    e=m%10;

    usart_init();
    usart_data(a); // higher byte data
    usart_data(b);
    usart_data('.'); // providing space
    usart_data(c);
    usart_data(d);
    usart_data(0x20); // providing space
    usart_data(0x93); // degree symbol
    usart_data('C'); // temp.in celcius
    usart_data(0x0D); // goto next line
    usart_data(0x0A); // start new line

    }
    return 0;

    }

    //*********** TWI programming ***********//

    void i2c_init(void) //initialization

    {
    TWSR=0x00;TWBR=0x89;TWCR=0x04; ////SCL=38.743khz for crystal=11.0592Mhz, enable the TWI mode
    }

    void i2c_start(void)
    {

    TWCR=(1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
    while((TWCR & (1<<TWINT))==0);
    }

    void i2c_stop()
    {
    TWCR=(1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
    }

    void i2c_write(unsigned char data)

    {
    TWDR=data;
    TWCR=(1<<TWINT)|(1<<TWEN);
    while(!(TWCR & (1<<TWINT))); //generates ACK after completion of 8th clock pulse
    }

    unsigned char i2c_read(unsigned char islast)
    {
    //if(islast==0)
    // TWCR=(1<<TWINT)|(1<<TWEN)|(1<<TWEA);
    //else

    TWCR=(1<<TWINT)|(1<<TWEN);
    while(!(TWCR & (1<<TWINT)));
    return TWDR;
    }

    //****** USART code *******//

    void usart_init(void)

    {
    UCSRB=(1<<TXEN);
    UCSRC=(1<<UCSZ1)|(1<<UCSZ0)|(1<<URSEL);
    UBRRL=0x47; // Setting 9600 baud rate
    }

    void usart_send(unsigned char ch)
    {
    while(!(UCSRA & (1<<UDRE)));
    UDR=ch;
    }

    void usart_data(unsigned char l_d)

    {
    unsigned char ld;
    ld=l_d;
    usart_send(ld);
    }

  56. Hi zygmunt, This is siva i am doing project on mpu6050 with atmega16. Can you please send your views regarding how to proceed, how to read values. It is very great if you provide a sample code.

  57. hi,
    I started with mph recently and made some progress(I think) : I could get the roll angle for the sensor, but Im not satisfied with the results I got. I got zero offset of 0.5 deg and the offset increases with increase in set angle. For 15 deg, I get about 14 and for 30, I get about 28.4. Is there a way to get improved results.

  58. Hi,
    thank you all for sharing your experience.
    I have a basic question, about the DMP values, as you mentioned its:
    “quaternion values in easy matrix form: w x y z”
    can you please explain what is the matrix and the values meaning?
    Finally I need to construct a Translation+Rotation matrix.

    Thank you for any kind of help or directing

  59. Hi, im trying to calculate translation, linear distance, using accelerometer. do you have suggestions? is someone here did it?
    Thank you

    1. That’s very hard to do, because as you integrate from acceleration to velocity then to position, you introduce growing errors every time. I don’t know of a simple way to do that using an accelerometer.
      Debra

      1. Hi, thank you for your time:)
        Measuring distance with only accelerometer input is like you said – not accurate at all, in the first seconds it make sense and then it start drifting.
        I know I need to use other navigation information like compass and gyro, but I don’t know how to combine them for this purpose. (im using mpu9150)
        So maybe someone here did it? or have a theoretical idea?

  60. I’m currently working a project based on the Freescale Freedom K20D50M witch is a ARM Cortex M4 based development board.

    I created a driver for the MPU6050 and the DMP based on the I2CDevlib (I can’t use this one because is not an Arduino project).

    The loading part of the DMP (loading the memory bank) seems to work. I take proper precautions with the reading of the FIFO (overflow and others) but when I read it, I only get 0 on all the measurements in the quaternions (W, X, Y and Z).

    Does anyone had a similar problem? (reading always zero in the quaternions)

    Thanks beforehand

  61. I suggest removing / uncommenting the dmp offset commands. It stops the drifting for 10 seconds issue. No idea why it works like that.

  62. Hi ,
    I am from Bangladesh. I am trying to make a self balancing robot. By searching Internet for MPU6050 tutorial I came to your site. Thank you for your good quality tutorial. I am getting DMP data in Arduino serial monitor without having any problem.But now I want to enjoy some visualization. Processing 2 (32 bit) has some problem with my graphics card(internet people suggested for using Processing 1.5) However it(1.5) had some RXTX mismatch error !!! I tried to implement a fix and processing 1.5 now says it is stable . However, the processing sketch does not show any movement. it seems, it is not reading the COM port properly.I specified the COM8 as you said and tried varying baudrate(i am using uno). But no change !!! You wrote “You can download my slightly modified version of the Processing sketch here”. I am afraid, i am not getting any download link of your code. Can you make me any suggestion ?

  63. Hi, Thanks for your posts as I am learning a lot from your blog.

    I am new to this field and interested to do arduino projects. I have a question related to I2C communication of MPU6050 with arduino. Since, I read in datasheet that communication with registers of MPU is performed using I2C at 400kHz, but in my arduino code I can simply set the baud rate to my desired rate (eg. 57600), so where is this 400KHz rate coming into picture while doing I2C communication ?

    1. Hi,
      The 400 kHz is the rate at which the MPU-6050 DMP writes to its own buffer. If you don’t read out the buffer faster than it is written to, the buffer will overflow. Hope that helps.
      Debra

  64. hi
    i have downloaded the toxic library from where you said

    i extracted with winrar and pasted them to the sketchbook – liberaries ( i am using arduino 1.5.6 r)

    but i still get the error( MPU6050TeapotDemo.pde:-1: error: ‘import’ does not name a type)

    may you please help me on this problem

    1. The toxiclibs are processing libraries, not arduino libraries. They provide quaternion algorithms to the processing sketch. Perhaps you’re not using them correctly?

  65. HI admin
    I not use library I2C because i use PIC CCS C.
    So: If I put MPU horizontally (Z-axis = 90* with ground ) and I move around the z-axis circular: how can I calculate the angle of rotation around the Z-axis?
    :D. Can you write detail for me?. Thank you very much

    1. If you use the DMP data, you should be able to get the rotation angle about any axis you choose, since, as you can see in the video, the DMP computations seem to compensate for yaw drift. However, if you’re talking about rotating the IMU by 90 degrees, that doesn’t change the direction of the Z-axis. Rotation of the IMU does not rotate the axes along with it. The axes are fixed in space, while the IMU rotates.

  66. Following the stackoverflow instructions, it says to unzip the file to the libraries folder. However, there is only a ‘lib’ folder not a ‘libraries’ folder.

    Unfortunately, even after placing the zip contents in ‘lib’, ‘library’ and ‘libraries’ (creating the last 2 folders), I still get the error:

    No library found for toxi.geom
    No library found for toxi.processing

    I can’t imagine it’s this hard to install a processing library. Is it possible that it’s a version issue? I’m using Processing 2.2.1.. the toxiclibs-complete-0020.zip contains toxiclibs_p5 and toxiclibscore however I’m not sure of their versions.

    Any suggestions?

  67. Thanks that was it! Since there was never any install for processing, I assumed everything was in the folder where the .exe is. Didn’t notice that it created a libraries folder

    (
    C:\Documents and Settings\…\My Documents\Processing\libraries (WinXP) or
    C:\Users\…\AppData\Processing\libraries (Win8)
    )

    Runs fine now!

  68. Hi! I have two questions I was hoping you could help me with.

    1) When I’m running the Arduino sketch with the #define OUTPUT_READABLE_QUATERNION line uncommented and my GY-521 completely motionless, I notice that the X and Z values change before they eventually level out. For instance, the Z values change from 0 to about -.2, and the X values rise to -.07 and then drop back to -.01. Any idea why the output fluctuates if the chip is stationary?

    2) What are the offsets that you provide to the gyroscope and accelerometer for? (lines 196-200)

    1. 1) I’m not sure about the X values – I haven’t noticed those drifting – but the Yaw (Z-axis rotation) values should drift for the first 8 to 10 seconds because of gyroscopic drift. Accelerometer data is used to compensate for gyroscopic drift in pitch and roll, but the accelerometer is not able to give Yaw values, so cannot be used to compensate for the Yaw drift. The DMP appears to collect data for the first several seconds that the MPU6050 is stationary, then use that data to calibrate and compensate for the Yaw drift.

      2) I’m not sure which file you’re referring to. I don’t see offsets in lines 196-200 of the pde file. Can you elaborate?

  69. I’m sorry, I was referring to the MPU6050_DMP6 demo from the MPU_6050 library example. It’s lines involving the gyro and accelerometer offsets are 201-204 (not sure how I messed that up).
    An additional question: If I change the FS_SEL to 3 (for applications involving quicker movements), will the chip be less accurate when its rotational velocity is far slower than 2000 [deg/sec]?

  70. Hi, thank for the tutorial. I have a question here about the basic of the sensors.
    In the beginning you said that accelerometer can’t compute the yaw value. So how come the code “OUTPUT_READABLE_YAWPITCHROLL” in the library can compute the yaw value? All I know is that yaw can be calculated by magnetometer or compass.

    1. The accelerometer can’t provide yaw data, but the gyroscope can. The gyroscope data drifts, however. You can use accelerometer data combined with gyroscope data to correct the drifting in pitch and roll, but not in yaw. I think that the MPU6050 measures the yaw drift in the first 8-10 seconds of operation while the IMU is stationary and calibrates to compensate for the drift based on the measured drift rate.

  71. i have write somee code

    clear all
    close all
    clc

    arduin=serial(‘COM14′,’BaudRate’,9600);
    fopen(arduin);
    tic;

    while (toc<=20) % stop after 20 secs
    data = fscanf(arduin,'%f');
    end
    save real_data.mat data;
    fclose(arduin);

    give me error : Warning: Unsuccessful read: A timeout occurred before the Terminator was reached.

  72. Hi!

    I’m using the MPU6050_DMP code from Arduino’s “Examples” for a project I’m doing. Specifically, I’m utilizing the #define OUTPUT_READABLE_EULER and #define OUTPUT_TEAPOT portions. I’m also using the Processing sketch you’ve provided, with some additional code to read and save whatever’s in the serial port to a text file. But when I run the Processing sketch I receive the error: java.lang.NullPointerException. Any idea what could be causing this?

    I’ve tried backtracking a little bit to see what could possibly be causing my error. To start, I’m only utilizing the #define OUTPUT_READABLE_EULER portion of the Arduino code. As for Processing, I’m just using a small program that reads whatever’s in the serial port to a string and then saves it to a text file. When I run the Arduino sketch and open the Serial Monitor, all the code is printing out and displaying neatly, line by line. But the moment I run the Processing sketch as well, the display becomes extremely jumbled and mixed up. I don’t receive any error while they run simultaneously, but I’m also not sure why this jumbling occurs.

    If you could offer any insight it would be greatly appreciated!

    1. Most likely the Processing sketch is not reading the correct serial port to obtain the Arduino data. If you look at the setup function in the Processing sketch, it obtains a list of available ports and uses the variable “portIndex” to index into the list of ports. Try changing the “portIndex” variable to a different value (like 0) to get the sketch to read from a different port.

      1. In regards to the Processing sketch not reading the correct serial port; if my airplane moves as I move the IMU, doesn’t that mean it is reading the correct port?

  73. Hi!
    I need to be able to detect the orientation of the torso of a person. Basically I need to know what angle the person’s torso is and then be able to then take some action. How can I do this? i am confused as in how do I set my limits and what parameters do i use Yaw, Pitch, Roll or individual 6 axis readings.

    1. I don’t know where to find the teapot code. The only Processing sketches I was able to find show an airplane rather than a teapot. The teapot was probably from an earlier version of the sketch.

  74. How can I get the Arduino sketch to, “Send any character to begin DMP programming and demo”?
    I don’t want to have to open the Serial Monitor and manually type something. I’ve tried using Serial.println(‘p’) and Serial.write(‘p’) but haven’t had any luck. If you could offer some advice, that would be awesome! Also, if you do know how, where should I put that line of code? After the buffer initially empties? Or after it is waiting for data?

  75. Thank you everyone for sharing all your work, time and knowledge.

    Firstly I’m a bit of a newbie but reading lots and just diving in.
    I’m hoping to use this code as a basis for my project where I want to send the yaw, pitch and roll data (and GPS in the future) via Bluetooth to an android phone app.

    I’ve got the data coming through to the serial monitor fine and I’ll be working on the Bluetooth code via HC-05 next.

    I’m using a Multiwii SE v2.5 board since it’s got the sensors and a lot more built-in and is still cheap.
    The only problem I have so far is that when the board is level the sensor reading is not and out by roughly 8 degrees one way and 4 degrees the other.

    Any ideas or suggestions on why or what I could do. I have built in a calibration off set in the android app, but can I calibrate the sensor also directly or in the arduino code.

    Thanks again.

    Michael

    1. Michael… even though I aught to … I still haven’t properly set this value too…
      I think what you want is to tweak this…

      // supply your own gyro offsets here, scaled for min sensitivity, rod didn’t change any
      mpu.setXGyroOffset(220);
      mpu.setYGyroOffset(76);
      mpu.setZGyroOffset(-85);
      mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  76. You Legend,
    Thank You soooh much for this tutorial! Geek Mom Rocks!
    I now have my roll values controlling a servo which will control my huge single line lift kites.
    BIG SMILEY FACE. I’ll make sure to share all my details in a post (windswept-and-interesting.co.uk) video already on youtube.

  77. Ciao amico!
    very thanks for all this!
    I need to read the YPR from arduino uno… I tryed the scketch *.ino and it work properly, but i cant to run the modified ‘teapot’ demo. it can be sent to arduino and the window appear, but the airplane arrow dont moves in any way… Some suggestion please?
    Very thanks in advance!

  78. hi, iam using the mpu6050 for a balancing robot and am having trouble solving an issue. Has anyone experienced the following:

    rotating for balance through the x axis i get good response and the robot balances.
    rotating through the y axis the robot will not stand up.

    by simply turning the mpu 90 degrees and reading the x instead of y, it works ok.

    Ive been through my code which manually reads the mpu register data, gets the angle and fuses it using a complimentary filter. Ive checked the data from the accelerometer and gyro, the x and y are responding near perfect at the same time. am i missing something as i understand your x and y axis should be the same so you can decide to use either. In my case i can only rotate through x as y is slugish and the bot just falls over.

    the few lines of code are below if its any help. But to sum up, has anyone experienced this and have you used both axis with no problems??

    read gyro registers then pool them as below:

    //POOL GYRO RESULTS AND GET GYRO RATE IN DEGREES PER SECOND
    Gyro_result[x] = (double)((Gyro_data[0] << 8)| Gyro_data[1])/131;
    Gyro_result[y] = (double)((Gyro_data[2] << 8)| Gyro_data[3])/131;
    Gyro_result[z] = (double)((Gyro_data[4] << 8)| Gyro_data[5])/131;
    //POOL AND SCALE ACCELEROMETER RESULTS TO DEGREES
    Accel_result[x] = (double)((Accel_data[0] << 8) | Accel_data[1])/16384;
    Accel_result[y] = (double)((Accel_data[2] << 8) | Accel_data[3])/16384;
    Accel_result[z] = (double)((Accel_data[4] << 8) | Accel_data[5])/16384;

    //GET PITCH AND ROLL CONVERTED TO DEGREES * 180/PI
    Pitch = atan2(Accel_result[y],Accel_result[z])*57.2957;
    Roll = atan2(Accel_result[x],Accel_result[z])*57.2957;

    //FUSE GYRO AND ACCELEROMETER USING COMP FILTER
    Angle_x = 0.97*(Angle_x +(Gyro_result[x]*0.01) +(0.03*Pitch));
    Angle_y = 0.97*(Angle_y +(Gyro_result[y]*0.01) +(0.03*Roll));

    many thanks
    john

  79. Hi There,
    great great work, thank you. Your complementary algorithm link at MIT seems to be broken, I was interested to read it, can you please fix the link?

  80. Mudar versión de Android— En las alteraciones del editor visual, podemos mudar la versión de Android
    con que se está mostrando el layout actual.

  81. Hi, I’m trying to use the MPU-6050 to read the acceleration, I’m using the data coming from the DMP, the scale is default so +-2g, but in what unit of measurement are the output values (coming from the dmp ) ?

    1. You’ve probably figured this out by now, but if you check the MPU6050 datasheet, it says that the gyroscope readings are in degrees/second, and it appears that the accelerometer readings are in multiples of g. However the units of the accelerometer readings divide out in the calculation of the angle’s arctangent.

  82. I’ve developed my own DMP over MPU6050 and based on Quaternions, with 32bits microcontroller (Atmel sam3x) and Real Time Operating System (FreeRTOS).

    It’s a part of a project of driving a Drone for long distance, large autonomy and heavy load without no visibility.

    Take a look:
    https://youtu.be/fBF8X8zn_AE

  83. I tried several libraries, sketches , boards – everything i could. but i cannot get any data of mpu6050. it gives me all zero values. i2caddress scanner gives me address of 0x68 and it also shows MPU connected successfully. but in DMP6 example of library i get following –
    IniþInitializing I2C devices…
    Testing device connections…
    MPU6050 connection successful

    Send any character to begin DMP programming and demo:
    Initializing DMP…

    —— and it is just stuck there

    /////////////////////////////////////////////////////////////////////////////////////////
    for MPU6050_Raw example it gives following –

    Testing device connections…
    MPU6050 connection successful
    Updating internal sensor offsets…
    0 0 0 220 76 -85
    0 0 0 220 76 -85
    a/g: 0 0 0 0 0 0
    a/g: 0 0 0 0 0 0
    a/g: 0 0 0 0 0 0
    a/g: 0 0 0 0 0 0
    a/g: 0 0 0 0 0 0
    a/g: 0 0 0 0 0 0

    All other sketches either give zero values to all or gets stuck with DMP initialization.
    can you please help … what is going wrong.

  84. Hello every body,

    I’m trying your to do your tutorial with arduino uno and mpu6050 (gy-88a module ) under ubuntu system and it gives me this error :

    core.a(main.cpp.o): In function `main’:
    /usr/share/arduino/hardware/arduino/cores/arduino/main.cpp:11: undefined reference to `setup’
    /usr/share/arduino/hardware/arduino/cores/arduino/main.cpp:14: undefined reference to `loop’
    collect2: error: ld returned 1 exit status

    Do you knew what it means.
    It’s been 15 days since I’m trying to do it and without succes

    thank at advance

  85. I am trying to interface the mpu 650 module to node mcu to calibrating the values for robot i am
    trying to useed the display the calibrating values (X-Y axis) please provide the code for node mcu.
    thankyou.

  86. I have a problem with MPU6050 gyro sensors, I am making school project with Vehicle Accident GPS tracking. When the normal condition, eventhing is ok. Gyro is working his axis,but when over limit gyro’s x,y,z values, LCD and Serial monitor is stop their process. how can i do that?

  87. Hello,
    thank u for the great job.
    I want to know what is the principle of the DMP ?
    before the fusion algorithme, how it get the angle?
    which integral or formule it use to get angles?

    1. Hi,
      I don’t know how the values are computed. I believe that information is proprietary, though you could search the manufacturer’s website.
      Best,
      Debra

Leave a Reply

Your email address will not be published. Required fields are marked *