I have Arduino mega2560 and MPU6050.

I connected to Vcc pin of the gyro to +5V on the mega, the SDA to SDA pin, SDL to SDL pin, AD0 and GND to 2 GND pins.

I used this code:


const int MPU=0x68; //I2C address of MPU
int GyX,GyY,GyZ;
float pitch=0;
float roll=0;
float yaw=0;

float v_pitch;
float v_roll;
float v_yaw;
float a_pitch;
float a_roll;
float a_yaw;

void setup(){
  Wire.write(0x6B); //power management register 1

void loop() {
  if(v_pitch==-1) {
  if(v_roll==1) {
  pitch= pitch + a_pitch; roll= roll + a_roll;
  yaw= yaw + a_yaw;
  Serial.print(" | pitch = ");
  Serial.print(" | roll = ");
  Serial.print(" | yaw = ");

But the values I am reading are not angles at all, plus when I move the X axes for 45 degrees, the value is not like that at all.


I don't want to use the pre made library as I want to understand how to read from Gyroscope. Any ideas?

  • What is the 0.046? The mpu6050 uses 3.3v.
    – Jot
    Sep 4, 2018 at 19:11
  • I dont know why its used by author. He said that eaxh 46ms the loop is reading a value.
    – alim1990
    Sep 4, 2018 at 19:15
  • @Jot no It is better to use 5v for more accurate results as I read on arduino forums.
    – alim1990
    Sep 4, 2018 at 19:16
  • Thanks for the explanation about the 46ms of the loop. The loop time can be measured with millis. The 5v depends on the sensor module that you have. Which arduino forums told you to destroy the sensor with 5v? Please add all the extra information to your question.
    – Jot
    Sep 4, 2018 at 19:37
  • Please refer to this link addicore.com/GY-521-MPU6050-p/170.htm where it says the power supply is from 4.3 to 9V.
    – alim1990
    Sep 5, 2018 at 6:21

1 Answer 1


It is possible to use your sketch and get angle information. It is not very accurate and it drifts.

I started with your sketch, and improved it until I had the angle. The Serial.println functions take time and they are printed too often. To measure the interval between the samples, I used millis.

This test is done with:

  • arduino M0 board (with 3.3v i2c bus)
  • gy-521 module with onboard voltage regulator, but no level shifter
  • vcc of the gy-521 module connected to 5v (because of the voltage regulator)

The arduino M0 is faster than the arduino mega or uno.

// test with arduino M0 board
#include <Wire.h>

#define SER SerialUSB    // 'Serial' for arduino Uno, 'SerialUSB' for Leonardo and Arduino M0

const int MPU = 0x68;   // I2C address of MPU

int16_t x_offset = 0;
int16_t y_offset = 0;
int16_t z_offset = 0;

float pitch = 0.0;
float roll  = 0.0;
float yaw   = 0.0;

unsigned long previousMillisSample;
unsigned long previousMillisOutput;
const unsigned long intervalOutput = 500;  // half a second update to serial monitor

void setup() {
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");


  Wire.write(0x6B); //power management register 1

  // get the values of the gyro and use them as offset
  GetXYZ(x_offset, y_offset, z_offset);

void loop() {

  int16_t GyX;
  int16_t GyY;
  int16_t GyZ;

  GetXYZ(GyX, GyY, GyZ);

  unsigned long currentMillisSample = millis();  // time in millis when this sample is taken

  // remove offset
  GyX -= x_offset;
  GyY -= y_offset;
  GyZ -= z_offset;

  float v_pitch = float(GyX) / 131.0;    // convert to float and calculate degrees
  float v_roll  = float(GyY) / 131.0;
  float v_yaw   = float(GyZ) / 131.0;

  // calculate time in milliseconds between previous and current sample
  unsigned long t = currentMillisSample - previousMillisSample;

  // time in seconds as a float
  float float_t = float(t) / 1000.0;

  // adjust the number of degrees during the time
  float a_pitch = v_pitch * float_t;
  float a_roll  = v_roll  * float_t;
  float a_yaw   = v_yaw   * float_t;

  // remember the time in millis when the sample was taken for the next time
  previousMillisSample = currentMillisSample;

  pitch += a_pitch; 
  roll  += a_roll;
  yaw   += a_yaw;

  // Once in a while, print the output
  if(millis() - previousMillisOutput >= intervalOutput) {
    previousMillisOutput = millis();

    SER.print("pitch = ");
    SER.print(" | roll = ");
    SER.print(" | yaw = ");

void GetXYZ( int16_t &x, int16_t &y, int16_t &z) {
  // Get the gyro values
  Wire.write(0x43);     // first register of gyro values

  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();

Can you try this sketch? It drifts between 10 and 60 degrees per 5 minutes. That is a lot. I think the sensor is better than that, so that means the sketch is far from perfect.

  • Once I get home (in 2 hours) I will try it. Your sketch drift 60degrees per 5min or mine ?
    – alim1990
    Sep 8, 2018 at 6:14
  • The changes I made was only to make it work (remove offset; less println; measure time between samples with millis; do float calculations with float variables). My sketch drifts up to 60 degrees per 5 minutes with a faster arduino m0 board.It drifts a lot more when I wildly move the sensor. For the arduino uno or mega, you have to change the define to: #define SER Serial
    – Jot
    Sep 8, 2018 at 6:58
  • Can we reduce the drifts ? It would make the drone unstable even with correct PID values
    – alim1990
    Sep 8, 2018 at 12:10
  • 1
    @droidnation , this was only for fun and to learn about arduino. Now it is time to buy a mpu-9250 and use the ahrs filter: learn.sparkfun.com/tutorials/mpu-9250-hookup-guide For a drone you need sensor fusion, that is a filter that combines the accelerometer and the gyro and a magnetometer. The kalman filter was used previously but today the ahrs is used.
    – Jot
    Sep 8, 2018 at 12:59
  • 1
    @droidnation Maybe a little. The MPU9250 has less noise and a magnetometer. Less noise makes the results of the ahrs filter a lot better.
    – Jot
    Sep 8, 2018 at 16:36

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.