Friday, February 22, 2013

AVR Atmega 10dof IMU attitude estimation using Mahony filter + processing

Updated to version 02

This is a 10DOF (Degrees of Freedom) AVR Atmega libray.
An inertial measurement unit, or IMU, is an electronic device that measures and reports on a craft's velocity, orientation, and gravitational forces, using a combination of accelerometers and gyroscopes and magnetometers.

This IMU is built on:
  • 3DOF ADXL345 3-axis accelerometer
  • 3DOF L3G4200D 3-axis gyroscope
  • 3DOF HML5883L 3-axis magnetometer
  • 1DOF BMP085 barometer

Attitude estimation is computed using Mahony complementary filter algorithm available from here:

Setup parameters are stored in files:
adxl345/adxl345.h (setup the accelerometer)
l3g4200d/l3g4200d.h (setup the gyroscope)
hmc5883l/hmc5883l.h (setup the magnetometer)
bmp085/bmp085.h (setup the barometer)
imu10dof01/imu10dof01.h (setup the 10dof attitude estimation)

This library was developed on Eclipse, built with avr-gcc on Atmega328 @ 16MHz.

  • 02: attitude estimation refactored to fit standards
  • 01: first release


  • read risk disclaimer
  • excuse my bad english


  1. Replies
    1. Hello, this one is from a chinese ebay seller.

  2. Hello Davide. I've got a question: how shoud I connect SDA and SCL to Arduino? SDA to A5 and SCL to A4?

    1. Hello. It's depends on Arduino, for ATmega based most of the time A4 SDA, A5 SCL. Also use pull-up 4k7 resistors.

  3. OK. I've already connected but the cube is not rotating. Is that because of I don't have this resistors?

    1. It could be. But it could also be other things. At first you check your i2c bus with a logic analizer if you have one. If not, debug using UART. Check where and if the firmware get blocked somewhere. Then, when you have check that firmware work, you can check the UART to pc communication bus.

  4. Hi, I'm tring to use your code with mpu-6050 and hmc5883l. I modified a liitle bit your mpu-6050 library and i'm getting gyro,accel and megnetometer readings. The problem starts when i want to filter that thata with 9dof mahony filter. I modified two functions: mpu6050_mahonyUpdate and mpu6050_updateQuaternion(). You can see my code here:


    When i run this program nothing is send through rs-232, it seems like the whole program stops in mpu6050_mahonyUpdate function - which i also took from your library. Do you havy any ideas why?

    1. Hello, try to debug your code to check the exact point where the things stops to works. Note that on Atmega328 it may also be a problem of RAM usage, it also can be that you are trying to make too much thins in the Timer clock.

    2. Hi, thank you for your answer, the reason of that was very simple - I forgot to include libm :) unfortunately i have another problem, it turned out that my megnetometer readings are corrupted. When i collect magnetometer readings to file it looks like this:

      156.0 -261.0 -474.0
      157.0 -262.0 -473.0
      224.0 -7968.0 -7968.0
      156.0 -261.0 -468.0
      -7968.0 -7968.0 -7968.0
      156.0 -261.0 -468.0
      155.0 -262.0 -475.0
      154.0 -261.0 -477.0
      152.0 -264.0 -478.0
      153.0 -261.0 -474.0
      154.0 -263.0 -471.0
      155.0 -260.0 -472.0
      157.0 -262.0 -471.0
      224.0 -7968.0 -7968.0
      153.0 -260.0 -471.0
      157.0 -261.0 -473.0

      Every 5-7th measurement is wrong, I think that is because I connected SDA i SCL lines from mpu-6050 and mc5883l together. When I connect only hmc5883l without mpu-6050 everything works correct. In main file i call function init() which looks like this:
      void init(){

      The rest you can see here:

      Do you have any idea why it happens? Maybe when i read magnetometer i should put mpu-6050 to sleep mode? Or do you know how to read magnetometer through mpu-6050 auxilary lines?

    3. You should check the I2C bus with a logic analizer.
      Connecting the both chip to the same line, should not make conflict, but it could happens that mpu6050 get a data ready interrup, and try to read data, affecting the magn communfication.
      You could try connecteing the mpu6050 in pass-throught mode, look at the mpu6050 page 26 (7.11) of datasheet.
      Or you could write some sorte of stand by interrupt while reading magentometer.

  5. This comment has been removed by the author.

  6. Hello sir ,I am using 6050 for (gyro+acc) and lsm303 for magnetic reading . It works fine if i use only gyro and acc and the cube shows not overshooting or lag.But as soon as i include magnetic readings in code, and process ,the cube starts overshooting and there is lag.A little help will be appreciated .Thanks in advanced. My controller is atmega 16

    1. Debug you magnetometer data. Debug your timer interrupt, Mahony algorithm takes some resource, and need precise timing. Check that you are running at 16Mhz. Check that x,y,z correlation is correct.

  7. Hello Davide, do you have any description of the Mahony algorithm?


    1. Hello, you can find the original madgwick paper, and the mahony reference code: in this page here:
      It comes with a little of math, but it is well explained and understandable.

  8. Hola Davide, tenes la carpeta con el proyecto en eclipse para abrirlo directamente en eclipse?

    Hello Davide, have you a proyect in eclipse for open it diretcly with eclipse?

    1. Yes, if you give me your mail i can send it to you.

    2. Thanks!!! mail: GOOD WORK

  9. Hi Davide:

    I have implemented the mahony filter using the mpu6050 and hmc5883l magnetometer. it seems like the pitch and roll are working properly, however the yaw angle is not really converging.... any tips on fixing the issue?

    1. Hello, there may be many things that make the yaw angle not working. It may be some calibration issue, it may be F_CPU settings, proportional gain, integral gain, sample frequency. It's something about a drift?

  10. Hello Davide,

    Thanks for your reply. I guess I should be more clear on the hardware I have been using.

    I am using APM 2.6 along with "HAL library" for coding. I couldn't really answer what F_CPU settings I am using.

    The problem exhibits the yaw will never converge to a certain value. the proportional gain is being set to 150, and the integral is 0.5, I don't really understand the mahony algorithm too well. I have integralFBX = 0.3 integralFBY = 0.3 integralFBz=0.3. what are these values for?
    I used the system timer to checked the sampling rate, it is around 189 Hz as for now.
    I don't really know where to look up for F_CPU setting. as this HAL library have everything coded inside. I can try to find it, if that piece of information is vital for getting the code to work. Thank you.

    1. Ok, so you are on STM32. If you are using my library you have to check that "#define imu10dof01_sampleFreq" should be the imu10dof01_updateQuaternion launch frequency. Proportional in intefral gain is a little to much to me, the sould be someting next to 1. integralFBX, integralFBY, integralFBz=0.3 are runtime integral feedback variables. Take a look at the papers here: There's a bit of math there but it's what is lying on the Mahony alghotritm.

    2. Dear Davide:
      I did a bit more work on this Mahony filter, I fixed a few problem, calibrated the magnetometer, gyro and accelerometer. I wrote a if statement code to ensure the sampling frequency stays at 1000Hz.
      Now the problem is all three values ( pitch, roll and yaw) are drifting. if I do move the sensor, the values shows very erratic change. Also, if I change any one of the values of pitch roll or yaw, it will affect the other two.. the proportional gain is 1 and the integral gain is set at 0.7.
      do you have any suggesstion?

  11. I start to suspecting the formula I used to convert from quaternion to euler angle is wrong.... which formula did you use to convert quaternion to euler angle?

    1. Take a look at the processing sketch (the .pde file). You can find the commented formula there.

  12. Dear Davide:

    I have tried different way to fix the filter, but it seems like all the values are drifting, have you ever run into this kind of problem? thanks.

    1. I have tried different boards, none of them give me big drift.