Remove gravity from accelerometer of MPU-6050

Question

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 ?

Answer 1

I may have come to a solution :

Vector minusGravity(Vector vec)
{


 float x = vec.XAxis, y = vec.YAxis, z = 0;
 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;
}

Now this is far better, as after calculating effect of roll,pitch,yaw in every step the x,y,z changes. I missed that part. I was calculating directly, so it was giving me wrong value.

vec.XAxis -= x * cos(yawRad) - y * sin(yawRad)

then, vec.XAxis -= x * cos(pitchRad) + z * sin(pitchRad) doesn't give the same result as the wrong one vec.XAxis -= x * cos(yawRad) - y * sin(yawRad) + x * cos(pitchRad) + z * sin(pitchRad) + 0;