Arduino Mega and gyroscope erroneous values

Question

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:

#include<Wire.h>

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.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); //power management register 1
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop() {
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);
  GyX=Wire.read()<<8|Wire.read();
  GyY=Wire.read()<<8|Wire.read();
  GyZ=Wire.read()<<8|Wire.read();
  v_pitch=(GyX/131);
  if(v_pitch==-1) {
    v_pitch=0;
  }
  v_roll=(GyY/131);
  if(v_roll==1) {
    v_roll=0;
  }
  v_yaw=GyZ/131;
  a_pitch=(v_pitch*0.046);
  a_roll=(v_roll*0.046);
  a_yaw=(v_yaw*0.045);
  pitch= pitch + a_pitch; roll= roll + a_roll;
  yaw= yaw + a_yaw;
  Serial.print(" | pitch = ");
  Serial.print(pitch);
  Serial.print(" | roll = ");
  Serial.print(roll);
  Serial.print(" | yaw = ");
  Serial.println(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?

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() {
  SER.begin(9600);
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");

  Wire.begin();

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

  // 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(pitch);
    SER.print(" | roll = ");
    SER.print(roll);
    SER.print(" | yaw = ");
    SER.println(yaw);
  }
}

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

  Wire.requestFrom(MPU,6);
  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.