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


Reply to This

Replies to This Discussion

Psam, I had to change mpuinterrupt to true to get mine to work. I got the same errors until I changed that.


while ((mpuInterrupt == true) && (fifoCount < packetSize))

and then a few lines down from there, it is like this:

mpuInterrupt = false;

Hey Martin,

I'm a student trying to get an MPU-6000 working with an M0 processor over SPI. I'm not familiar with arduino or "sketches", but most of this code looks like C, do you have any idea how to port it? I'd be willing to pay for the inconvenience.

Hello Tanner,

Thanks for your interest in my code.

Unfortunately, I have no coding experience with the ARM Cortex-M0 (or any ARM processor). The Arduino code is indeed C-like, so if you already have experience coding in C or C++, and combined with the comments in my code, you should be able to understand what my code is doing. However, translating it for the M0, I suspect this will be quite a job. I did the translation for Arduino from I2C to SPI (so only the bus interface protocol changed, not the Arduino coding environment), and that took over 100 hours.

But don't let that stop you from trying!

Most importantly you should start writing code (or find open source code) which creates the SPI protocol on the M0. I have glanced at the M0 documentation, it provides some kind of serial wire interface, and since SPI is a really simple protocol, it should not be to hard to get a SPI bus working.

Next step will be a really good understanding of my code, and then translate it piece by piece.Test every small step (so, run the code on the M0 / MPU-6000 board), to see if it works (use a lot of debug statements), and continue building. Give every improved coding file a new number (I always start with _001), to be able to revert to a previously working one if things foul up.

Given the amount of time that will probably be involved in this, I will not be able to do this port for you, I'm sorry.

Do you have a specific reason why you want to use the M0 processor over an Arduino? Since for Arduino, all is readily available...

Best regards, Martin

Sorry Daniil, this answer comes way too late of course, but might be interesting for later readers.

Once in a while, the FIFO inside the MPU-6000 overflows (because the Arduino sketch can't keep up with the MPU-6000 data rate), resulting in an erroneous reading by the Arduino code. At the time and for my application, this was no big issue, since I was only keeping track of the pitch, roll and yaw without actively using the data realtime, and manually filtered out these false readings from the output file afterwards.

However, I realize that if the pitch, roll and yaw readings are being directly used for i.e. flight stabilization, awkward things might happen.

This calls for an improvement in the code to avoid FIFO overflow, or to ignore the reading for one sample. FIFO overflow is noted by the code already, and it shows by a short flash of the blue led, so the code could be adapted from there.

Hello Jon-Paul,

As you might have figured out by now (sorry for the large delay in answering your question), this is most likely because there is no, or no correct, SPI communication between your Arduino processor and the MPU-6000. The code first writes a lot of values over SPI to the MPU-6000 ("writing byte to register (done)" - lines), then starts reading the values back from the MPU and verifies them. The "$$$ dmpMemory: byte verification error" point to a verification failure, so something went wrong in either writing or reading the values to/from the MPU, or both.

In the EAGLE files (schematic and board layout) for the APM 2.6 ( ) I can see that the /CS pin of the MPU-6000 is connected through a voltage level shifter to hardware pin 19 (PB0) of the Atmega2560 Arduino processor. Looking at the pin mapping ( ) you should use ChipSelPin1 = 53 instead of 40.

Best regards,


Hello Psam,

You have the same issue as Jon-Paul, and I have described the probable solution a few replies above ^^.

Best regards,


Hi Harry and Psam, I am running an ArduIMU+ V3 on an Arduino Uno and seem to be getting the same thing as Psam. I've changed the code you have pointed out above but it unfortunately has not fixed the problem. Any help or pointers would be greatly appreciated, thank you.

Thanks Martin

 I finally got it working what happened is that my voltage regulator for my APM 2.6 had blown and as such The MPU6000 was receiving 5V instead of the 3.3V required operating voltage. I remedied this problem temporarily by using a AT Mega 2560 board and connecting the the compass with the 3.3V and ground on the board therefore the AT Mega 2560 was supplying the 3.3 volts needed for the Mpu6000 in the long run I have ordered voltage regulators

This was also the reason for the bad gyro health message I keep observing in mission planner. Once again thank you for your help much appreciated.  helps to understand how to fix voltage regulators

Hello Paven,
I don't understand: you say you are running an ArduIMU+ V3 on an Arduino Uno. But these are both hardware boards, and also completely different. You can't run hardware on hardware...
Do you mean to say you are trying to run my code on an Arduino Uno board? If so, how did you connect the MPU-6000 to it? I suppose the MPU-6000 is on a separate break-out board then.
Please provide more information, so that I will be able to help you if I can.
Best regards,

Hi Psam,

Nice to read that you have it all working now!

Lucky you: MPU-6000 Absolute Maximum Ratings, Supply Voltage VDD: -0.5V to +6V, so the MPU should be undamaged by the 5V supply it accidently got. I'm not sure about the other components - but obviously now you have it working they seem to have all survived.

Good luck repairing the board with a new 3.3 V regulator. Advise: buy +3.00 dioptry reading glasses (those cheap ones from a drugstore will do fine), this helps tremendously for this kind of miniature soldering, because compared to a normal magnifying glass you still see depth, and are not hampered by holding the magnifying glass or the need to keep the board fixed underneath it.

see my comment below on how to make this work on the APM Mini with an ATMEGA2560

I had the above problem as well when using the Mini APM 2.6.First be sure to change the select pin to 53 for the ATMEGA2560.

I found a post with the solution. The barometer uses port 40 as its CS. You have to set it to output and write it high to prevent the barometer from taking control of the SPI.

Add this right before you initialize the SPI.:

pinMode(40, OUTPUT);
digitalWrite(40, HIGH); 

I also made some other changes to do a cleaner reset as per the data shoot for the MPU6000. So my test vehicle had the following for the MPU init:


byte dmpInitialize()
// Trigger a full device reset.
// A small delay of ~50ms may be desirable after triggering a reset.
DEBUG_PRINTLN(F("Resetting MPU6000..."));
SPIwrite(0x6B, 0x80, ChipSelPin1); // DEVICE_RESET
DEBUG_PRINTLN(F("Disabling sleep mode..."));

SPIwrite(0x68, 0x7, ChipSelPin1); // reset sensors
delay(100); //
SPIwrite(0x6A, 0x10 ,ChipSelPin1); // disable I2C
// Setting the SLEEP bit in the register puts the device into very low power
// sleep mode. In this mode, only the serial interface and internal registers
// remain active, allowing for a very low standby current. Clearing this bit
// puts the device back into normal mode. To save power, the individual standby
// selections for each of the gyros should be used if any gyro axis is not used
// by the application.
// disable sleep mode
SPIwrite(0x6B, 0x00, ChipSelPin1); // wake up from SLEEP

// get MPU hardware revision

<original code resumes here>


This took a long time to figure out, as I had checked everything in the code twice. Now I just need a I2Cdev lib that has SPI under the hood.....


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

© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service