MPU6050 Gyro value going back to initial value irrespective of Angle

Hi,

I'm trying to get the Angle from MPU6050 GyroScope Angular rate, Value is going back to initial value irrespective of position of the Module.

Can anyone tell me whats the problem with the following code ?

following sum up the angular rate to angle calculation

    readGyroX();
    
    currentTime = millis();
    double dt = (currentTime - prevTime)/1000.0;
    prevTime = currentTime;
    rotX = rotX + prevRotX * dt;
    prevRotX = rotX;
 
    Serial.println(rotX);

This is the entire code.

#include <Wire.h>
const byte GYRO_SENSITIVITY_SCALE_FACTOR = 131; //for 200 degrees / sec
const int MPU6050_ADDR = 0b1101000;
const byte PWR_REG_ADDR = 0x6B;
const byte GYRO_CONFIG_REG_ADDR = 0x1B;
const byte GYRO_READ_START_ADDR = 0x43;
int16_t gyroX;
float rotX;
float prevRotX=0;
unsigned long prevTime=0,currentTime;

void setup() {
 Wire.begin();
 Serial.begin(115200);
 configureMPU();
}//end of setup

void loop() {
    
  if(prevTime == 0){
    readGyroX();
    prevTime = millis();  
  }else{
    readGyroX();
    
    currentTime = millis();
    double dt = (currentTime - prevTime)/1000.0;
    prevTime = currentTime;
    rotX = rotX + prevRotX * dt;
    prevRotX = rotX;
 
    Serial.println(rotX);
  }//end of ifElse
  
}//end of loop

void readGyroX(){
   Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_READ_START_ADDR);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR,2,true);  // request a total of 14 registers
  gyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  rotX = gyroX / GYRO_SENSITIVITY_SCALE_FACTOR;
}//end of readGyroX Fcn

void configureMPU(){
  //Power Register
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_REG_ADDR);//Access the power register
  Wire.write(0b00000000);
  Wire.endTransmission();

  //Gyro Config
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_CONFIG_REG_ADDR);
  Wire.write(0b00000000);
  Wire.endTransmission();
}//end of setUpMPU Fcn
  Wire.requestFrom(MPU6050_ADDR,2,true);  // request a total of 14 registers

The comment says 14 registers
The code says 2 registers

I also note that you are reading without checking that any data is available

   Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_READ_START_ADDR);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR,2,true);  // request a total of 14 registers
  gyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)

Hi, Thanks for reply,
I modify the code to read only GYRO_X Value , forgot to change the comment.

I have taken code excerpt from Arduino Playground - MPU-6050

it doesn't have any condition to check for data availability.

The Gyros when directly read will give values near zero.

I calculate the Gyro rotation by using AxRaw, AyRaw, and AzRaw.

The formula I use for gyro rotation is written in Python, I have my MPU-6050 on a Raspberry Pi but here is the Python code I use to figure out current Gyro Angles:

fGet_x_rotation(AxRaw, AyRaw, AzRaw) + X_Rotation_Offset)
         fGet_y_rotation(AxRaw, AyRaw, AzRaw) + Y_Rotation_Offset))


def fDist(a,b):
    return math.sqrt((a*a)+(b*b));
 

def fGet_y_rotation(x,y,z):
    radians = math.atan2(y, fDist(x,z))
    return math.degrees(radians);

def fGet_x_rotation(x,y,z):
    radians = math.atan2(x, fDist(y,z))
    return -math.degrees(radians);

I'm sure the code will convert over to Arduino C or you can find the equivalent already written for the Arduino, but you'll know what you are looking form

Idahowalker:
The Gyros when directly read will give values near zero.

I calculate the Gyro rotation by using AxRaw, AyRaw, and AzRaw.

Hi, thanks for reply.

Correct me if i am wrong , I think those are accelerometer values.

You cannot get rotational position from the gyros alone. They drift with changes in temperature and other factors you cannot measure and correct.

Idaho's code seems to be using acceleration (the force of gravity) to do this without using the gyros at all.

Using both acceleration and the gyros will give you very accurate pitch and roll values. The math is complex and there are constants to tune to your preference. Google should find a dozen good examples.

I think

rotX = rotX + prevRotX * dt;

is wrong, since rotX is Current Angular Velocity and prevRotX is previous Angle (I'm terrible at naming variables).

We need to multiply current Angular velocity by time to get the angle difference and add it to the previous angle so

the correct one (I think )

rotX = prevRotX  + rotX * dt;

But iam getting too much drift. like with in a minute the angle is crossing 360.
is it normal ?

Yes, that drift is normal. You can subtract a simple offset but that will eventually drift too.

rotX = prevRotX  + (rotX-offsetRotX) * dt;

How to calculate drift ?

is following implementation correct ?

void setup(){
   
   int totalVal = 0;
    for (int i =0; i < 1000 ; i++){
       readGyroX();
       totalVal = totalVal + rotX;
}//end of for

  offsetVal = totalVal/1000;

}//end of setup

It depends on the declaration of offsetVal.

MorganS:
It depends on the declaration of offsetVal.

offsetVal will be declared as global variable i.e

float offsetVal;
void setup(){
...
}

or is it something else ?
Can you please explain bit more about it ?

ALWAYS post ALL your code, so that you can avoid so much back and forth.

You are taking the correct approach to estimate the average drift rate, and subtract that from the current gyro angular rate reading. There are three drift rates, one for each axis.

offsetVal should be declared to be the same type as the individual gyro reading, but you cannot add 1000 16 bit integer values without using intermediate variables declared as long (32 bit) integers.

In other words, this is wrong:

int totalVal = 0;

Hi,

Total Code is i'm currently trying is

#include <Wire.h>
const float GYRO_SENSITIVITY_SCALE_FACTOR = 131.0; //for 200 degrees / sec
const int MPU6050_ADDR = 0b1101000;
const byte PWR_REG_ADDR = 0x6B;
const byte GYRO_CONFIG_REG_ADDR = 0x1B;
const byte GYRO_READ_START_ADDR = 0x43;
int16_t gyroX;
float rotX;
float prevRotX=0;
unsigned long prevTime=0,currentTime;

float offsetVal;

void setup() {
 Wire.begin();
 Serial.begin(115200);
 configureMPU();

  float totalVal;
  for (int i=0; i< 20; i++){
    readGyroX();
    Serial.println(rotX);
    totalVal = totalVal + rotX;
  }//end of for
  Serial.println(totalVal);
 offsetVal = totalVal / 100;
 Serial.print("offset Val");
 Serial.println(offsetVal);
 while(1); // to check offset data
}//end of setup

void loop() {
   
  if(prevTime == 0){
    readGyroX();
    prevTime = millis(); 
  }else{
    readGyroX();
   
    currentTime = millis();
    double dt = (currentTime - prevTime)/1000.0;
    prevTime = currentTime;
    rotX =  prevRotX + (rotX-offsetVal)* dt;
    prevRotX = rotX;
    Serial.println(rotX);
  }//end of ifElse
  delay(100);
}//end of loop

void readGyroX(){
   Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_READ_START_ADDR);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR,2,true);  
  gyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  rotX = gyroX / GYRO_SENSITIVITY_SCALE_FACTOR;
}//end of readGyroX Fcn

void configureMPU(){
  //Power Register
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_REG_ADDR);//Access the power register
  Wire.write(0b00000000);
  Wire.endTransmission();

  //Gyro Config
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_CONFIG_REG_ADDR);
  Wire.write(0b00000000);
  Wire.endTransmission();
}//end of setUpMPU Fcn

while(1) is used to check offsetVal.

For some weird reason i'm getting Overflow in Serial Monitor.

-2.83
-2.75
-2.73
-2.81
-2.93
-2.97
-2.85
-2.73
-2.78
-2.85
-3.01
-2.95
-2.82
-2.77
-2.79
-2.95
-2.87
-2.80
-2.85
-2.85
ovf
offset Valovf

Edit :

  for (int i=0; i< 20; i++){
    readGyroX();
    Serial.println(rotX);
    totalVal = totalVal + rotX;
  }//end of for
  Serial.println(totalVal);
 offsetVal = totalVal / 20;

also yields same result

Try initializing totalVal to zero:

float totalVal=0.0;

You should try simpler projects first, so that you learn the language and special features of the Arduino. If you don't, expect the type of frustration you are currently having with a complex project to continue for a long time.

Thanks it works.

Final Code is

#include <Wire.h>
const float GYRO_SENSITIVITY_SCALE_FACTOR = 131.0; //for 200 degrees / sec
const int MPU6050_ADDR = 0b1101000;
const byte PWR_REG_ADDR = 0x6B;
const byte GYRO_CONFIG_REG_ADDR = 0x1B;
const byte GYRO_READ_START_ADDR = 0x43;
int16_t gyroX;
float rotX;
float prevRotX=0;
unsigned long prevTime=0,currentTime;

float offsetVal;
const int noOfSamplesForOffset = 1000;

void setup() {
 Wire.begin();
 Serial.begin(115200);
 configureMPU();

  float totalVal = 0;
  for (int i=0; i< noOfSamplesForOffset; i++){
    readGyroX();
    Serial.println(rotX);
    totalVal = totalVal + (rotX);
  }//end of for
  Serial.println(totalVal);
 offsetVal = totalVal / (noOfSamplesForOffset);
 Serial.print("offset Val");
 Serial.println(offsetVal);
 //while(1);
}//end of setup

void loop() {
   
  if(prevTime == 0){
    readGyroX();
    prevTime = millis(); 
  }else{
    readGyroX();
   
    currentTime = millis();
    double dt = (currentTime - prevTime)/1000.0;
    prevTime = currentTime;
    rotX =  prevRotX + (rotX-offsetVal)* dt;
    prevRotX = rotX;
    Serial.println(rotX);
  }//end of ifElse
  delay(100);
}//end of loop

void readGyroX(){
   Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_READ_START_ADDR);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR,2,true);  
  gyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  rotX = gyroX / GYRO_SENSITIVITY_SCALE_FACTOR;
}//end of readGyroX Fcn

void configureMPU(){
  //Power Register
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_REG_ADDR);//Access the power register
  Wire.write(0b00000000);
  Wire.endTransmission();

  //Gyro Config
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_CONFIG_REG_ADDR);
  Wire.write(0b00000000);
  Wire.endTransmission();
}//end of setUpMPU Fcn

There is Still Drift , But it is better than previous code.
Drift is unavoidable can i assume that is this the best we can do with gyro ? or any improvements we can do with gyro alone ?

Not without spending thousands of dollars on the gyro alone.

MorganS:
Not without spending thousands of dollars on the gyro alone.

Got It :slight_smile:

Any Idea why i get "ovf" when float variable is not initialized (#13)?

In https://www.arduino.cc/en/Reference/VariableDeclaration

int inputVariable1;
int inputVariable2 = 0;     // both are correct

It is clearly mentioned both are correct (I know it is int, i didn't find any info on floats).

Both are indeed correct, but in the second case, the variable is guaranteed to be initialized to a useful value. It is a good idea to use the second form.

jremington:
It is a good idea to use the second form.

If I understand it correctly it's not mandatory to use second form.

So what might be the reason for "ovf" ?

If I understand it correctly it's not mandatory to use second form.

Correct. If you don't use the second form, you may encounter just the sort of difficulties that you are now encountering.

So what might be the reason for "ovf" ?

It is likely that the random bits in the memory location assigned to the uninitialized floating point variable represent an invalid floating point number.