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


Reply to This

Replies to This Discussion

Martin, thank you very much.

I was gathering information about these subjects for three weeks because I wanted to do the same as you have done.

Your code is a excellent start point.

My objetive is to build an EFIS and test it in a real aircraft. By now I have bought some stuff: an Arduino Uno, an Arduino Mega, an Arduimu+ v3, a FreeIMU 4.3, a GPS (Mediatek 3329), a diferential pressure sensor (MPXV7002DP) ...

Maybe we could colaborate in the future ...

In any case, thank you ...

Hello Manuel,

Gladly done, nice to read you can use my code as a good starting point. Personally I would hesitate to put it into a real airplane, because I don't think any of these components are considered flight tested/ air worthy. But of course, it can be done. Just put a good large red override switch somewhere ;-) Good luck with your project!

Best regards, Martin

Well, I get to OUTPUT_TEAPOT and then nothing happens.  Maybe my version is too early?

Hello Harry,

I'm not sure what you mean by "my version is too early". The revision of the MPU-6000? The Arduino IDE version? The ArduIMU version?

Anyway, this is what you should see in the Serial Monitor after uploading and first startup of the sketch of the ArduIMU+ V3:

############# MPU-6000 Data Acquisition #############
Initializing SPI Protocol...
...SPI Protocol initializing done.
Initializing Digital Motion Processor (DMP)...
Writing DMP memory.......... done.
Verifying DMP memory.......... success!
Writing DMP configuration... done.
Verifying DMP configuration... success!
Writing DMP update 1/7 ..... done.
Verifying DMP update 1/7 ..... success!
Writing DMP update 2/7 ..... done.
Verifying DMP update 2/7 ..... success!
Writing DMP update 3/7 ..... done.
Verifying DMP update 3/7 ..... success!
Writing DMP update 4/7 ..... done.
Verifying DMP update 4/7 ..... success!
Writing DMP update 5/7 ..... done.
Verifying DMP update 5/7 ..... success!
Writing DMP update 6/7 ..... done.
Verifying DMP update 6/7 ..... success!
Writing DMP update 7/7 ..... done.
Verifying DMP update 7/7 ..... success!
... Digital Motion Processor (DMP) initializing done.
Enabling DMP... done.
Enabling interrupt detection... done.
DMP ready! Waiting for first data from MPU-6000...
You have chosen the following output(s):

(hereafter the chosen output(s) starts - repetitive)

Please note the success! after each verification line. If that reads FAILED!, there is an issue. Also, during initializing of the DMP, you should see two short flashes of the blue LED, then the red LED should start flashing about every second (every 20 main loops of collecting a packet of data from the FIFO and creating output). Very brief blue LED flashes now denote a FIFO overflow / FIFO reset.

Have you tried another output besides the Teapot output? Also, you might benefit from reading the entire header of the sketch carefully.

Hope this helps solving your problem. Best regards, Martin

I dont know what's going on.  I can get FreeIMU outputting quaternions and Cube in processing2.0 working.  I was hoping this DMP version would fix the drift.  If I got it right, FreeIMU is accessing the DMP too. 

What I meant by early version is maybe my MPU is different somewhere.  It was one of the first batches of the ArduIMU V3.  I get the exact output you show, but then nothing else.  I'll read your header again.  It compiles so small it leaves a ton of room to do something useful.

I tried quaternions and it does the same thing as teapot, initializes and then nothing.

As far as Z-axis (yaw) drift is concerned, sometimes it takes a few seconds to up to a minute to stabilize, thereafter drift is virtually non-existant. X and Y axis (roll / pitch) are always stable from the beginning. It helps a lot during testing when you tape the ArduIMU+ V3 to a small block of wood or something for stability.

The revision of the MPU-6000 is lasered on the package of the chip, see the info header in the sketch for an explanation.

You could put a Serial.print statement in the main loop to verify if the main loop is running at all. You could also activate the DEBUG state in the sketch (a few lines below the info header), but that might be too much information, and is not always fully commented.

Please dont get me wrong, I think what you did, Martin, is tremendous.  My problem is I'm always in over my head, trying to learn a few things and now I'm stuck again.


Best I can tell, all the banks get written and verified and then DMP is disabled waiting for me to get smart enough to turn it on.

I changed a line of code and it's working. :(

That's good news! Was that a line of your own appended code, or do I have to change one in my original code?

Line 566, I changed mpuinterrupt false to true.  A few lines down it is false.  I'm not sure where it is better to change, but it works.


Great work!

Did you see the "Martinez brushless gimbal" firmware? Also Arduino. It has a relatively simple IMU that performs (imo) amazingly well.

It just appears to do, for each iteration:

- construct gravity vector v1 by accelerometers

- rotate gravity vector v2 by the rotation rate * dt angles, using small-angle approximations sinx=x and cosx=1

- complementary filter v2<--v2*(1-a) + v1*a

- calculate pitch and roll using the usual atan and what more formulae. In their implementation, pitch and roll are really reversed, in order to match the mechanical dependency of the typical gimbal, where rolling moves the pitch axis. There's no yaw but this could be easily added.

Could be fun to performance compare simple vs. full feature..



Wow, I added a section of code to Serial.write roll,pitch, yaw in the DIYd binary format.  I added it as a #ifdef OUTPUT_BINARY, followed Martin's examples and it was simple.  It connects to the Legacy Ardupilot and stabilizes.  No GPS or magnetometer yet which will be tricky for me. 

The more I look at this code, the more I admire and learn from it. 

Reply to Discussion


© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service