1

I have bought MPU-6050 a month ago for a project, I have got a hang I2C protocol and register maps of MPU. I have learnt how to get raw values of accelerometer and gyroscope. What I needed for my project is roll and pitch angles and acceleration in x,y,z axes. I have tried some algorithms as shown below:


First I saw a code which uses only raw accelerometer values to get roll and pitch. Here is a small snippet of that code:

x=(AcX)/16384;
y=(AcY)/16384;
z=(AcZ)/16384;
roll = (atan2(y, z)+PI)*57.295779513082320876798154814105;
pitch = (atan2(x , z)+PI)*57.295779513082320876798154814105;

As you can see this uses principle of angle between two vectors, the angles were pretty accurate. But the problem with this code is that as the sensor moves in a particular direction without changing the roll or pitch they still seemed to change (The roll and pitch values are dependent on accel values).
Next, I saw about using Complementary Filter my code:

#include<Wire.h>
const int  MPU_addr=0x68, dt=20;
float AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float xa,ya,za,xg,yg,zg;
float gan_x = 0 ,gan_y = 0,aan_x = 0,aan_y = 0, anx = 0, any = 0, gain  =0.95;
int xao = -521 ,yao = 1073 ,zao = 1724 ,xgo = 94 ,ygo =31 ,zgo = 60;                       //offset values
void setup()
{
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1A);
  Wire.write(0b00000110);
  Wire.endTransmission(true);
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B);
  Wire.write(0b00000000);
  Wire.endTransmission(true);
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1C);
  Wire.write(0b00000000);
  Wire.endTransmission(true);
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0b00000000);
  Wire.endTransmission(true);
}
void loop()
{ 
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  xa = (AcX-xao)/16384;                                                    //sensitivity adjusting
  ya = (AcY-yao)/16384;
  za = (AcZ-zao)/16384;
  xg = (GyX-xgo)/131;
  yg = (GyY-ygo)/131;
  zg = (GyZ-zgo)/131;
  gan_x = dt * xg + anx;                                                       // integrating
  gan_y = dt * yg + any;
  aan_x = (atan2(ya,za)+PI)*57.295779513082320876798154814105;
  aan_y = (atan2(xa,za)+PI)*57.295779513082320876798154814105;
  anx = (gain) * gan_x + (1-gain) * aan_x;                                     // filtering
  any = (gain) * gan_y + (1-gain) * aan_y;
  //Serial.print(" | GyX = "); 
  Serial.print(gan_x);
  Serial.print(" ");
  //Serial.print(" | GyY = "); 
  Serial.println(gan_y);
}

This code gives good gyroscope values in terms of angular velocity but I want it in degrees is there any way to change those gyro values to degrees(like in first case)?


I saw Jeff Rowberg's library and I can't seem understand all of the code. Can anyone please provide any reference material explaining the functions present in i2cdevlib.h (Ex: mpu.dmpInitialize(), mpu.setDMPEnabled() etc) it would be a lot of help.


I also would like to know if there are any other alternative IMUs (Gyro+accel) other than MPU6050 which are more of an open source than 6050 (in terms of accessing DMP which seems to be main drawback for MPU) and easier to work with.
I am using Arduino Nano.

-Thank you in advance

migrated from electronics.stackexchange.com Aug 31 '16 at 19:26

This question came from our site for electronics and electrical engineering professionals, students, and enthusiasts.

  • This code gives good gyroscope values in terms of angular velocity but I want it in degrees is there any way to change those gyro values to degrees(like in first case)? - And what the angular velocity is measured in? – Eugene Sh. Aug 31 '16 at 18:50
  • Angular velocity = Degrees per second, I want gyro values in degrees @EugeneSh. – Teja Allani Sep 2 '16 at 12:01
  • All the references of Jeff Rowberg's library are in his library. His code is pretty well commented. I used the raw.ino of his examples for one of my projects and it worked perfectly. – Len Sep 2 '16 at 12:54
  • Just use Jeff Rowberg's code and go through all the lines in his code, perhaps even open up the .cpp and the .h to get a better understanding what is being done with your code. – Len Sep 2 '16 at 12:54
  • 1
    @Len, I read through every line of code in .h and .cpp file patiently. I finally got an outline of how it works. The problem when I first saw those files was my C++ programming. I didn't know how library is made and what does .h have to do with .cpp file. I thought they were separate and had no link to each other. After hours of googling finally I was capable of understanding link between .h and .cpp that .cpp had function definitions which were declared in .h file. Now I can use that library with satisfaction :D. Thank you for the help. – Teja Allani Sep 4 '16 at 16:32
1

I finally got this working after one sleepless night

#include<Wire.h>
const int MPU = 0x68;
int t = 0,dt = 1;
int AcX,AcY,AcZ,GyX,GyY,GyZ,tmp;
int AcXo,AcYo,AcZo,GyXo,GyYo,GyZo;
float roll = 0,pitch = 0,rollgy = 0,pitchgy = 0,rollac = 0,pitchac = 0,Ax,Ay,Az,Gx,Gy,Gz,gain = 0.95;
void MPUconfig(int Addr,int data)
{
  Wire.beginTransmission(MPU);
  Wire.write(Addr);
  Wire.write(data);
  Wire.endTransmission();
}
void MPUread()
  {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission();
    Wire.requestFrom(MPU,14);  // request a total of 14 registers
    AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
    AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
    tmp=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
    GyX=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    GyY=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
    GyZ=Wire.read()<<8|Wire.read();
  }
void offset()
{
  MPUconfig(0x1A,0b00000000);
  int ax = 0, ay = 0,az = 0,gx = 0,gy = 0,gz = 0,i;
MPUread();
MPUread();
ax = AcX;
ay = AcY;
az = AcZ;
gx = GyX;
gy = GyY;
gz = GyZ;
for(i=0;i<1100;i++)
{
    MPUread();
    if(i>100)
    {
      ax = (ax+AcX)/2;
      ay = (ay+AcY)/2;
      az = (az+AcZ)/2;
      gx = (gx+GyX)/2;
      gy = (gy+GyY)/2;
      gz = (gz+GyZ)/2;
    } 
    delay(2);
 }
AcXo = ax;
AcYo = ay;
AcZo = 16384-az;
GyXo = gx;
GyYo = gy;
GyZo = gz;
MPUconfig(0x1A,0b00000110);
}
void setup() 
{
  Serial.begin(115200);
  Wire.begin();
  MPUconfig(0x1A,0b00000110);
  MPUconfig(0x1B,0b00000000);
  MPUconfig(0x1C,0b00000000);
  MPUconfig(0x6B,0b00000000);
  offset();
}

void loop()
{
  t = millis();
  MPUread();
  Ax = (float)(AcX-AcXo)/16384;
  Ay = (float)(AcY-AcYo)/16384;
  Az = (float)(AcZ-AcZo)/16384;
  Gx = (float)(GyX-GyXo)/131;
  Gy = (float)(GyY-GyYo)/131;
  Gz = (float)(GyZ-GyZo)/131;
  pitchgy = (Gy * ((float) dt/1000)) + pitch;
  rollgy = (Gx * ((float) dt/1000)) + roll;
  pitchac = atan2(Ax,Az) * (float)(180/PI);
  rollac =  atan2(Ay,Az) * (float) (180/PI);
  roll = gain * rollgy + (1-gain)* rollac;
  pitch = gain * pitchgy + (1-gain) * pitchac; 
  Serial.print(roll);
  Serial.print(" "); Serial.print(pitch);
  Serial.println(" ");
  dt = millis()-t;
}
  • Hi, Teja Allani, Thanks for sharing the code. It works. One issue puzzles me though, in the offset subroutine, the code takes the 2nd MPUread() for the initialization. Is there any reason for doing so? I did some tests by marking the 2nd MPUread out and my tests do reveal different results. – Jesse Sep 3 '16 at 4:05
  • Further, it seems that the code yields angles that are limited to (-45, 45). Where do I need to change for angle computation extended to (-90, 90)? – Jesse Sep 3 '16 at 11:40
  • Hello :) @Jesse , The 2nd MPUread() because the first couple of readings are not stable it takes some time for MPU to give stable results, for this same reason I put i>100 in the loop so 1st 100 readings are not used for calibration. – Teja Allani Sep 4 '16 at 14:32
  • and for that angles use atan2() instead of atan() to change the limits of the angles. This code still needs some improvement because I found that when roll or pitch cross 90 degrees they seemed to effect each other( I can't explain this phenomenon clearly test it out after replacing atan() with atan2() ) maybe its because the values of Ax and Ay become negative after crossing 90 degrees and this sudden change from +ve to -ve and effects filter. _Also try changing gain value and observe results _ – Teja Allani Sep 4 '16 at 14:32
  • Thanks for the feedback. I did check that atan() takes one argument and the angle can be between [-90,90], leading me to think that some improvements are needed. Anyway, you got most of it in one sleepless night is quite impressive. Best wishes. – Jesse Sep 4 '16 at 14:41

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.