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

Attachments:

Reply to This

Replies to This Discussion

I got that code from a math website http://www.euclideanspace.com/maths/geometry/rotations/conversions/...

At certain attitudes, he warns of "singularity" which this tests for and corrects.

/*
if (DCM_Matrix[1][0]> 0.998 ){
euler_z = atan2(DCM_Matrix[0][2], DCM_Matrix[2][2]);
euler_x = M_PI/2;
euler_y = 0;
}
if (DCM_Matrix[1][0] < -0.998){
euler_z = atan2(DCM_Matrix[0][2], DCM_Matrix[2][2]);
euler_x = -M_PI/2;
euler_y = 0;
}
*/

I'm not sure if I got the axis order right, but is how to use quaternions to get a matrix and then use that matrix to get eulers.  It will work with Martin's code and faster and smaller than how I first tried to do it. 

Harry good night. I wonder if it is to implement some sort of filter to help reduce some errors in the values ​​of my arduimu v3. Thank you.

Evan could help me, because the values ​​of my valore with angular velocity are too high for small angles. What do I do?

I noticed that too.  I had better results when I used FreeIMU to output quaternions and then build a 3x3 matrix and use that to compute the eulers.  It's much faster than the way I first tried it and it doesn't produce any erroneous readings.

Another doubt my values of the Euler angles according quemovimento aarduimu v3 introduces small angles such as 10 ° or 20 °, but my values are angular velocity 140 ° / s or economies. Is wrong?

Hi Martin,

 

I have a project with an arduIMU V3 and the aim is the following:

Detect a Full Rotation around Yaw, Yaw45, Pitch, Roll Axis, and standing Flat,
The software should set on the corresponding LED:
No Moves (standing flat) = Yellow
Yaw 360° = Blue
Pitch 360° = Orange
Roll 45 or 315° && Full Yaw Rotation = Green
Roll 360° = Red
Keep any of the detected movement leds ON during 10s

I am very stuck with this project for the following reasons:

-I have tried lots of solutions without finding any that would give me stable results(FreeIMU, DCM, I am now trying to use yours which actually give more stable results)

-The problem is that I cannot find any software that gives me independant results for each axis, If I do a roll or a pitch, yaw always have glitches ( which would be normal for pitch since you invert the board, but it is never predictible) I can send you a copy of the traces I have got for each algorithm if you'd like.

-I don't have much time to dedicate to this project anymore

 

Would you be interested to finish this project?


I am ready to pay.

 

Thanks a lot,

 

Best Regards.

Attachments:

Yes

Hi Martin,

Came across your post whilst researching ways of integrating the onboard MPU6000 of the APM2.6 into my custom rover firmware.

You have documented the code extremely well, thanks so much for posting.  What's your thoughts on possibilities of getting this up and running on the APM 2.6.  After changing ChipSelPin1 to 40 (having checked the example MPU6000 sketch in the AP_InitialSensor library) I spun up the example, but get so far as:

############# MPU-6000 Data Acquisition #############
Initializing SPI Protocol...
...SPI Protocol initializing done.
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Initializing Digital Motion Processor (DMP)...
Resetting MPU6000...
SPI (/CS40) reading 1 byte from register 0x6B... 0xFF (done)
SPI (/CS40) writing 1 byte to register 0x6B... 0xFF (done)
bit_7 set to 1
Disabling sleep mode...
SPI (/CS40) reading 1 byte from register 0x6B... 0xFF (done)
SPI (/CS40) writing 1 byte to register 0x6B... 0xBF (done)
bit_6 set to 0
Selecting user bank 16...
SPI (/CS40) writing 1 byte to register 0x6D... 0x70 (done)
Selecting memory byte 6...
SPI (/CS40) writing 1 byte to register 0x6E... 0x06 (done)
Checking hardware revision...
Revision @ user[16][6] = FF
Resetting memory bank selection to 0...
SPI (/CS40) writing 1 byte to register 0x6D... 0x00 (done)
Reading OTP bank valid flag...
SPI (/CS40) reading 1 byte from register 0x00... 0xFF (done)
bit_0 = 1
OTP bank is valid!
Reading gyro offset TC values...
SPI (/CS40) reading 1 byte from register 0x01... 0xFF (done)
SPI (/CS40) reading 1 byte from register 0x02... 0xFF (done)
SPI (/CS40) reading 1 byte from register 0x03... 0xFF (done)
X gyro offset = 63
Y gyro offset = 63
Z gyro offset = 63
########################### Writing DMP code to MPU memory banks (1929 bytes)
Writing DMP memory.......... @@@ write bank 0
SPI (/CS40) writing 1 byte to register 0x6D... 0x00 (done)
SPI (/CS40) writing 1 byte to register 0x6E... 0x00 (done)

...

...

@@@ write bank 1
SPI (/CS40) writing 1 byte to register 0x6D... 0x01 (done)
SPI (/CS40) writing 1 byte to register 0x6E... 0x00 (done)

...

...

>> read bank 6
SPI (/CS40) writing 1 byte to register 0x6D... 0x06 (done)
SPI (/CS40) writing 1 byte to register 0x6E... 0x00 (done)
$$$ dmpMemory: byte verification error
FF
$$$ dmpMemory: byte verification error
FF
$$$ dmpMemory: byte verification error
FF

and so on, with a huge list of verification errors.

Any thoughts on this would be most appreciated, many thanks.

Hi Daniil,

I too am trying to get this code working on an APM (version 2.6).  I would be most most grateful if you are able to attach a working version of the code, many thanks.

I wonder how the matrix of direction cosines (DCM). The function of gyroscopes and accelerometers in this matrix.

Hi Martin,

First of all thanks for your code it waa a good start point for me. I have the APM 2.6 and I would like to plot  row, pitch and yaw values. I have commented in #define OUTPUT_READABLE_ROLLPITCHYAW. However when i run the sketch and use the serial monitor to view my results it give me the message shown in the word document i have attached. Any suggestions as to why this is? Thank you

Attachments:

This is what I see in  the serial monitor


$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
FAILED!
Writing DMP configuration... done.
Verifying DMP configuration... FAILED!
Writing DMP update 1/7 ..... done.
Verifying DMP update 1/7 ..... success!
Writing DMP update 2/7 ..... done.
Verifying DMP update 2/7 ..... FAILED!

############# MPU-6000 Data Acquisition #############
Initializing SPI Protocol...
...SPI Protocol initializing done.
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Initializing Digital Motion Processor (DMP)...
Writing DMP memory.......... done.
Verifying DMP memory.......... $$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte verification error
$$$ dmpMemory: byte veri

Reply to Discussion

RSS

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service