Help with MPU-6050

Question

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

Answer 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;
}