MPU6050.cpp

00001 /**
00002  * Includes
00003  */
00004 #include "MPU6050.h"
00005 
00006 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
00007     this->setSleepMode(false);
00008     
00009     //Initializations:
00010     currentGyroRange = 0;
00011     currentAcceleroRange=0;
00012     alpha = ALPHA;
00013 }
00014 
00015 //--------------------------------------------------
00016 //-------------------General------------------------
00017 //--------------------------------------------------
00018 
00019 void MPU6050::write(char address, char data) {
00020     char temp[2];
00021     temp[0]=address;
00022     temp[1]=data;
00023     
00024     connection.write(MPU6050_ADDRESS * 2,temp,2);
00025 }
00026 
00027 char MPU6050::read(char address) {
00028     char retval;
00029     connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
00030     connection.read(MPU6050_ADDRESS * 2, &retval, 1);
00031     return retval;
00032 }
00033 
00034 void MPU6050::read(char address, char *data, int length) {
00035     connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
00036     connection.read(MPU6050_ADDRESS * 2, data, length);
00037 }
00038 
00039 void MPU6050::setSleepMode(bool state) {
00040     char temp;
00041     temp = this->read(MPU6050_PWR_MGMT_1_REG);
00042     if (state == true)
00043         temp |= 1<<MPU6050_SLP_BIT;
00044     if (state == false)
00045         temp &= ~(1<<MPU6050_SLP_BIT);
00046     this->write(MPU6050_PWR_MGMT_1_REG, temp);
00047 }
00048 
00049 bool MPU6050::testConnection( void ) {
00050     char temp;
00051     temp = this->read(MPU6050_WHO_AM_I_REG);
00052     return (temp == (MPU6050_ADDRESS & 0xFE));
00053 }
00054 
00055 void MPU6050::setBW(char BW) {
00056     char temp;
00057     BW=BW & 0x07;
00058     temp = this->read(MPU6050_CONFIG_REG);
00059     temp &= 0xF8;
00060     temp = temp + BW;
00061     this->write(MPU6050_CONFIG_REG, temp);
00062 }
00063 
00064 void MPU6050::setI2CBypass(bool state) {
00065     char temp;
00066     temp = this->read(MPU6050_INT_PIN_CFG);
00067     if (state == true)
00068         temp |= 1<<MPU6050_BYPASS_BIT;
00069     if (state == false)
00070         temp &= ~(1<<MPU6050_BYPASS_BIT);
00071     this->write(MPU6050_INT_PIN_CFG, temp);
00072 }
00073 
00074 //--------------------------------------------------
00075 //----------------Accelerometer---------------------
00076 //--------------------------------------------------
00077 
00078 void MPU6050::setAcceleroRange( char range ) {
00079     char temp;
00080     range = range & 0x03;
00081     currentAcceleroRange = range;
00082     
00083     temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
00084     temp &= ~(3<<3);
00085     temp = temp + (range<<3);
00086     this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
00087 }
00088 
00089 int MPU6050::getAcceleroRawX( void ) {
00090     short retval;
00091     char data[2];
00092     this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
00093     retval = (data[0]<<8) + data[1];
00094     return (int)retval;
00095 }
00096     
00097 int MPU6050::getAcceleroRawY( void ) {
00098     short retval;
00099     char data[2];
00100     this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
00101     retval = (data[0]<<8) + data[1];
00102     return (int)retval;
00103 }
00104 
00105 int MPU6050::getAcceleroRawZ( void ) {
00106     short retval;
00107     char data[2];
00108     this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
00109     retval = (data[0]<<8) + data[1];
00110     return (int)retval;
00111 }
00112 
00113 void MPU6050::getAcceleroRaw( int *data ) {
00114     char temp[6];
00115     this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
00116     data[0] = (int)(short)((temp[0]<<8) + temp[1]);
00117     data[1] = (int)(short)((temp[2]<<8) + temp[3]);
00118     data[2] = (int)(short)((temp[4]<<8) + temp[5]);
00119 }
00120 
00121 void MPU6050::getAccelero( float *data ) {
00122     int temp[3];
00123     this->getAcceleroRaw(temp);
00124     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
00125         data[0]=(float)temp[0] / 16384.0 * 9.81;
00126         data[1]=(float)temp[1] / 16384.0 * 9.81;
00127         data[2]=(float)temp[2] / 16384.0 * 9.81;
00128         }
00129     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
00130         data[0]=(float)temp[0] / 8192.0 * 9.81;
00131         data[1]=(float)temp[1] / 8192.0 * 9.81;
00132         data[2]=(float)temp[2] / 8192.0 * 9.81;
00133         }
00134     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
00135         data[0]=(float)temp[0] / 4096.0 * 9.81;
00136         data[1]=(float)temp[1] / 4096.0 * 9.81;
00137         data[2]=(float)temp[2] / 4096.0 * 9.81;
00138         }
00139     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
00140         data[0]=(float)temp[0] / 2048.0 * 9.81;
00141         data[1]=(float)temp[1] / 2048.0 * 9.81;
00142         data[2]=(float)temp[2] / 2048.0 * 9.81;
00143         }
00144     
00145     #ifdef DOUBLE_ACCELERO
00146         data[0]*=2;
00147         data[1]*=2;   
00148         data[2]*=2;
00149     #endif   
00150 }
00151 //--------------------------------------------------
00152 //------------------Gyroscope-----------------------
00153 //--------------------------------------------------
00154 void MPU6050::setGyroRange( char range ) {
00155     char temp;
00156     currentGyroRange = range;
00157     range = range & 0x03;
00158     temp = this->read(MPU6050_GYRO_CONFIG_REG);
00159     temp &= ~(3<<3);
00160     temp = temp + range<<3;
00161     this->write(MPU6050_GYRO_CONFIG_REG, temp);
00162 }
00163 
00164 int MPU6050::getGyroRawX( void ) {
00165     short retval;
00166     char data[2];
00167     this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
00168     retval = (data[0]<<8) + data[1];
00169     return (int)retval;
00170 }
00171     
00172 int MPU6050::getGyroRawY( void ) {
00173     short retval;
00174     char data[2];
00175     this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
00176     retval = (data[0]<<8) + data[1];
00177     return (int)retval;
00178 }
00179 
00180 int MPU6050::getGyroRawZ( void ) {
00181     short retval;
00182     char data[2];
00183     this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
00184     retval = (data[0]<<8) + data[1];
00185     return (int)retval;
00186 }
00187 
00188 void MPU6050::getGyroRaw( int *data ) {
00189     char temp[6];
00190     this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
00191     data[0] = (int)(short)((temp[0]<<8) + temp[1]);
00192     data[1] = (int)(short)((temp[2]<<8) + temp[3]);
00193     data[2] = (int)(short)((temp[4]<<8) + temp[5]);
00194 }
00195 
00196 void MPU6050::getGyro( float *data ) {
00197     int temp[3];
00198     this->getGyroRaw(temp);
00199     if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
00200         data[0]=(float)temp[0] / 301.0;
00201         data[1]=(float)temp[1] / 301.0;
00202         data[2]=(float)temp[2] / 301.0;
00203         }               //7505.5
00204     if (currentGyroRange == MPU6050_GYRO_RANGE_500){
00205         data[0]=(float)temp[0] / 3752.9;
00206         data[1]=(float)temp[1] / 3752.9;
00207         data[2]=(float)temp[2] / 3752.9;
00208         }
00209     if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
00210         data[0]=(float)temp[0]/ 1879.3;
00211         data[1]=(float)temp[1] / 1879.3;
00212         data[2]=(float)temp[2] / 1879.3;
00213 
00214         }
00215     if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
00216         data[0]=(float)temp[0] / 939.7;
00217         data[1]=(float)temp[1] / 939.7;
00218         data[2]=(float)temp[2] / 939.7;
00219         }
00220 
00221 }
00222 //--------------------------------------------------
00223 //-------------------Temperature--------------------
00224 //--------------------------------------------------
00225 int MPU6050::getTempRaw( void ) {
00226     short retval;
00227     char data[2];
00228     this->read(MPU6050_TEMP_H_REG, data, 2);
00229     retval = (data[0]<<8) + data[1];
00230     return (int)retval;
00231 }
00232 
00233 float MPU6050::getTemp( void ) {
00234     float retval;
00235     retval=(float)this->getTempRaw();
00236     retval=(retval+521.0)/340.0+35.0;
00237     return retval;
00238 }
00239 
00240 /**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more.
00241  function for getting angles in degrees from accelerometer
00242 */
00243 void MPU6050::getAcceleroAngle( float *data ) {
00244     float temp[3];
00245     this->getAccelero(temp);
00246     
00247     data[X_AXIS] = atan (temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
00248     data[Y_AXIS] = atan (-1*temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
00249     data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This one is not used anywhere later on
00250     
00251 //    data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES;  //This spits out values between -180 to 180 (360 degrees)
00252 //    data[X_AXIS] = atan2 (-1*temp[X_AXIS], temp[Z_AXIS]) * RADIANS_TO_DEGREES;    //but it takes longer and system gets unstable when angles ~90 degrees
00253 }
00254 
00255 ///function for getting offset values for the gyro & accelerometer
00256 void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
00257     float gyro[3];      
00258     float accAngle[3];
00259     
00260     for (int i = 0; i < 3; i++) {
00261         accOffset[i] = 0.0;     //initialise offsets to 0.0
00262         gyroOffset[i] = 0.0;
00263     }
00264     
00265     for (int i = 0; i < sampleSize; i++){
00266         this->getGyro(gyro); //take real life measurements  
00267         this->getAcceleroAngle (accAngle);
00268         
00269         for (int j = 0; j < 3; j++){
00270             *(accOffset+j) += accAngle[j]/sampleSize;    //average measurements
00271             *(gyroOffset+j) += gyro[j]/sampleSize;
00272         }
00273         wait (0.01);    //wait between each reading for accuracy 
00274     } 
00275 }
00276 
00277 ///function for computing angles for roll, pitch anf yaw
00278 void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){
00279     float gyro[3];
00280     float accAngle[3];
00281     
00282     this->getGyro(gyro);                //get gyro value in rad/s
00283     this->getAcceleroAngle(accAngle);   //get angle from accelerometer
00284     
00285     for (int i = 0; i < 3; i++){
00286         gyro[i] -= gyroOffset[i];       //substract offset values
00287         accAngle[i] -= accOffset[i];    
00288     }
00289     
00290     //apply filters on pitch and roll to get accurate angle values
00291     angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; 
00292     angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS];
00293     
00294     //calculate Yaw using just the gyroscope values - inaccurate
00295     angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval;     
00296 }
00297 
00298 ///function for setting a different Alpha value, which is used in complemetary filter calculations
00299 void MPU6050::setAlpha(float val){
00300     alpha = val;   
00301 }
00302 
00303 ///function for enabling interrupts on MPU6050 INT pin, when the data is ready to take
00304 void MPU6050::enableInt( void ){
00305     char temp;
00306     temp = this->read(MPU6050_RA_INT_ENABLE);
00307     temp |= 0x01;
00308     this->write(MPU6050_RA_INT_ENABLE, temp);
00309 }
00310 
00311 ///function for disabling interrupts on MPU6050 INT pin, when the data is ready to take
00312 void MPU6050::disableInt ( void ){
00313     char temp;
00314     temp = this->read(MPU6050_RA_INT_ENABLE);
00315     temp &= 0xFE;
00316     this->write(MPU6050_RA_INT_ENABLE, temp);
00317 }