Osman Kamerić
Second
Fork of
MPU6050
by 
Osman Kamerić
Diff: MPU6050.cpp
- Revision:
- 2:e2081d9de7af
- Parent:
- 0:662207e34fba
- Child:
- 3:b21c8c3456be
--- a/MPU6050.cpp Wed May 08 00:34:55 2013 +0000
+++ b/MPU6050.cpp Wed Feb 01 11:09:14 2017 +0000
@@ -41,7 +41,29 @@
*/
#include "MPU6050.h"
-
+int16_t accelData[3],gyroData[3],tempData;
+float accelBias[3] = {0, 0, 0}; // Bias corrections for acc
+float gyroBias[3] = {0, 0, 0}; // Bias corrections for gyro
+enum Ascale
+{
+ AFS_2G=0,
+ AFS_4G,
+ AFS_8G,
+ AFS_16G
+};
+enum Gscale
+{
+ GFS_250DPS=0,
+ GFS_500DPS,
+ GFS_1000DPS,
+ GFS_2000DPS
+};
+int Gscale = GFS_250DPS;
+float aRes;
+float gRes;
+int Ascale = AFS_2G;
+float ax,ay,az;
+float gx,gy,gz;
#define useDebugSerial
//instead of using pgmspace.h
@@ -52,7 +74,7 @@
/** Default constructor, uses default I2C address.
* @see MPU6050_DEFAULT_ADDRESS
*/
-MPU6050::MPU6050() : debugSerial(USBTX, USBRX)
+MPU6050::MPU6050() : debugSerial(p30, p31)
{
devAddr = MPU6050_DEFAULT_ADDRESS;
}
@@ -63,7 +85,7 @@
* @see MPU6050_ADDRESS_AD0_LOW
* @see MPU6050_ADDRESS_AD0_HIGH
*/
-MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX)
+MPU6050::MPU6050(uint8_t address) : debugSerial(p30, p31)
{
devAddr = address;
}
@@ -81,7 +103,37 @@
#ifdef useDebugSerial
debugSerial.printf("MPU6050::initialize start\n");
#endif
-
+ //i2Cdev.frequency(400000); // fast i2c: 400 kHz
+
+ /* Wake up the device */
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x00); // wake up the device by clearing the sleep bit (bit6)
+ //wait_ms(100); // wait 100 ms to stabilize
+
+ /* Get stable time source */
+ // PLL with X axis gyroscope reference is used to improve stability
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x01);
+
+ /* Configure Gyroscope and Accelerometer */
+ // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz
+ // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate)
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, 0x03);
+
+ /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */
+ // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above)
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x04);
+
+ /* Accelerometer configuration */
+ //uint8_t temp;
+ //i2Cdev.readByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG,&temp);
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5]
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, temp & ~0x18); // Clear AFS bits [4:3]
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, temp | Ascale<<3); // Set full scale range
+
+ /* Gyroscope configuration */
+ //i2Cdev.readByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, &temp);
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, temp & ~0xE0); // Clear self-test bits [7:5]
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, temp & ~0x18); // Clear FS bits [4:3]
+ //i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, temp | Gscale<<3); // Set full scale range
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
setFullScaleGyroRange(MPU6050_GYRO_FS_250);
setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
@@ -108,6 +160,169 @@
return deviceId == 0x34;
}
+void MPU6050::calibrate(float* dest1, float* dest2){
+ uint8_t data[12]; // data array to hold acc and gyro x,y,z data
+ uint16_t fifo_count, packet_count, count;
+ int32_t accel_bias[3] = {0,0,0};
+ int32_t gyro_bias[3] = {0,0,0};
+ float aRes = 2.0/32768.0;
+ float gRes = 250.0/32768.0;
+ uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g
+ //uint16_t gyrosensitivity = 131; // = 1/gRes = 131 LSB/dps
+
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x80); // set bit7 to reset the device
+ wait_ms(100);
+
+ /*Get stable time source */
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x01); // PLL with X axis gyroscope reference is used to improve stability
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00); // Disable accel only low power mode
+ wait(0.2);
+
+ /* Configure device for bias calculation */
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00); // Disable all interrupts
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FIFO_EN, 0x00); // Disable FIFO
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0x00); // Turn on internal clock source
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00); // Disable I2C master
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_USER_CTRL, 0x00); // Disable FIFO and I2C master modes
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_USER_CTRL, 0x04); // Reset FIFO
+ wait(0.015);
+
+
+ /* Configure accel and gyro for bias calculation */
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, 0x01); // Set low-pass filter to 188 Hz
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
+
+ /* Configure FIFO to capture accelerometer and gyro data for bias calculation */
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_USER_CTRL, 0x40); // Enable FIFO
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FIFO_EN, 0x78); // Enable accelerometer and gyro for FIFO (max size 1024 bytes in MPU-6050)
+ wait(0.08); // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds.
+ // accX: 2 byte, accY: 2 byte, accZ: 2 byte. gyroX: 2 byte, gyroY: 2 byte, gyroZ: 2 byte. 12*80=960 byte < 1024 byte
+
+
+ /* At end of sample accumulation, turn off FIFO sensor read */
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FIFO_EN, 0x00); // Disable FIFO
+ i2Cdev.readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FIFO_COUNTH, 2, &data[0]); // Read FIFO sample count
+ fifo_count = ((uint16_t)data[0] << 8) | data[1];
+ packet_count = fifo_count/12; // The number of sets of full acc and gyro data for averaging. packet_count = 80 in this case
+
+ for(count=0; count<packet_count; count++)
+ {
+ int16_t accel_temp[3]={0,0,0};
+ int16_t gyro_temp[3]={0,0,0};
+ i2Cdev.readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_FIFO_R_W, 12, &data[0]); // read data for averaging
+
+ /* Form signed 16-bit integer for each sample in FIFO */
+ accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ;
+ accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
+ accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
+ gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
+ gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
+ gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
+
+ /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */
+ accel_bias[0] += (int32_t) accel_temp[0];
+ accel_bias[1] += (int32_t) accel_temp[1];
+ accel_bias[2] += (int32_t) accel_temp[2];
+ gyro_bias[0] += (int32_t) gyro_temp[0];
+ gyro_bias[1] += (int32_t) gyro_temp[1];
+ gyro_bias[2] += (int32_t) gyro_temp[2];
+ }
+ /* Normalize sums to get average count biases */
+ accel_bias[0] /= (int32_t) packet_count;
+ accel_bias[1] /= (int32_t) packet_count;
+ accel_bias[2] /= (int32_t) packet_count;
+ gyro_bias[0] /= (int32_t) packet_count;
+ gyro_bias[1] /= (int32_t) packet_count;
+ gyro_bias[2] /= (int32_t) packet_count;
+
+ /* Remove gravity from the z-axis accelerometer bias calculation */
+ if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;}
+ else {accel_bias[2] += (int32_t) accelsensitivity;}
+
+ /* Output scaled accelerometer biases for manual subtraction in the main program */
+ dest1[0] = accel_bias[0]*aRes;
+ dest1[1] = accel_bias[1]*aRes;
+ dest1[2] = accel_bias[2]*aRes;
+
+ /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */
+ data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
+ data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
+ data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
+ data[3] = (-gyro_bias[1]/4) & 0xFF;
+ data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
+ data[5] = (-gyro_bias[2]/4) & 0xFF;
+
+ /* Push gyro biases to hardware registers */
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_XG_OFFS_USRH, data[0]);
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_XG_OFFS_USRL, data[1]);
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_YG_OFFS_USRH, data[2]);
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_YG_OFFS_USRL, data[3]);
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ZG_OFFS_USRH, data[4]);
+ i2Cdev.writeByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ZG_OFFS_USRL, data[5]);
+
+ /* Construct gyro bias in deg/s for later manual subtraction */
+ dest2[0] = gyro_bias[0]*gRes;
+ dest2[1] = gyro_bias[1]*gRes;
+ dest2[2] = gyro_bias[2]*gRes;
+ }
+void MPU6050::readAccelData(int16_t* dest)
+{
+ uint8_t rawData[6]; // x,y,z acc data
+ i2Cdev.readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 6, &rawData[0]); // read six raw data registers sequentially and write them into data array
+
+ /* Turn the MSB LSB into signed 16-bit value */
+ dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // ACCEL_XOUT
+ dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // ACCEL_YOUT
+ dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // ACCEL_ZOUT
+}
+void MPU6050::readGyroData(int16_t* dest)
+{
+ uint8_t rawData[6]; // x,y,z gyro data
+ i2Cdev.readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_XOUT_H, 6, &rawData[0]); // read the six raw data registers sequentially and write them into data array
+
+ /* Turn the MSB LSB into signed 16-bit value */
+ dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // GYRO_XOUT
+ dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // GYRO_YOUT
+ dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // GYRO_ZOUT
+}
+void MPU6050::getGres()
+{
+ switch(Gscale)
+ {
+ case GFS_250DPS:
+ gRes = 250.0/32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0/32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0/32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0/32768.0;
+ break;
+ }
+}
+void MPU6050::getAres()
+{
+ switch(Ascale)
+ {
+ case AFS_2G:
+ aRes = 2.0/32768.0;
+ break;
+ case AFS_4G:
+ aRes = 4.0/32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0/32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0/32768.0;
+ break;
+ }
+}
// AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)
/** Get the auxiliary I2C supply voltage level.
@@ -532,7 +747,7 @@
i2Cdev.writeByte(devAddr, MPU6050_RA_MOT_THR, threshold);
}
-// MOT_DUR register
+// register
/** Get motion detection event duration threshold.
* This register configures the duration counter threshold for Motion interrupt
@@ -3194,7 +3409,12 @@
if (prefetchEnabled) bank |= 0x40;
i2Cdev.writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
}
-
+//Read certain register
+uint8_t MPU6050::readThisByte(uint8_t address)
+{
+ i2Cdev.readByte(devAddr, address, buffer);
+ return buffer[0];
+}
// MEM_START_ADDR register
void MPU6050::setMemoryStartAddress(uint8_t address)