Gyroscope and Accelerometer (GY-521/MPU6050) - issue with accuracy

Hey,

I’m using the previously mentioned gyro/accel as part of a glider project to try and achieve accurate pitches and rolls. I am getting the data from the accelerometer fine, however for the gyroscope it’s not very accurate even for short time frames which I thought was where it is very accurate. I would, if this worked properly, be using the complementary filter to have a more accurate roll angle value

I can have it so that the gyro is reading zero degrees of roll, however when I begin rotating it and set it at 90, it will display about 65/70. Putting it back flat to its zero degrees position can display a value of -12 degrees or so, however it usually goes back down to zero after a few seconds, but this problem persists.

I’m at a loss at what to do, any ideas on getting this gyro to be more accurate?? The code I’ve found here and there on the internet and added to is shown below if it helps

//need to readd servo initialisation
//is the tilt angle in the correct direction (roll angle?)

#include <Wire.h>
#include <Servo.h>

Servo servoLeft;          // Define left and right wing servos
Servo servoRight;

long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;

float error, previous_error, integral, derivative, kp, ki, kd, sp, output; //PID controller vars
float tiltAngle, angleY; //accel roll angle, accel+gyro roll angle

long gyroX, gyroY, gyroZ, gyroRotY; //gyro raw data, gyro roll angle
float rotX, rotY, rotZ, timer; //angular velocities, timer vars

int isFlying, servo1Default, servo2Default; //for detecting launch

void setup() {
  Serial.begin(9600); //allows values to be read by computer
  Wire.begin();
  setupMPU(); //applies settings and initialises accel/gyro

  isFlying = 1; //determines if the glider has been launched
  integral = 0;
  previous_error = 0;

  servoLeft.attach(10);  // Set left servo to digital pin 10
  servoRight.attach(9);  // Set right servo to digital pin 9
  
  servo1Default = 0; //this will be digital pin 5, and is the set angle of the RH servo's default position
  servo2Default = 0; //this will be digital pin 6, and is the set angle of the LH servo's default position

  //set values for constants
  kp = 1;
  ki = 1;
  kd = 0.05;
  sp = 0;

  timer = micros();
  
}

void loop() {
  recordAccelRegisters();  //check accelerometer readings
  processAccelData(); //process the data
  updateTilt(); //get roll angle with accelerometer data

  double elapsedTimeInSeconds = ((double)(micros()-timer)/1000000);
  recordGyroRegisters();
  processGyroData();
    gyroRotY += rotY * elapsedTimeInSeconds; //angle of roll adding with an angular velocity and its corresponding time
    Serial.println("elapsed time is ");
    Serial.print(elapsedTimeInSeconds);
  timer = micros();
  Serial.print("rotation of the system is  ");
  Serial.println(angleY);
  Serial.print("gyro rotation is ");
  Serial.println(gyroRotY);
  
  while (isFlying == 1) { //wait for launch
    recordAccelRegisters();
    Serial.print("No launch detected, gForceY is:");
    Serial.println(gForceY);
    Serial.print(isFlying);
    if (gForceY > 0.5) {
      isFlying = 2; //breaks out of launch wait loop upon g force detection
      Serial.print("Glider has been launched");
      break;
    }
  }

  //PID controller code to adjust servos for elevons
  error = sp - tiltAngle; //how far out is the tilt angle from desired
  integral = integral + error*0.01;
  derivative = (error - previous_error)/0.01;
  output = kp*error + ki*integral + kd*derivative;
  previous_error = error;
  servoLeft.write(output);
  //Serial.print("PID controller has set left servo output to be: ");
  //Serial.println(output); 
  servoRight.write(-output);
  //Serial.print("PID controller has set right servo output to be: ");
  //Serial.println(output); 
  
  
  if ((rotX > 10) || (rotX <-2)){ // if the angle of attack is bad whilst stable, fix
    pitchFix();
  }


  //printData();
 
}

void pitchFix(){
  //write code to add extra deflection to elevons until angle of attack is at the perfect amount
  Serial.print("the fix for pitch needs to codedddd");
}

void updateTilt(){
  
  tiltAngle = (atan2(gForceX, gForceZ)*(180/3.14159)); //calculates the tilt angle using accelerometer values
  Serial.print("tilt angle test w no moving average: ");
  Serial.println(tiltAngle);
}


void setupMPU(){
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();  
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4) 
  Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s 
  Wire.endTransmission(); 
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5) 
  Wire.write(0x00000000); //Setting the accel to +/- 2g
  Wire.endTransmission(); 
}

void recordAccelRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processAccelData();
}

void processAccelData(){
  gForceX = accelX / 16384.0;
  gForceY = accelY / 16384.0; 
  gForceZ = accelZ / 16384.0;
}

void recordGyroRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processGyroData();
}

void processGyroData() {
  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0; 
  rotZ = gyroZ / 131.0;
}

void printData() {
  Serial.print("Gyro (deg)");
  Serial.print(" X=");
  Serial.print(rotX);
  Serial.print(" Y=");
  Serial.print(rotY);
  Serial.print(" Z=");
  Serial.print(rotZ);
  Serial.print(" Accel (g)");
  Serial.print(" X=");
  Serial.print(gForceX);
  Serial.print(" Y=");
  Serial.print(gForceY);
  Serial.print(" Z=");
  Serial.println(gForceZ);
}

You need to (1) subtract off gyro drift (determined by averaging several hundred readings at startup while the sensor is at rest) and (2) numerically integrate the gyro readings correctly.

I doubt that your time intervals are correct for the numerical integration.

I suggest to use the excellent, state of the art, open source RTIMULib AHRS library.