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.
You need to be a member of diydrones to add comments!
Replies
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:
code:
...
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 () ;/ / * /
...
terminal:
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! :)
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.
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. http://diydrones.com/profiles/blogs/demo-showing-how-much
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?
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.
Martin,
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
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?
Thanks
Hi,
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..
Regards
Soren
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 ...
-
1
-
2
-
3
of 3 Next