MPU-3050 GYRO Drift - How much is too much?

[Tim]

Thank you for being here...

I am working towards (or away from) a balancing robot...

 

I hope an MPU-3050 gyro is an ancestor of the MPU-6050...

I am using I2C and getting data from the gyro for display using arduino uno.

I am also attempting to use the OFFSET registers for automatic BIAS removal through averaging.

It appears to be at least close, with values floating around 0-10 X, -20 Y, -1 Z.

I guess I should ask if that is close enough, or back to averaging better or what might I be doing wrong with my offsets...

 

Anyway, I selected +/- 2,000 deg/sec for full scale

// MPU-3050 FULL SCALE range FS_SEL = 0 +/-250  deg/sec Sensitivity Scale Factor LSB deg/sec 131
// FULL SCALE range FS_SEL = 1 +/-500  deg/sec Sensitivity Scale Factor LSB deg/sec 65.5
// FULL SCALE range FS_SEL = 2 +/-100  deg/sec Sensitivity Scale Factor LSB deg/sec 32.8
// FULL SCALE range FS_SEL = 3 +/-2000 deg/sec Sensitivity Scale Factor LSB deg/sec 16.4
#define GYROSCOPE_SENSITIVITY (1 / 16.4)

 Initialized offsets, started the gyro measurement mode... and did samples for averaging....

  // Default at power-up:
  //    Gyro at 250 degrees second
  //    Acceleration at 2g
  //    Clock source at internal 8MHz
  //    The device is in sleep mode.
  //
  // FULL SCALE range FS_SEL = 0 +/-250  deg/sec Sensitivity Scale Factor LSB deg/sec 131
  // FULL SCALE range FS_SEL = 1 +/-500  deg/sec Sensitivity Scale Factor LSB deg/sec 65.5
  // FULL SCALE range FS_SEL = 2 +/-100  deg/sec Sensitivity Scale Factor LSB deg/sec 32.8
  // FULL SCALE range FS_SEL = 3 +/-2000 deg/sec Sensitivity Scale Factor LSB deg/sec 16.4
  //writeTo(gyro_sensorAddr,0x16,0x02); //Set gyro to +/-250deg/sec and 98Hz low pass filter
  writeTo(gyro_sensorAddr,0x16,0x1A); //Set gyro to +/-2000deg/sec and 98Hz low pass filter
  writeTo(gyro_sensorAddr,0x15,0x09); //Set gyro to a 100Hz sample rate
  
  // Initialize the bias offsets in the GYRO OFFSET registers... These will be automatically subtracted
  // from the gyro sensor values before they go into the sensor registers.
  //#define MPU3050_X_OFFS_H      0x0C
  //#define MPU3050_X_OFFS_L      0x0D
  //#define MPU3050_Y_OFFS_H      0x0E
  //#define MPU3050_Y_OFFS_L      0x0F
  //#define MPU3050_Z_OFFS_H      0x10
  //#define MPU3050_Z_OFFS_L      0x11
  writeTo(gyro_sensorAddr, MPU3050_X_OFFS_H, 0); // 0x0C - Reset high order byte of gyro X offset
  writeTo(gyro_sensorAddr, MPU3050_X_OFFS_L, 0); // 0x0D - Reset low  order byte of gyro X offset
  writeTo(gyro_sensorAddr, MPU3050_Y_OFFS_H, 0); // 0x0E - Reset high order byte of gyro Y offset
  writeTo(gyro_sensorAddr, MPU3050_Y_OFFS_L, 0); // 0x0F - Reset low  order byte of gyro Y offset
  writeTo(gyro_sensorAddr, MPU3050_Z_OFFS_H, 0); // 0x10 - Reset high order byte of gyro Z offset
  writeTo(gyro_sensorAddr, MPU3050_Z_OFFS_L, 0); // 0x11 - Reset low  order byte of gyro Z offset

  //#define MPU3050_USER_CTRL     0x3D
  //#define MPU3050_PWR_MGM       0x3E
  // RESET conditions and take out of sleep mode (which is default at power up)
  writeTo(gyro_sensorAddr, MPU3050_USER_CTRL, 0x03); //Reset FIFO function; Reset gyro analog and digital functions
  delay(100); //wait for gyro
  writeTo(gyro_sensorAddr, MPU3050_PWR_MGM, 0x00); //Bring Gyro out of default power up SLEEP mode 

  Serial.println("\nShowing a sample of the gyro X axis collected for averaging the BIAS OFFSETS...");
  for (i = 0; i < 10; i += 1) 
  {
    // Read the values in a stable position over time into uint8_t type
    // The get routines will store them into uint16_t array structures
    getGyroscopeReadings(gyroResult);
    getAccelerometerReadings(accelResult);
    
    // Now we have uint16_t types waiting
    // Put them into a uint16_t
    gx = gyroResult[0]; 
    gy = gyroResult[1]; 
    gz = gyroResult[2]; 
    // Take the 2's complement of data values and place them into int16_t types
    real_gx = ~(gx - 1); 
    real_gy = ~(gy - 1); 
    real_gz = ~(gz - 1); 
    // Accumulate the number of sensor readings for averaging
    // GYRO READINGS
    totalGyroXValues += real_gx;
    totalGyroYValues += real_gy;
    totalGyroZValues += real_gz;
    

    // SAMPLE PRINTOUT OF GYRO READING ONLY
    Serial.println(real_gx);

   delay(50);
  }
  
  // Take the average for each axis and place it back into an unsigned int
  biasGyroX  = totalGyroXValues   / 10;
  biasGyroY  = totalGyroYValues   / 10;
  biasGyroZ  = totalGyroZValues   / 10;
  Serial.print("\n"); Serial.print("\n");
  Serial.println("Showing the totals collected for averaging the BIAS OFFSETS...");
  Serial.print("totalGyroXValues\t");  Serial.print(totalGyroXValues);  Serial.print("\n");
  Serial.print("totalGyroYValues\t");  Serial.print(totalGyroYValues);  Serial.print("\n");
  Serial.print("totalGyroZValues\t");  Serial.print(totalGyroZValues);  Serial.print("\n");
  Serial.print("\n"); Serial.print("\n");
  Serial.println("And now the BIAS OFFSETS...");
  Serial.print("biasGyroX\t");  Serial.print(biasGyroX);  Serial.print("\n");
  Serial.print("biasGyroY\t");  Serial.print(biasGyroY);  Serial.print("\n");
  Serial.print("biasGyroZ\t");  Serial.print(biasGyroZ);  Serial.print("\n");
  //=====================================================================================
   /* write the value to the corresponding offset register */
  //#define MPU3050_X_OFFS_H      0x0C
  //#define MPU3050_X_OFFS_L      0x0D
  //#define MPU3050_Y_OFFS_H      0x0E
  //#define MPU3050_Y_OFFS_L      0x0F
  //#define MPU3050_Z_OFFS_H      0x10
  //#define MPU3050_Z_OFFS_L      0x11
  uint8_t HI, LO;
  HI = biasGyroX >> 8;
  LO = biasGyroX & 0xFF;
  Serial.print("HI: "); Serial.print(HI);Serial.print("\tLO: "); Serial.println(LO);
  writeTo(gyro_sensorAddr, MPU3050_X_OFFS_H, HI); //Set high order byte of gyro offset
  writeTo(gyro_sensorAddr, MPU3050_X_OFFS_L, LO); //Set low order byte of gyro offset

  HI = biasGyroY >> 8;
  LO= biasGyroY & 0xFF;
  Serial.print("HI: "); Serial.print(HI);Serial.print("\tLO: "); Serial.println(LO);
  writeTo(gyro_sensorAddr, MPU3050_Y_OFFS_H, HI); //Set high order byte of gyro offset
  writeTo(gyro_sensorAddr, MPU3050_Y_OFFS_L, LO); //Set low order byte of gyro offset

  HI = biasGyroZ >> 8;
  LO = biasGyroZ & 0xFF;
  Serial.print("HI: "); Serial.print(HI);Serial.print("\tLO: "); Serial.println(LO);
  writeTo(gyro_sensorAddr, MPU3050_Z_OFFS_H, HI); //Set high order byte of gyro offset
  writeTo(gyro_sensorAddr, MPU3050_Z_OFFS_L, LO); //Set low order byte of gyro offset

  delay(100); //wait for gyro

  Serial.print("Pitch gyro\tPitch accel\t");
  Serial.print("Roll gyro\tRoll accel\n");

I get reasonable values at the start but the "drift" is 1 degree per second! What is acceptable drift and the technique used with an accelerometer (in my case an ADL-345)?

Within the Loop() function I am doing the following for data collection and eventual fusion...

  // Calculate the pitch in degrees as measured by the gyros, angle = angle + gyro reading * time step.
  // The 131 represents (in this case) 131, 65.5, 32.8, 16.4...
  // Since we have chosen 2000 deg/sec as our Full Scale Range (FS_SEL = 0,1,2,3), 
  // per the data sheet, 16.4 would be the scale factor we use for conversion
  // And that shows up as LSB in deg/sec... 
  // And so... (I think) For each bump in the value of the returned word by 16.4 LSB bits, 
  // we can add a deg/sec to our final conversion
  // And further so... the following raw data returned to us can be converted to degrees/second?
  // GYROSCOPE_SENSITIVITY is 1/(Either 131, 65.5, 32.8, or 16.4...)
  gxRate = (d_gx * GYROSCOPE_SENSITIVITY); 
  // Since we are keeping track of and controlling the amount of time we spend collecting our data,
  // we can continually add the rate in degrees per second multiplied by our desired interval.
  // The assumption is, of course, that the rate throughout is continously of the degrees per second 
  // calculated in our short length of sample time. 
  // Obviously any deviation of that assumption in truth can lead to error build-up
  // But hey... what is a GYRO HEAD supposed to do?... 
  gxAngle += gxRate * timeStep;

  // Convert the accelerometer value to DEG/SEC. 
  // With 10 bits measuring over a +/-Xg range we can find how to convert by using the equation:
  // Gs = Measurement Value * (G-range/(2^10)) or Gs = Measurement Value * (2/1024)=0.00195
  // Gs = Measurement Value * (G-range/(2^10)) or Gs = Measurement Value * (4/1024)=0.0039
  // Gs = Measurement Value * (G-range/(2^10)) or Gs = Measurement Value * (8/1024)=0.0078
  // pitch = atan2(accx,sqrt(accy^2+accz^2)) //pitch = atan2(accYval, accZval)+PI
  // roll  = atan2(accy,sqrt(accx^2+accz^2)) //roll  = atan2(accXval, accZval)+PI 
  // Calculate the pitch in degrees as measured by the accelerometers. 
  // Note that 8g into 11 bits gives 256 bits/g (2048 / 8g)
  // Note that 4g into 10 bits gives 256 bits/g (1024 / 4g)
  // Gee... either or...
  //pitchAccel = atan2(real_ay * 0.0038, real_az * 0.0038) * 180.0 / (2*PI);
   pitchAccel = (atan2(-real_ay * 0.0038, real_az * 0.0038)+PI) * RAD_TO_DEG;


//==============
  compositeAngle = (0.98 *(gxAngle)+(0.02 *(pitchAccel))); 

How can I calm down my rapid degradation of angle for pitch gyro? In the last 10 minutes I am at 2000 degrees... (if you catch my drift)....

Tim