Jump to content
I2Cdevlib Forums

Recommended Posts

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

 

Share this post


Link to post
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

Loading...

×
×
  • Create New...