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