MPU6050.cpp

00001 /*   MPU6050 Library   
00002 *    
00003 *    @author: Baser Kandehir 
00004 *    @date: July 16, 2015
00005 *    @license: MIT license
00006 *     
00007 *   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
00008 *
00009 *   Permission is hereby granted, free of charge, to any person obtaining a copy
00010 *   of this software and associated documentation files (the "Software"), to deal
00011 *   in the Software without restriction, including without limitation the rights
00012 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00013 *   copies of the Software, and to permit persons to whom the Software is
00014 *   furnished to do so, subject to the following conditions:
00015 *
00016 *   The above copyright notice and this permission notice shall be included in
00017 *   all copies or substantial portions of the Software.
00018 *
00019 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00020 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00021 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00022 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00023 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00024 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00025 *   THE SOFTWARE.
00026 *
00027 */
00028 
00029 // Most of the code is adapted from Kris Winer's MPU6050 library
00030 
00031 #include "MPU6050.h"
00032 
00033 /* For LPC1768 board */
00034 //I2C i2c(p9,p10);         // setup i2c (SDA,SCL)  
00035 
00036 /* For NUCLEO-F411RE board */
00037 static I2C i2c(PTE25,PTE24);         // setup i2c (SDA,SCL)
00038 
00039 /* Set initial input parameters */
00040 
00041 // Acc Full Scale Range  +-2G 4G 8G 16G  
00042 enum Ascale
00043 {
00044     AFS_2G=0,  
00045     AFS_4G,
00046     AFS_8G,
00047     AFS_16G
00048 };
00049 
00050 // Gyro Full Scale Range +-250 500 1000 2000 Degrees per second
00051 enum Gscale
00052 {
00053     GFS_250DPS=0,   
00054     GFS_500DPS,
00055     GFS_1000DPS,
00056     GFS_2000DPS
00057 };
00058 
00059 // Sensor datas
00060 float ax,ay,az;
00061 float gx,gy,gz;
00062 float axx;
00063 int16_t accelData[3],gyroData[3],tempData;
00064 float accelBias[3] = {0, 0, 0};  // Bias corrections for acc
00065 float gyroBias[3] = {0, 0, 0};   // Bias corrections for gyro 
00066 
00067 // Specify sensor full scale range
00068 int Ascale = AFS_2G;
00069 int Gscale = GFS_250DPS;
00070 
00071 // Scale resolutions per LSB for the sensors
00072 float aRes, gRes; 
00073 
00074 // Calculates Acc resolution
00075 void MPU6050::getAres()
00076 {
00077     switch(Ascale)
00078     {
00079         case AFS_2G:
00080             aRes = 2.0/32768.0;
00081             break;
00082         case AFS_4G:
00083             aRes = 4.0/32768.0;
00084             break;
00085         case AFS_8G:
00086             aRes = 8.0/32768.0;
00087             break;
00088         case AFS_16G:
00089             aRes = 16.0/32768.0;
00090             break;         
00091     }
00092 }
00093 
00094 // Calculates Gyro resolution
00095 void MPU6050::getGres()
00096 {
00097     switch(Gscale)
00098     {
00099         case GFS_250DPS:
00100             gRes = 250.0/32768.0;
00101             break;
00102         case GFS_500DPS:
00103             gRes = 500.0/32768.0;
00104             break;
00105         case GFS_1000DPS:
00106             gRes = 1000.0/32768.0;
00107             break;
00108         case GFS_2000DPS:
00109             gRes = 2000.0/32768.0;
00110             break;
00111     }
00112 }
00113     
00114 void MPU6050::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
00115 {
00116     char data_write[2];
00117     data_write[0]=subAddress;           // I2C sends MSB first. Namely  >>|subAddress|>>|data|
00118     data_write[1]=data;
00119     i2c.write(address,data_write,2,0);  // i2c.write(int address, char* data, int length, bool repeated=false);  
00120 }
00121 
00122 char MPU6050::readByte(uint8_t address, uint8_t subAddress)
00123 {
00124     char data_read[1];  // will store the register data    
00125     char data_write[1];
00126     data_write[0]=subAddress;
00127     i2c.write(address,data_write,1,1);  // have not stopped yet
00128     i2c.read(address,data_read,1,0);    // read the data and stop
00129     return data_read[0];
00130 } 
00131 
00132 void MPU6050::readBytes(uint8_t address, uint8_t subAddress, uint8_t byteNum, uint8_t* dest)
00133 {
00134     char data[14],data_write[1];  
00135     data_write[0]=subAddress;      
00136     i2c.write(address,data_write,1,1);
00137     i2c.read(address,data,byteNum,0);
00138     for(int i=0;i<byteNum;i++)         // equate the addresses
00139         dest[i]=data[i];
00140 }
00141 
00142 // Communication test: CHECKADDRESS register reading 
00143 void MPU6050::checkaddress()
00144 {
00145     uint8_t checkaddress = readByte(MPU6050_ADDRESS, CHECKADDRESS_MPU6050);   // Should return 0x68
00146     pc.printf("Sensor Address ID: 0x%x \r\n",checkaddress);
00147     
00148     if(checkaddress==0x68)
00149     {
00150         pc.printf("IMU is online now... \r\n");  
00151     }
00152     else
00153     {
00154         pc.printf("No connection to IMU !! Check the connections... \r\n");  
00155     }  
00156 }
00157 
00158 // Initializes MPU6050 with the following config:
00159 // PLL with X axis gyroscope reference 
00160 // Sample rate: 200Hz for gyro and acc
00161 // Interrupts are disabled
00162 void MPU6050::init()
00163 {      
00164     i2c.frequency(400000);                      // fast i2c: 400 kHz
00165   
00166     /* Wake up the device */
00167     writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);  // wake up the device by clearing the sleep bit (bit6) 
00168     wait_ms(100); // wait 100 ms to stabilize  
00169     
00170     /* Get stable time source */
00171     // PLL with X axis gyroscope reference is used to improve stability
00172     writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);
00173     
00174     /* Configure Gyroscope and Accelerometer */
00175     // Disable FSYNC, acc bandwidth: 44 Hz, gyro bandwidth: 42 Hz
00176     // Sample rates: 1kHz, maximum delay: 4.9ms (which is pretty good for a 200 Hz maximum rate)
00177     writeByte(MPU6050_ADDRESS, CONFIG, 0x03);
00178     
00179     /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */
00180     // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above)
00181     writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04);
00182     
00183     /* Accelerometer configuration */
00184     uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
00185     writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0xE0);      // Clear self-test bits [7:5]
00186     writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp & ~0x18);      // Clear AFS bits [4:3]
00187     writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, temp | Ascale<<3);  // Set full scale range 
00188     
00189     /* Gyroscope configuration */       
00190     temp = readByte(MPU6050_ADDRESS, GYRO_CONFIG);
00191     writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0);      // Clear self-test bits [7:5]
00192     writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18);      // Clear FS bits [4:3]
00193     writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3);  // Set full scale range 
00194 }
00195 
00196 // Resets the device
00197 void MPU6050::reset()
00198 {
00199     writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80);  // set bit7 to reset the device
00200     wait_ms(100);    // wait 100 ms to stabilize        
00201 }
00202 
00203 void MPU6050::readAccelData(int16_t* dest)
00204 {
00205     uint8_t rawData[6];  // x,y,z acc data            
00206     readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);   // read six raw data registers sequentially and write them into data array
00207     
00208     /* Turn the MSB LSB into signed 16-bit value */
00209     dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // ACCEL_XOUT
00210     dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // ACCEL_YOUT
00211     dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // ACCEL_ZOUT
00212 }
00213 
00214 void MPU6050::readGyroData(int16_t* dest)
00215 {
00216     uint8_t rawData[6];  // x,y,z gyro data            
00217     readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);   // read the six raw data registers sequentially and write them into data array
00218     
00219     /* Turn the MSB LSB into signed 16-bit value */
00220     dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // GYRO_XOUT
00221     dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // GYRO_YOUT
00222     dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // GYRO_ZOUT    
00223 }
00224     
00225 int16_t MPU6050::readTempData()
00226 {
00227     uint8_t rawData[2];  // temperature data
00228     readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);   // read the two raw data registers sequentially and write them into data array 
00229     return (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);   // turn the MSB LSB into signed 16-bit value
00230 }
00231 
00232 /* Function which accumulates gyro and accelerometer data after device initialization. 
00233    It calculates the average of the at-rest readings and 
00234    then loads the resulting offsets into accelerometer and gyro bias registers. */
00235 /* 
00236     IMPORTANT NOTE: In this function;
00237          Resulting accel offsets are NOT pushed to the accel bias registers. accelBias[i] offsets are used in the main program.
00238          Resulting gyro offsets are pushed to the gyro bias registers. gyroBias[i] offsets are NOT used in the main program.
00239          Resulting data seems satisfactory.
00240 */
00241 // dest1: accelBias dest2: gyroBias
00242 void MPU6050::calibrate(float* dest1, float* dest2)
00243 {
00244     uint8_t data[12];       // data array to hold acc and gyro x,y,z data
00245     uint16_t fifo_count, packet_count, count;   
00246     int32_t accel_bias[3] = {0,0,0}; 
00247     int32_t gyro_bias[3] = {0,0,0};
00248     float aRes = 2.0/32768.0;   
00249     float gRes = 250.0/32768.0;
00250     uint16_t accelsensitivity = 16384; // = 1/aRes = 16384 LSB/g
00251     //uint16_t gyrosensitivity = 131;    // = 1/gRes = 131 LSB/dps
00252     
00253     reset();     // Reset device
00254     
00255     /* Get stable time source */
00256     writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);    // PLL with X axis gyroscope reference is used to improve stability
00257     writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00);    // Disable accel only low power mode 
00258     wait(0.2);
00259       
00260     /* Configure device for bias calculation */
00261     writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts
00262     writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00);      // Disable FIFO
00263     writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);   // Turn on internal clock source
00264     writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
00265     writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
00266     writeByte(MPU6050_ADDRESS, USER_CTRL, 0x04);    // Reset FIFO
00267     wait(0.015);   
00268     
00269     /* Configure accel and gyro for bias calculation */
00270     writeByte(MPU6050_ADDRESS, CONFIG, 0x01);       // Set low-pass filter to 188 Hz
00271     writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00);   // Set sample rate to 1 kHz
00272     writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
00273     writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
00274      
00275     /* Configure FIFO to capture accelerometer and gyro data for bias calculation */
00276     writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  
00277     writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78);     // Enable accelerometer and gyro for FIFO  (max size 1024 bytes in MPU-6050)
00278     wait(0.08);                                    // Sample rate is 1 kHz, accumulates 80 samples in 80 milliseconds. 
00279     // accX: 2 byte, accY: 2 byte, accZ: 2 byte. gyroX: 2 byte, gyroY: 2 byte, gyroZ: 2 byte.   12*80=960 byte < 1024 byte                                               
00280                                                                                                
00281     /* At end of sample accumulation, turn off FIFO sensor read */
00282     writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00);             // Disable FIFO
00283     readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]);  // Read FIFO sample count
00284     fifo_count = ((uint16_t)data[0] << 8) | data[1];
00285     packet_count = fifo_count/12;                          // The number of sets of full acc and gyro data for averaging. packet_count = 80 in this case
00286     
00287     for(count=0; count<packet_count; count++)
00288     {
00289         int16_t accel_temp[3]={0,0,0}; 
00290         int16_t gyro_temp[3]={0,0,0};
00291         readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
00292         
00293         /* Form signed 16-bit integer for each sample in FIFO */
00294         accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ; 
00295         accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;
00296         accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;    
00297         gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;
00298         gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;
00299         gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
00300         
00301         /* Sum individual signed 16-bit biases to get accumulated signed 32-bit biases */
00302         accel_bias[0] += (int32_t) accel_temp[0]; 
00303         accel_bias[1] += (int32_t) accel_temp[1];
00304         accel_bias[2] += (int32_t) accel_temp[2];  
00305         gyro_bias[0]  += (int32_t) gyro_temp[0];
00306         gyro_bias[1]  += (int32_t) gyro_temp[1];
00307         gyro_bias[2]  += (int32_t) gyro_temp[2];
00308     }
00309     
00310     /* Normalize sums to get average count biases */
00311     accel_bias[0] /= (int32_t) packet_count; 
00312     accel_bias[1] /= (int32_t) packet_count;
00313     accel_bias[2] /= (int32_t) packet_count;
00314     gyro_bias[0]  /= (int32_t) packet_count;
00315     gyro_bias[1]  /= (int32_t) packet_count;
00316     gyro_bias[2]  /= (int32_t) packet_count;
00317       
00318     /* Remove gravity from the z-axis accelerometer bias calculation */  
00319     if(accel_bias[2] > 0) {accel_bias[2] -= (int32_t) accelsensitivity;}  
00320     else {accel_bias[2] += (int32_t) accelsensitivity;}
00321     
00322     /* Output scaled accelerometer biases for manual subtraction in the main program */
00323     dest1[0] = accel_bias[0]*aRes;
00324     dest1[1] = accel_bias[1]*aRes;
00325     dest1[2] = accel_bias[2]*aRes;
00326     
00327     /* Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup */
00328     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
00329     data[1] = (-gyro_bias[0]/4)       & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
00330     data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;
00331     data[3] = (-gyro_bias[1]/4)       & 0xFF;
00332     data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;
00333     data[5] = (-gyro_bias[2]/4)       & 0xFF;
00334 
00335     /* Push gyro biases to hardware registers */
00336     writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); 
00337     writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]);
00338     writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]);
00339     writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]);
00340     writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]);
00341     writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]);
00342 
00343     /* Construct gyro bias in deg/s for later manual subtraction */
00344     dest2[0] = gyro_bias[0]*gRes;   
00345     dest2[1] = gyro_bias[1]*gRes;
00346     dest2[2] = gyro_bias[2]*gRes;
00347 }
00348 
00349 void MPU6050::complementaryFilter(float* pitch, float* roll)
00350 {
00351     /* Get actual acc value */
00352     readAccelData(accelData);
00353     getAres();
00354     ax = accelData[0]*aRes - accelBias[0];
00355     ay = accelData[1]*aRes - accelBias[1];
00356     az = accelData[2]*aRes - accelBias[2]; 
00357     
00358     axx = 9.80665f * ax;
00359 
00360     /* Get actual gyro value */
00361     readGyroData(gyroData);
00362     getGres();     
00363     gx = gyroData[0]*gRes;  // - gyroBias[0];      // Results are better without extracting gyroBias[i]
00364     gy = gyroData[1]*gRes;  // - gyroBias[1]; 
00365     gz = gyroData[2]*gRes;  // - gyroBias[2]; 
00366 
00367     float pitchAcc, rollAcc;
00368 
00369     /* Integrate the gyro data(deg/s) over time to get angle */
00370     *pitch += gx * dt;  // Angle around the X-axis
00371     *roll -=  gy * dt;  // Angle around the Y-axis
00372     
00373     /* Turning around the X-axis results in a vector on the Y-axis
00374     whereas turning around the Y-axis results in a vector on the X-axis. */
00375     pitchAcc = atan2f(accelData[1], accelData[2])*180/PI;
00376     rollAcc  = atan2f(accelData[0], accelData[2])*180/PI;
00377   
00378     /* Apply Complementary Filter */
00379     *pitch = *pitch * 0.98 + pitchAcc * 0.02;
00380     *roll  = *roll  * 0.98 + rollAcc  * 0.02;  
00381 }