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


Reply to This

Replies to This Discussion

I found all the areas where g matters, I think.  I notice after changing the divisor on q that the servos are much smoother, so another flight test tomorrow.

I've successfully flown the DCM version without GPS or magnetometer and am looking to match that.

This is awesome. Finally had the chance to use it last night. I have already started implementing it into my custom code for the Arduimu v3. You just saved a year long project from failure. I will keep you updated with my development. I have made some pretty substantial improvements(I think at least) to the current arduIMU code and this was the final piece of the puzzle. Thanks for sharing. I'll be sure to give you credit and keep you updated on my code.

Have you changed DCM?  Just wondering what you improved.

I've got GPS working if you use Ublox, but it isnt making any centrifigal corrections yet so it isnt very useful except to pass the fix through the binary messages. 

No I haven't done any DCM stuff. I just made it easier to communicate its information with other systems. It was hard to pull information from the IMU originally. I also sped up the system(useless now). I made it lighter and ripped out all the unnecessary code. Added a simple yaw drift compensation. 

Just basic stuff. The biggest thing(for me) was being able to communicate with other systems, in this case my custom autopilot.

My ublox works fine with the arduIMU but I did change some of that code around. I don't remember if it worked originally.

Hello Bill,

Good question. What is happening in detail in the DMP after activation by my sketch is not known to me (and probably to no one outside Invensense and its customers at the moment).What likely happens is that the DMP combines measurements from the accelerometers and gyroscopes to calculate a quaternion (this is what Invensense calls Sensor Fusion), from which for instance roll, pitch and yaw can easily be derived by the user.

The sketch (and so the DMP in the MPU-6000) does not use airspeed or GPS ground speed information at the moment.

However I have connected a GPS module to the ArduIMU+ V3's dedicated GPS input connector, which works fine and is quite easily implemented in the sketch (since all GPS modules have output strings according to the NMEA standard - so most sketch code is not hardware specific). Adding GPS ground speed information should be easy then.

Adding airspeed information can be done by using two simple electronic barometers (like the BMP085) in a pitot-static pressure system. But I suppose you already know that.

I am not investigating this any further, since my system is in a low acceleration environment, in which I only need to know the approximate roll, pitch and yaw angles.

Best regards, Martin

Nice to hear I saved your project by putting my sketch on DIY Drones. I would be very interested in your final sketch after you finish. But first, I wish you success on finalizing your project!

The problem I'm having now is I can get magnetometer and GPS data, but I don't know how to use it on the DMP data.  It seems counterproductive to use DMP and then turn around and feed it to DCM.  Any general ideas on how to apply GPS.groundspeed to the accelerometers and Heading_X/Y to yaw?

So I am currently trying to merge the original code with the your DMP code. The old code provides support for GPS location, yaw drift correction using COG, and centripetal acceleration correction(all things that rely on GPS). Your code provides the DMP which would replaces the out date DCM matrix. 

Integrating GPS and Yaw drift correction is fairly straight forward, but the centripetal acceleration is the tricky part. I know how it works in the DCM. The acceleration vector is scaled based on SOG before it is thrown in the DCM. The problem is that the DMP is currently being used a closed system, meaning the acceleration vector can not be scaled for centripetal acceleration before entering the DMP. 

My question is, is there a way to alter the acceleration values prior to being feed into the DMP? Can inputs be directly written to the DMP or does the DMP only take input from the sensors?

For those who don't know what centripetal acceleration does to the IMU, here's a good post that currently shows the problem with the DMP.

For what it might be worth, I compared plotted output of the DCM version of IMU and then did it again with the DMP version.  The first thing I noticed is that the DMP is about 10x faster.  I used SerialChart to plot euler angles.  I left yaw out and only looked at pitch and roll. First image is DMP.


To get an idea how much faster the DMP is running, I purposely added a delay(100) to the printing and the output started to look similar to the DCM.  Like I said, I only did it to see how much faster it is.


The core DCM algorithm has a latency of one data sample, so it should not produce any delay.

Any delay you see in DCM is likely due to latency introduced by filtering of the raw values produced by the gyros, either by the gyros themselves, or by anti-aliasing analog filters or digital filtering.


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