0

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 ?

  • 1
    I understand the problem is here .... what is the problem? .... what do you expect to see? .... what do you see instead? – jsotola Apr 5 at 21:02
  • I just want to see acceleration due to movement, not due to gravity. I'm trying to counter gravity portion. – Maifee Ul Asad Apr 6 at 3:04
  • When I put it on a surface or holding it, there should be no acceleration. – Maifee Ul Asad Apr 6 at 3:07
  • @jsotola I may have found a solution, added an answer , can you verify it, if it is correct or not ? – Maifee Ul Asad Apr 6 at 6:58
  • A general comment: you are trying to compute pitch, roll, yaw by integrating the readings from the gyroscope and and to account for gravity. In my experience this is unreliable beyond a few seconds and you would need more sophisticated methods if you want to track orientation. – Christian Lindig Apr 6 at 9:07
0

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;

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.