MPU-6050 Gyroscope, loosing pitch and etc. during movments back to start

Hi, I would like to ask you, if you have any advice or solution how to solve issue that I am using MPU-6050 and example project from Arduino, which should show actual pitch/roll/yaw. (using arduino nano and 115200 baud comunication)

Problem is:
On the start of code is calibration, after that I get every few milliseconds value from sensor. I start with sensor flat on table. Calibration set it up for 0. When I start moving it changing pitch/roll/yaw perfectly. But when I move back on start position I don’t have anymore 0/0/0.
It is because I dont move so smooth and dont make the same moves to back position. (smooth, and during the same time).


  // Calculate Pitch, Roll and Yaw
  pitch = pitch + norm.YAxis * timeStep;
  roll = roll + norm.XAxis * timeStep;
  yaw = yaw + norm.ZAxis * timeStep;

Full code:

    MPU6050 Triple Axis Gyroscope & Accelerometer. Pitch & Roll & Yaw Gyroscope Example.
    Read more:
    (c) 2014 by Korneliusz Jarzebski
// WORKING one! but loosing calibration and after few turns it change zero position.

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// Timers
unsigned long timer = 0;
float timeStep = 0.01;

// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;

void setup() 

  // Initialize MPU6050
  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.

  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.

void loop()
  timer = millis();

  // Read normalized values
  Vector norm = mpu.readNormalizeGyro();

  // Calculate Pitch, Roll and Yaw
  pitch = pitch + norm.YAxis * timeStep;
  roll = roll + norm.XAxis * timeStep;
  yaw = yaw + norm.ZAxis * timeStep;

  // Output raw
  Serial.print(" Pitch = ");
  Serial.print(" Roll = ");
  Serial.print(" Yaw = ");

  // Wait to full timeStep period
  delay((timeStep*1000) - (millis() - timer));

Another point is that I don’t need to check every 5-10ms values from sensor. I guess that where I am loosing data when I check it less often and “calculate” that way of move.
But for me and my project I would like to get them max like 0.1s - 5s. I guess that I can still get them every 5ms and just wait like 100x loops and start using that data, but anyway… Anyone have some idea how to stop loosing after few moves the “calibrated” point from start?

I want to use using this sensor on my bike to know about pitch and acceleration and saving these data on sd card. but I cannot use that when I need to ask for data (to have them correctly) every few ms :frowning:

So my idea is every half of second - second to ask for data from sensor, calculate pitch and all angles, and save them. But when I start with 0 coordinates, move on side and move back on the same spot, I am not on 0 back but on some different values :frowning:


You have to calibrate the gyro, in order to remove the offsets. I don't know what that library does, but most people calculate the average of a few hundred gyro values, collected at startup with the sensor held fixed, and subtract the averages from subsequent readings.

But even then the angles will drift, because of inaccurate integration, gyro noise and the temperature dependence of the offsets.

Integration accuracy is improved by taking more frequent readings (smaller time steps).

Since you have an accelerometer, you should use that to calculate the Pitch and roll. You can still fuse the gyroscope with the accelerometer to get good calculations.