My first 6 DOF IMU Sensors Fusion Implementation: ADXL345, ITG3200, Arduino and Processing
When you have created two breakout boards for the ADXL345 accelerometer and the ITG3200 gyroscope and you have those two nice sensors what you can do? Simple: create an implementation of an Attitude sensor fusion which runs with those chips!
So, I started documenting myself and reading lot of stuff on IMUs (inertial measurement units - composed by an accelerometer and a gyroscope) and MARG sensor (an IMU which also has a magnetometer to sense heading).
One really good article you should read to get started with IMUs is A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications available on http://www.starlino.com/ . Also from the same website there is a nice implementation of the theory from the IMU guide: you can find it on Arduino code for simplified Kalman filter. Using a 5DOF IMU.
So, I took the algorithm above and converted it to be used with the ADXL345 and the ITG3200. I came out with two implementations both with parts in Arduino and Processing code (see the attachments to this page below).
IMPORTANT: If you use the IMU Digital Combo Board from Sparkfun you will need to change the address of the Gyroscope from 0x69 in my code to 0x68.
The first implementation simply reads the raw accelerometer and gyroscope values on the Arduino while the Processing code (running on the PC) will compute the sensor fusion algorithm, produce the orientation vector and siplay a nicely oriented cube.
In other implementation all the sensor fusion logic is implemented in Arduino code so it will run embedded in the microcontroller. Looks like Arduino can coupe with that without any problems! So, once the orientation is computed is all sent to the PC where the Processing application will display incoming data and the oriented cube.
You can see a demonstration in the following video:
All the code is available in the attachments below. You can see the circuit in the picture below. Note that these are custom made breakout boards, your pin configurations will vary depending on the schematics of your breakout board. If you have questions just leave a comment below.
The processing programs need to be executed on the Processing IDE, available for download from http://processing.org/
Remember to adjust the serial port configuration in the Processing code to match your connection to the Arduino in your system. Here I use /dev/ttyUSB9, if you are under windows you'll probably have to use something like COM3 or something like that. See in the Arduino IDE under Tools->Serial Port to get the exact value you have to use.
| Attachment | Size |
|---|---|
| Arduino Code IMU 6DOF - ADXL345 ITG3200 on chip filter | 15.03 KB |
| Arduino Code IMU 6DOF ADXL345 ITG3200 | 13.64 KB |
| Processing Code | 37.56 KB |
| Processing code for on chip version | 38.77 KB |




I`m trying to use this
I`m trying to use this code(Arduino Code IMU 6DOF - ADXL345 ITG3200 on chip filter), but on processing cube turns just randomly. Had any of you experienced such a issues?
thank you for help;)
Oriented cube
Hello,
First of all, thank you for sharing your great work. I'm developing an IMU using a PIC along with Sparkfun IMU - 6DOF ITG3200/ADXL345. I've succesfully ported your code and I'd like to display an oriented cube like yours but using Labview, instead of Processing... Any idea of how to do it starting from RwEst values? I'm not able to understand how you do it in Processing, but I'm looking for something similar.
Thank you very much.
Accelerometer sensitivity
for accelerometer algorithm is
R_x/y/z = (AdcR_x/y/z * Vref / 1023 – VzeroG) / Sensitivity
I am having problem with value of sensitivity(mv/g).for +/-2g value is 478.5mV/g.But what is the value for +/-4g,+/-8g,+/-16g.I dont event know how to calculate accelerometer sensitivity in mv/g.can you please help.
Calibration
Hi Fabio,
I'm trying to use your code and for some reason when I update the offsets for the breakout board I have nothing changes. I set the offsets to zero and oriented it with z facing downwards, had a look at the serial monitor, and then noted the offsets so I could put them in and get zero for x and y when it's in this orientation. They don't read zero, but read the same number that they were when the offsets were zero. I saved after the update and uploaded the code again, so I don't know what I'm missing.
I just saw that those offsets
I just saw that those offsets are for the gyroscope. I corrected that and now I'm just going to add in my own terms to calibrate the accelerometer.
Accelerometer data
Hi Fabio
Very nice guide btw!
I am using the combined IMU Board in combination with an Arduino Pro Mini and tried to run your programms on it.
The problem is that i dont get data from the accelerometer, whereas the gyroscope sends different values and seems ok. The serial monitor just shows me -512,-512,511 as accelerometer data and does not change. Do you have an idea what the reason for this could be? Is it possible that only the accelerometer is broken?
Hi there, I have the 6 DOF
Hi there,
I have the 6 DOF Combo Board from Sparkfun. Occasionally the ITG3200 stops working. Any idea why or anybody with the same problem?
Hi, i am trying to use this
Hi, i am trying to use this code for my imu3000 of 6 DOF that gives data by i2c to my arduino uno r3, I have a code that gives me each data but I don’t know what do I have to do to change In your code so I can see it in the simulation of processing? And I install processing but I don’t know what do I have to open with all the folders that you attach
and what version of arduino
and what version of arduino software are you using?
How can I used it in a dspic30f
Hello I am using and imu-3000 that outputs data of 3axis gyro and 3 accel with i2c I want to ask you if is more harder to use this imu with a dsp30f4011 and programmed with the software mikroC
Hi, I'm not familiar with
Hi, I'm not familiar with that development environment, so I don't really have an idea.
Quaternion
Hello, I am a big fan, I just wanted to know whether you are using the quaternion method for state estimation in your code, if so in which part? Is it at the part where you combine the accelerometer and gyroscope values?
No, this version of the code
No, this version of the code doesn't use the quaternion approach. You can have a look at the FreeIMU library if you are interested in that.
Using two IMUs
Dear Fabio.
Thank you very much for this post, it´s really useful.
I´m working on a bit more complex kalman filter, by using different sensors: 2 IMU and an inclinometer....
I´m using two IMU Digital Combo Board from Sparkfun, but actually I only need to use the Gyroscopes of each IMU.
The problem I have is that I don´t know which pin of the Arduino I have to plug the SCL and SDA from the IMU.
As you say, I need to change the address of the Gyroscope from 0x69 to 0x68.Regarding the inputs to Arduino, the SDA and SCL are plug to A4 and A5 respectively. This works if I use only 1 IMU.
But if I need two IMUs:
what is the address of the Gyroscope for the second IMU?
which inputs of Arduino I have to connect the SCL and SDA?
where can I define these inputs?
Thank you very much,
Sergio
In order to connect two
In order to connect two ITG3200 on the same I2C bus you need to modify the AD0 pin on the ITG3200. Unfortunately, the breakout board you are using doesn't have configurable address for the ITG3200.
An option may be to use an I2C multiplexer or using an IMU which has configurable sensor address, my FreeIMU offers this possibility as you have pads on the bottom which allows to change addresses of the sensors.
calibration.
Hello Fabio...how can i get zero reading on accelerometer raw values when its flat...and zer0 for gyro when not moving..? Do you have a the code for calibrating the sensors zer0..? Thank you.
See
See http://www.varesano.net/blog/fabio/drift-free-attitude-and-heading-estim...
How.?
How can i use these file extensions... The .py,.m,.ui etc..! Shame on me.
Please kindly help
Hello Everyone.
I'm very much a newbie to all of this. And I'm having a bit of difficulty -
I've uploaded the sketch successfully to my Arduino Uno. I've installed Processing and hit Run of the pde. I don't see a square, neither do I see any movement.
I looked at the serial readout from the Arduino directly and all I'm seeing is a lot of strange characters in succession. What am I possibly doing wrong?
Thank you so much for your kind help!
Have you modified the
Have you modified the Processing code so that it uses the correct Serial interface?
Thank you for the reply
Thank you for the reply Fabio!
I have - and I'm receiving this error: readBytesUntil() byte buffer is too small for the 358 bytes up to and including char 10
Accelerometer calibration
Hi Fabio,
thank you for providing these amazing posts and for sharing your knowledge!
The following question could be a bit off-topicm I don't know...
I found one of your post in the page http://code.google.com/p/adxl345driver/issues/detail?id=1 in which you link a document concerning accelerometer calibration
http://www.st.com/stonline/products/literature/an/17353.pdf.
Because the link is no more available, can you send me that document?
Thanks
Alessandro
We implemented it in
We implemented it in http://www.varesano.net/blog/fabio/drift-free-attitude-and-heading-estim...
That calibration datasheet can be found googling for "AN3192".
Hi, So i got the sparkfun
Hi, So i got the sparkfun version and I uploaded the files with chip filter, where serial screen gives me hex numbers, and with processing, it looks like your second part of video, it shakes too much, first part was pretty smooth, so I thought to use the file that doesnt have chip filter, but i ran into Sprintf problem down there! can you help me! thank you
I just updated the On Chip
I just updated the On Chip version with a fixed code for Processing which should give much better results. Please try that.
What sprintf problems are you having?
I resolved that problem, so
I resolved that problem, so I'm going to put this in an airplane, I tried the on chip, which both are at 19200 baud, so when i checked the RwAcc is the same at RwEst, meaning the gyro and accelerometer together are still giving the same result, and it works fine but when i shake it, it is not stable like yours, it does weird stuff. You're look really stable! what should I do because I feel like I'm a little lost!
Thank you very much for the reply!
If you see the older
If you see the older comments, you'll see that this was a bug in the code which had RwEST=RwACC .. the bug should be fixed now in the new code linked above. Please try it.
So i downloaded both the
So i downloaded both the processing and arduino on chip code again, and when i run it, its stable when i shake it, unlike before but its kinda slow and unresponsive, as in if i tilt the sensor a little, pitching, the cube will do a roll first and then go to the angle of the pitch the sensor is at!
Help about using processing
Hi Fabio, thanks for this great blog. Actually I face a problem using processing. does proccessing use the same language of arduino which is (c) as I wrote the code using Arduino and can't implement it in processing. I'd be very grateful if u could help me
The Processing code needs to
The Processing code needs to be executed in the Processing IDE which you have to download from http://www.processing.org/
Dear Fabio I managed to get a
Dear Fabio
I managed to get a FreeIMU sensor.
I managed to upload the code successfully using arduino UNO.
However, I wonder why there are no readings in serial monitor?
Is it because of the sensor problem or my connection is loose?
Hi thanks for getting a
Hi thanks for getting a FreeIMU! If you have a v0.4.3 board, you don't have to use the code in this page. You have to use the FreeIMU library.
Strange peak
Hi Fabio,
I'm working with an ITG3005 and ADXL345, got them working together on an Arduino (my own libs), combined the values in a Kalman filter ... so far so good, all data ok, but ... every x seconds, randomly, i get a gyro peak, even when the board is steady.
Did you ever experience that ?
Great work by the way, you got me motivated to develop my own quadcopter ;)
No, I never experienced
No, I never experienced that.. I would suggest checking for stable power to the sensors and for overflow in the sensor values reading.
Good luck!
Gyro data zero position offset correction
Hi Fabio,
I have been doing a bit more work with your program, converting it to work on the Beagleboard & Raspberry Pi and I noticed another part of the code I would like to ask you about. In the code for the ‘on chip filter’ version, in the subroutine ‘getGyroscopeData’ the 3 lines of code:
result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
should the g_offx, g_offy, g_offz values be added (as here) or subtracted?
Ernie
Code Please
Hi Ernie,
Is it possible to share with us the code for linux, it would greatly help newbies like me :D.
That really depends on your
That really depends on your gyroscope chip. Ideally, when offsets get summed with the raw values, you should get near zero readings when the gyro is held still.
Thanks Fabio I take your
Thanks Fabio
I take your point.
Ernie
Estimated values from the Arduino
Hi Fabio,
This is a very useful blog. I have used your code with some success, but I have one question about the process code for the ‘on chip filter’ version. When it reads the data from the Arduino it reads the estimated values using the following code:
RwEst[0] = decodeFloat(inputStringArr[0]);
RwEst[1] = decodeFloat(inputStringArr[1]);
RwEst[2] = decodeFloat(inputStringArr[2]);
Should the input string not be from indexes 11, 12 & 13? The above code will set the same values as RwAcc[x].
Thanks Ernie
Hey Ernie! Nice catch! I
Hey Ernie! Nice catch! I wonder how nobody noticed this before?
Are you able to test and provide a fixed code? I don't currently have any ADXL345/ITG3200 board around..
Thanks!
Thanks for great tutorial
Hi Fabio,
If you change the read estimate code to:
RwEst[0] = decodeFloat(inputStringArr[11]);
RwEst[1] = decodeFloat(inputStringArr[12]);
RwEst[2] = decodeFloat(inputStringArr[13]);
Then the numbers & graphics display the same as the version with the filter code NOT ‘on the chip’. Also if you hold your video when you are showing the 2nd version (on chip filter) you can see that the RwEst values are the same as the RwAcc values.
Thanks Ernie
I updated the code as per
I updated the code as per your suggestion. Please verify the correct behaviour. Thanks!
Dear Fabio, Do you have the
Dear Fabio,
Do you have the code for hmc5883l?
I bought a tiny 9DOF module with hmc5883l from http://www.nbmarket.com/index.php/9dof-imu-itg3205-adxl345-hmc5883l-modu..., and I do not know is there any difference between itg3200 and itg3205.
Thanks!
There is no difference
There is no difference between 3200 and 3205. For the various libraries, refer to the FreeIMU library.
Roll & pitch question
Hi Fabio,
as far as I understood using your code with the Sparkfun board (so with one 3 axis accelerometer and one gyro) I should be able to estimate roll & pitch angle, roght ?
I am not a C expert so ... may I ask you if your "on chip" does still calculate the two angles or how to do that ?
Thanks for your help.
Mcroccolo
PS: I know, next time I will buy a FreeIMU... :-)
Using the code in this page
Using the code in this page you will get a sensor fusion algorithm which computes the pitch and roll. You can also use the FreeIMU library which has support for your board to compute yaw, pitch and roll. Since your board doesn't have a magnetometer, you'll see the yaw angle to drift away with time, but that's something which is unavoidable with this acc+gyro boards.
How do you open a .gz in
How do you open a .gz in processing? It wont open it, it says it has to be a .pde? Any help would be appreciated!
A gz is a compressed
A gz is a compressed archive.. you are expected to open it with something like 7-zip.
Hello, Thanks for this guide
Hello,
Thanks for this guide it has been very helpful for me. But I have a question about the axis.
Here you have combined Gyro[0] and atan2(Acc[0],Acc[2]) in order to filter the data. However the gyro rate you take from Gyro[0] is the rotation around X axis, but the other angle gives the rotation around Y axis. Isn't there anything wrong in this problem.
I checked the example from starlino there the axis for gyroscope and accelerometer is opposite with eachother. I mean the x axis of gyro and y axis of the accelerometer is in the same direction. So this fixes the problem. However here on this sensor, the axis have the same direction.
The code is working nice, therefore I should be missing something here. Sorry for my bad english
By the way I used IMU Digital
By the way I used IMU Digital Combo Board on my project. So I am referring to the axis configuration used on that sensor.
Post new comment