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: 22837


Reply to This

Replies to This Discussion

I got confused by the name too. I kind of thought "I don't have a gaming console, and if I had I would not break it up for copter parts. Not for me" :) So I never considered Multiwii.

 The 3DR store says the v3 accelerometer can be set to +/- 16g full scale.


    DEBUG_PRINTLN(F("Setting accelerometer full scale range to +/- 2 g..."));
      SPIwriteBits(0x1C, 4, 2, 0x00, ChipSelPin1);

// first read byte, then insert bit values, then write byte:
// otherwise all other bits will be written 0, this may trigger unexpected behaviour
void SPIwriteBits(byte reg, byte bitStart, byte length, byte data, int ChipSelPin)

So, according to the great commenting and explanations, I would need to change 0x00 byte data to something else to set it at a higher scale.  It looks like the original DCM version set it to 0x08 to get +/- 4 g  

Is there any advantage or disadvantage to changing this?   

You can find all standard MPU-6000 settings in the register map attached. In register 0x1C, AFS_SEL (bits 4 & 3) selects the full scale range of the accelerometer outputs:

AFS_SEL - Full Scale Range:
0x00 - ±  2g
0x01 - ±  4g
0x02 - ±  8g
0x03 - ± 16g

Resolution @ 2g full scale range is 16384 bits / g. This reduces to 2048 bits / g @ 16g full scale range (see attached product specification). So depending on the g-values you expect in your application, it is best to choose the most fitting one without loosing more resolution than needed.

You can easily check if changing the full scale range works: in rest and put on a horizontal surface, the z-axis accelerometer value should read around 2048 @ 16 g full scale range.


"Yaw from accelerometer? Hmm how does that work?" Well, yes, that is the key question. In fact, when enabling the DMP as in my sketch, signals from both the accelerometers and the gyroscopes are combined (or fused, Sensor Fusion as Invensense calls it). This is the -also for me- unexplained part of the sketch in which the DMP is programmed and configured by writing its memory directly with hexadecimal values (like machine code) so that in the end the quaternions (w, x, y and z) created from Sensor Fusion are shifted into the FIFO for our use. Then from the quaternions stable (= without much drift) yaw / pitch / roll angles can be derived. This was in fact the main objective for writing my sketch: to get the quaternions out of an MPU-6000 using the SPI protocol. You might be interested in this video of an Invensense presentation:, which explains Sensor Fusion in some detail from 15:20 to 20:35. If you can find the time, the rest of this presentation is also very interesting to watch.

As already said by Jeff Rowberg in his sketch (and by others), the DMP program and configuration code was reconstructed from observed I2C traffic generated by the demo code from Invensense, and not extracted directly from that code. That is true of all transmissions in this sketch, and any documentation has been added after the fact by referencing the Invensense code.

Outside Invensense and their customers, no one seems to have disclosed the source of and documentation for this code. At least, I couldn't find it anywhere.

So, although it works and leads to stable yaw / pitch / roll angles, in fact it is not known how exactly the sensor data is fused by the DMP.

From 35:30 the (in)famous Teapot is used for explaining rotation. From 43:27 it leads up to explaining the quaternion in only 96 seconds. Hey, don't be scared, just listen to it ;-) "Pretty simple, actually...", just angle & rotation vector (w & x, y, z) combined.

Thanks for the reading.

I just flew it with the accels set to 4g.  It was oscillating too much.  One thing I wonder is if I change the accel range, does that mean I need to change how the quaternions q_w gets calculated?


float q_w = raw_q_w / 16384.0f 

is how It is currently, should it be float q_w = raw_q_w / 8192.0f;  when set to 4g?

The test of the raw accel showed the Z value was about 550 when set to 16g.


It could be the Ardupilot itself and its PID gains are why it was rocking the wings back and forth.  Pitch was good.  I flew it in FBW which limits the bank angle and it seemed to go back and forth between limits.  I didn't crash it, and was able to get it down.  

I should try it at 2g as well.  I know better than to start with a change.

I am struggling with this code... and probably due to theoretical misunderstandings as well.

The conversion between quaterions and Euler angles is clear, but still can't understand how this code works.

Is this code based on a look up table that was built based on empiric tests ? 

Can someone please give a short breakdown of the technic used here to calculate the Euler angles? 


Yes, indeed the raw values should then be divided by 8192 @ 4g full scale range to get the g-values. This division is made in several places in my sketch, you could create a variable to adapt all divisions at once when you change the full scale range (I was happy with 2g).

The value of 550 you get @ 16 g means either the MPU is not completely horizontal, or not completely steady in rest (tape it for instance to a block of wood). My experience is that if you look at the raw values running by on the serial monitor in the main loop, after a short while -depending on how steady the MPU is before moving it- the DMP calibrates the raw accelerometer values to around zero (to can see this as steadily decreasing values).

When the raw values no longer seem to decrease further, rotate it slowly around all three axis to see what the smallest and largest values are that you can get out of it.

What also happened to me was that in floating-point calculations I sometimes did not get enough digits of precision, sometimes it was rounded even to 0 or 1 g only. If that happens somewhere in your calculations (Serial.print all calculation steps to verify), that might explain going back and forth between limits.

But maybe the MPU is just not fast enough and/or your PID control loop starts to oscillate (being unstable).

Good flying you did! Luckily you could get your plane down in one piece, must have been like flying in heavy turbulence. If you have an extra channel on your TX/RX, maybe you could implement an override function that allows you to switch back to full manual control.

You will probably find your answers in some of my replies one page back in this discussion in answer on Søren Kuula's question, and by watching the Invensense presentation mentioned there. If not, please let me know.


Does the DMP compensate for acceleration in the attitude estimation computations? For some applications, it is not necessary to perform acceleration compensation, but for aviation applications it is definitely required.

Without acceleration compensation, there will be significant roll-pitch errors that arise because accelerometers measure the difference between acceleration and gravity.

For fixed wing, approximate compensation can be performed using airspeed.

For multicopter applications, the attitude computations require gyro, accelerometer, and GPS data.

For more information, take a look at these discussions of theory and implementation of acceleration compensation.

If the DMP does perform acceleration compensation, then it will use airspeed and/or GPS ground speed information.

Best regards,

Bill Premerlani


No worries about the switch, I can go manual any time I need.  I stayed with it through a few turns just to see.  It was very consistent, didnt diverge which is a good sign.  I'll get the g worked out and try it again.

More testing ahead.

Reply to Discussion



Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service