Find here working Arduino sketch for MPU-6000 / ArduIMU+ V3 using Digital Motion Processor (DMP)

Maybe you are interested in my Arduino sketch specifically written for the MPU-6000 on the ArduIMU+ V3 board from 3DRobotics Inc. It is attached to this post (v053_MPU6000_DMP6_SPI.ino). Please read the header carefully because it contains a lot of useful information.My sketch is based on Jeff Rowberg's MPU6050_DMP6_I2C sketch which makes use of the Digital Motion Processor to obtain the quaternion values from the MPU, and from there derives roll, pitch and yaw without almost any drift. My sketch is a translation of Jeff's sketch and also uses the DMP, but makes use of the SPI protocol i.s.o. the I2C protocol for transferring data (on ArduIMU+ V3 "MPU-6000 uses SPI for max performance"). My sketch works perfectly with the Teapot demo also from Jeff, I only corrected the axis ((Teapot_ArduIMU_V3.pde - attached). Please let me know if you like it. It took me a little over 100 hours to write and test it.

Views: 23219


Reply to This

Replies to This Discussion

Hi Martin,

Would it be possible for you to share your sketch with GPS implemented? I've been trying to ad GPS output without having to revert to an ancient Arduino IDE (023 or below) but haven't had any luck. 

I've been able to get your original sketch working without any trouble and have been impressed with the stability. Thanks for posting that. 

All the best,


The latest that I've tried is to use Martin's sketch to feed the gyro and accel to the DCM.  It seems counterproductive, but just maybe it is a hair faster.  I thought it might help me to understand the DCM if nothing else, so I tried it.  What I got was strange.  Using the ArduIMU Test, the plane spins and rotates and flips.  I can see the DCM reset and then it starts all over, looks like it's breathing in and out.

I wonder what the maximum angle of roll and pitch that arduimu v3 supports. Because it passes the 90 ° angles vary greatly.

I wonder what the maximum angle of roll and pitch that arduimu v3 supports. Because it passes the 90 ° angles vary greatly.

If your using Martin's sketch it won't show inverted orientation, if that's what you mean.  That has something to do with how the euler angles get calculated from the quaternions.  He warns about gimbal lock too where two axis are aligned the same. 

My doubt is only in relation to how much tilt my arduimu v3. I'm using the method of DCM. I realize that looking at an angle of 45 ° or 90 °, but not relate to observe the roll of my schedule. Appears as an angle smaller than observe. What is happening?

What is the calibration of sensors arduimu v3? Thank you.

I got it working using DMP and feeding raw gyro and accel to DCM.  It was spinning and flipping because I had to work on the scaling and the part of DCM where it outputs the Omega_Vectors wasn't activated.  Was missing a #define. 

It's faster so it seems worth the effort.  It does have a glitch in yaw though.  Every once in a while it resets the yaw to 0 and then it goes back to the right heading.  I have no idea why it does that or how to fix it. 

If anyone wants to try what I did, I can send it to you.   

I would like to send to me. Thank you.

Hello everyone!
Sorry for my English, I use "google translate". :)

There was a problem, I adapted this code (v053_MPU6000_DMP6_SPI.ino) for the board APM2.52.
During operation, periodically, the following error:

     Serial.print ("degrees");
     Serial.print (euler_x); Serial.print ("\ t");
     Serial.print (euler_y); Serial.print ("\ t");
     Serial.print (euler_z); Serial.print ("\ t");
     Serial.print (raw_q_x); Serial.print ("\ t");
     Serial.print (raw_q_y); Serial.print ("\ t");
     Serial.print (raw_q_z); Serial.print ("\ t");
     Serial.print (raw_q_w); Serial.println () ;/ / * /

degrees 2.02 0.64 1.54 -288 -95 -218 16379
degrees 2.03 0.63 1.54 -289 -94 -219 16379
degrees 2.03 0.63 1.55 -289 -94 -220 16379
degrees 2.04 0.62 1.56 -290 -93 -222 16379
degrees 2.04 0.62 1.57 -290 -93 -223 16379
degrees 2.04 0.62 1.58 -290 -93 -224 16379
degrees 65.42 nan -89.08 -11808 -135 27673 19500 / / this is a bug
degrees 2.09 0.56 1.79 -298 -85 -254 16379
degrees 2.09 0.55 1.79 -298 -84 -255 16379
degrees 2.10 0.55 1.80 -299 -84 -256 16379
degrees 2.10 0.55 1.81 -299 -84 -257 16379
degrees 2.10 0.55 1.82 -299 -84 -258 16379
degrees 2.10 0.55 1.82 -299 -83 -259 16379

At this time the board is on the table is stationary. In what could be the problem?
Thank you! :)

I think I've seen that bug myself.  The IMU will suddenly show a strange blip and then back to normal.  I'm having better luck working with Fabio's FreeIMU code.   

float sqw = q[0]*q[0];
float sqx = q[1]*q[1];
float sqy = q[2]*q[2];
float sqz = q[3]*q[3];

float DCM_Matrix[3][3]=
{(1-2*sqy -2*sqz), (2*q[1]*q[2]-2*q[3]*q[0]), (2*q[1]*q[3]+2*q[2]*q[0])},
{(2*q[1]*q[2]+2*q[3]*q[0]), (1-2*sqx-2*sqz), (2*q[2]*q[3]-2*q[1]*q[0])},
{(2*q[1]*q[3]-2*q[2]*q[0]), (2*q[2]*q[3]+2*q[1]*q[0]), (1-2*sqx-2*sqy)}

if (DCM_Matrix[1][0] = 1){
euler_z = atan2(DCM_Matrix[0][2], DCM_Matrix[2][2]);
euler_x = asin(DCM_Matrix[1][0]);
euler_y = 0;
if (DCM_Matrix[1][0] = -1){
euler_z = atan2(DCM_Matrix[0][2], DCM_Matrix[2][2]);
euler_x = asin(DCM_Matrix[1][0]);
euler_y = 0;

euler_z = atan2(-DCM_Matrix[2][0], DCM_Matrix[0][0]);
euler_y = asin(DCM_Matrix[1][0]); //,DCM_Matrix[2][2]);
euler_x = atan2(-DCM_Matrix[1][2],DCM_Matrix[1][1]);

Serial.print(SOFTWARE_VER); //output the software version
Serial.print ("EX0:");
Serial.print (",EX1:");
Serial.print (",EX2:");
Serial.print (",EX3:");
Serial.print (",EX4:");
Serial.print (",EX5:");
Serial.print (",EX6:");
Serial.print (",EX7:");
Serial.print (",EX8:");
Serial.print (",");
//Serial.print(roll * 180/M_PI);
Serial.print(euler_z *180/M_PI);
//Serial.print(pitch * 180/M_PI);
//Serial.print(yaw * 180/M_PI);
Serial.print (",");

Reply to Discussion


© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service