Remove gravity from accelerometer of MPU-6050

I am currently working with MPU-6050. Now I want to minus gravity from my accelerometer's value as we minus the offset.

Here is the code:

#include <Wire.h> #include <MPU6050.h> //read as rool:pitch:yaw:accX:accY:accZ MPU6050 mpu; unsigned long timer = 0; float timeStep = 0.01; float pitch = 0; float roll = 0; float yaw = 0; float accX = 0; float accY = 0; float accZ = 0; float rollRad = 0; float pitchRad = 0; float yawRad = 0; int count = 0; Vector minusGravity(Vector vec) { /* float x = vec.XAxis, y = vec.YAxis, z = vec.ZAxis; vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z * sin(pitchRad) + 0; vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) + 0 + y * cos(rollRad) - z * sin(rollRad); vec.ZAxis -= 0 - x * sin(pitchRad) + z * cos(pitchRad) + y * sin(rollRad) + z * cos(rollRad); */ return vec; } float accP(float value) { int x = (int)(value * 10); return ((float)(x)) / 10; } void setup() { Serial.begin(115200); while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) { } //mpu.calibrateGyro(); mpu.setThreshold(1); } void loop() { timer = millis(); Vector norm = mpu.readNormalizeGyro(); pitch = pitch + norm.YAxis * timeStep; roll = roll + norm.XAxis * timeStep; yaw = yaw + norm.ZAxis * timeStep; Vector normAccel = minusGravity(mpu.readNormalizeAccel()); rollRad = roll * 0.0174533; pitchRad = pitch * 0.0174533; yawRad = yaw * 0.0174533; Serial.print(roll); Serial.print(":"); Serial.print(pitch); Serial.print(":"); Serial.print(yaw); Serial.print(":"); accX = accP(normAccel.XAxis); accY = accP(normAccel.YAxis); accZ = accP(normAccel.ZAxis); Serial.print(accX); Serial.print(":"); Serial.print(accY); Serial.print(":"); Serial.println(accZ); delay((timeStep * 2500) - (millis() - timer)); } 

I understand the problem is here :

vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z * sin(pitchRad) + 0; vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) + 0 + y * cos(rollRad) - z * sin(rollRad); vec.ZAxis -= 0 - x * sin(pitchRad) + z * cos(pitchRad) + y * sin(rollRad) + z * cos(rollRad); 

New code :

 #include <Wire.h> #include <MPU6050.h> //read as rool:pitch:yaw:accX:accY:accZ MPU6050 mpu; unsigned long timer = 0; float timeStep = 0.01; float pitch = 0; float roll = 0; float yaw = 0; float accX = 0; float accY = 0; float accZ = 0; float rollRad = 0; float pitchRad = 0; float yawRad = 0; float gX = 0; float gY = 0; float gZ = -9.8; int count = 0; Vector minusGravity(Vector vec) { float x = gX, y = gY, z = gZ; vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) ; vec.YAxis -= x * sin(yawRad) - y * cos(yawRad) ; x = vec.XAxis; z = vec.ZAxis; vec.XAxis -= x * cos(pitchRad) + z * sin(pitchRad) ; vec.ZAxis -= - x * sin(pitchRad) + z * cos(pitchRad) ; y = vec.YAxis, z = vec.ZAxis; vec.YAxis -= y * cos(rollRad) - z * sin(rollRad); vec.ZAxis -= y * sin(rollRad) + z * cos(rollRad); return vec; } float accP(float value) { int x = (int)(value * 10); return ((float)(x)) / 10; } void setup() { Serial.begin(115200); while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) { } //mpu.calibrateGyro(); mpu.setThreshold(1); Vector normAccel = mpu.readNormalizeAccel(); gX = normAccel.XAxis; gY = normAccel.YAxis; gZ = normAccel.ZAxis; } void loop() { timer = millis(); Vector norm = mpu.readNormalizeGyro(); pitch += norm.YAxis * timeStep; roll += norm.XAxis * timeStep; yaw += norm.ZAxis * timeStep; Vector normAccel = minusGravity(mpu.readNormalizeAccel()); rollRad = roll * 0.0174533; pitchRad = pitch * 0.0174533; yawRad = yaw * 0.0174533; Serial.print(roll ); Serial.print(":"); Serial.print(pitch ); Serial.print(":"); Serial.print(yaw ); Serial.print(":"); accX = accP(normAccel.XAxis); accY = accP(normAccel.YAxis); accZ = accP(normAccel.ZAxis); Serial.print(accX); Serial.print(":"); Serial.print(accY); Serial.print(":"); Serial.println(accZ); delay((timeStep * 2500) - (millis() - timer)); } 

It is little buggy.

Can anyone help me figure out what is my mistake ?