I have been working on MPU6050 for some time and with extensive online search have managed to write a code that produces almost stable and usable values from MPU6050 in degrees i.e Yaw Pitch Roll. The code is simple with complementary filter and works great except for on roll axis. The roll axis also works fine on positive roll, however negative roll doesn't return back to 0 after the sensor is placed back on flat surface and seems to take some time to return to zero. I have attached the code and response graph after plotting the values on MS Excel where you can see stable response except for -Y axis which drifts and takes time to return to 0.
Please any kind of help will be really appreciated.
#include "Wire.h"
#define alpha 0.98
const int MPU=0x68; // I2C address of the MPU-6050
float timeStep, time, timePrev;
float arx, ary, arz, grx, gry, grz, gsx, gsy, gsz, rx, ry, rz, gyroScale = 131;
int16_t ax_offset=0, ay_offset=0, az_offset=0, gx_offset=0, gy_offset=0, gz_offset=0;
int16_t axraw, ayraw, azraw, gxraw, gyraw, gzraw, temp;
int i;
void setup() {
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(115200);
caliberate();
time=micros();
read_mpu();
rx = (180/3.141592) * atan(ayraw / sqrt(square(axraw) + square(azraw)));
ry = (-180/3.141592) * atan(axraw / sqrt(square(ayraw) + square(azraw)));
rz = 0;
}
void loop() {
timePrev = time;
// collect readings
time = micros();
read_mpu();
// set up time for integration
timeStep = (time - timePrev) / 1000000; // time-step in s
// apply gyro scale from datasheet
gsx = gxraw/gyroScale;
gsy = gyraw/gyroScale;
gsz = gzraw/gyroScale;
// calculate accelerometer angles
arx = (180/3.141592) * atan(ayraw / sqrt(square(axraw) + square(azraw)));
ary = (-180/3.141592) * atan(axraw / sqrt(square(ayraw) + square(azraw)));
//calculate gyro angle
grx = rx + (timeStep * gsx);
gry = ry + (timeStep * gsy);
grz = rz + (timeStep * gsz);
// apply filter
rx = (alpha * grx) + ((1-alpha) * arx);
ry = (alpha * gry) + ((1-alpha) * ary);
rz = grz;
Serial.print(ax_offset);Serial.print("\t");
Serial.print(ay_offset);Serial.print("\t");
Serial.print(az_offset);Serial.print("\t");
Serial.print(gx_offset);Serial.print("\t");
Serial.print(gy_offset);Serial.print("\t");
Serial.print(gz_offset);Serial.print("\t");
Serial.print(axraw);Serial.print("\t");
Serial.print(ayraw);Serial.print("\t");
Serial.print(azraw);Serial.print("\t");
Serial.print(gxraw);Serial.print("\t");
Serial.print(gyraw);Serial.print("\t");
Serial.print(gzraw);Serial.print("\t");
Serial.print(arx);Serial.print("\t");
Serial.print(ary);Serial.print("\t");
Serial.print(arz);Serial.print("\t");
Serial.print(gsx);Serial.print("\t");
Serial.print(gsy);Serial.print("\t");
Serial.print(gsz);Serial.print("\t");
Serial.print(grx);Serial.print("\t");
Serial.print(gry);Serial.print("\t");
Serial.print(grz);Serial.print("\t");
Serial.print(rx);Serial.print("\t");
Serial.print(ry);Serial.print("\t");
Serial.println(rz);
delay(10);
}
void read_mpu ()
{
Wire.beginTransmission(MPU);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,14,true); // request a total of 14 registers
axraw=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
ayraw=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
azraw=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
temp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
gxraw=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
gyraw=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
gzraw=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
axraw-=ax_offset;
ayraw-=ay_offset;
azraw-=az_offset;
gxraw-=gx_offset;
gyraw-=gy_offset;
gzraw-=gz_offset;
}
void caliberate ()
{
for(i=1;i<=10;i++)
{
Wire.beginTransmission(MPU);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,14,true); // request a total of 14 registers
axraw=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
ayraw=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
azraw=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
temp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
gxraw=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
gyraw=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
gzraw=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
ax_offset+=axraw;
ay_offset+=ayraw;
az_offset+=azraw;
gx_offset+=gxraw;
gy_offset+=gyraw;
gz_offset+=gzraw;
if(i>1)
{
az_offset/=2;
}
delay(100);
}
ax_offset/=10;
ay_offset/=10;
az_offset-=16384;
gx_offset/=10;
gy_offset/=10;
gz_offset/=10;
}
