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 }
00013 
00014 //--------------------------------------------------
00015 //-------------------General------------------------
00016 //--------------------------------------------------
00017 
00018 void MPU6050::write(char address, char data) {
00019     char temp[2];
00020     temp[0]=address;
00021     temp[1]=data;
00022     
00023     connection.write(MPU6050_ADDRESS * 2,temp,2);
00024 }
00025 
00026 char MPU6050::read(char address) {
00027     char retval;
00028     connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
00029     connection.read(MPU6050_ADDRESS * 2, &retval, 1);
00030     return retval;
00031 }
00032 
00033 void MPU6050::read(char address, char *data, int length) {
00034     connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
00035     connection.read(MPU6050_ADDRESS * 2, data, length);
00036 }
00037 
00038 void MPU6050::setSleepMode(bool state) {
00039     char temp;
00040     temp = this->read(MPU6050_PWR_MGMT_1_REG);
00041     if (state == true)
00042         temp |= 1<<MPU6050_SLP_BIT;
00043     if (state == false)
00044         temp &= ~(1<<MPU6050_SLP_BIT);
00045     this->write(MPU6050_PWR_MGMT_1_REG, temp);
00046 }
00047 
00048 int MPU6050::testConnection( void ) {
00049     char temp;
00050     temp = this->read(MPU6050_WHO_AM_I_REG);
00051 //    return ((temp & 0x7E) == (MPU6050_ADDRESS & 0x7E));
00052 //    return ((temp & 0x7E) == 0x18);
00053     return temp;
00054 }
00055 
00056 void MPU6050::setBW(char BW) {
00057     char temp;
00058     BW=BW & 0x07;
00059     temp = this->read(MPU6050_CONFIG_REG);
00060     temp &= 0xF8;
00061     temp = temp + BW;
00062     this->write(MPU6050_CONFIG_REG, temp);
00063 }
00064 
00065 void MPU6050::setI2CBypass(bool state) {
00066     char temp;
00067     temp = this->read(MPU6050_INT_PIN_CFG);
00068     if (state == true)
00069         temp |= 1<<MPU6050_BYPASS_BIT;
00070     if (state == false)
00071         temp &= ~(1<<MPU6050_BYPASS_BIT);
00072     this->write(MPU6050_INT_PIN_CFG, temp);
00073 }
00074 
00075 //--------------------------------------------------
00076 //----------------Accelerometer---------------------
00077 //--------------------------------------------------
00078 
00079 void MPU6050::setAcceleroRange( char range ) {
00080     char temp;
00081     range = range & 0x03;
00082     currentAcceleroRange = range;
00083     
00084     temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
00085     temp &= ~(3<<3);
00086     temp = temp + (range<<3);
00087     this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
00088 }
00089 
00090 int MPU6050::getAcceleroRawX( void ) {
00091     short retval;
00092     char data[2];
00093     this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
00094     retval = (data[0]<<8) + data[1];
00095     return (int)retval;
00096 }
00097     
00098 int MPU6050::getAcceleroRawY( void ) {
00099     short retval;
00100     char data[2];
00101     this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
00102     retval = (data[0]<<8) + data[1];
00103     return (int)retval;
00104 }
00105 
00106 int MPU6050::getAcceleroRawZ( void ) {
00107     short retval;
00108     char data[2];
00109     this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
00110     retval = (data[0]<<8) + data[1];
00111     return (int)retval;
00112 }
00113 
00114 void MPU6050::getAcceleroRaw( int *data ) {
00115     char temp[6];
00116     this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
00117     data[0] = (int)(short)((temp[0]<<8) + temp[1]);
00118     data[1] = (int)(short)((temp[2]<<8) + temp[3]);
00119     data[2] = (int)(short)((temp[4]<<8) + temp[5]);
00120 }
00121 
00122 void MPU6050::getAccelero( float *data ) {
00123     int temp[3];
00124     this->getAcceleroRaw(temp);
00125     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
00126         data[0]=(float)temp[0] / 16384.0 * 9.81;
00127         data[1]=(float)temp[1] / 16384.0 * 9.81;
00128         data[2]=(float)temp[2] / 16384.0 * 9.81;
00129         }
00130     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
00131         data[0]=(float)temp[0] / 8192.0 * 9.81;
00132         data[1]=(float)temp[1] / 8192.0 * 9.81;
00133         data[2]=(float)temp[2] / 8192.0 * 9.81;
00134         }
00135     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
00136         data[0]=(float)temp[0] / 4096.0 * 9.81;
00137         data[1]=(float)temp[1] / 4096.0 * 9.81;
00138         data[2]=(float)temp[2] / 4096.0 * 9.81;
00139         }
00140     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
00141         data[0]=(float)temp[0] / 2048.0 * 9.81;
00142         data[1]=(float)temp[1] / 2048.0 * 9.81;
00143         data[2]=(float)temp[2] / 2048.0 * 9.81;
00144         }
00145     
00146     #ifdef DOUBLE_ACCELERO
00147         data[0]*=2;
00148         data[1]*=2;   
00149         data[2]*=2;
00150     #endif   
00151 }
00152 
00153 //--------------------------------------------------
00154 //------------------Gyroscope-----------------------
00155 //--------------------------------------------------
00156 void MPU6050::setGyroRange( char range ) {
00157     char temp;
00158     currentGyroRange = range;
00159     range = range & 0x03;
00160     temp = this->read(MPU6050_GYRO_CONFIG_REG);
00161     temp &= ~(3<<3);
00162     temp = temp + range<<3;
00163     this->write(MPU6050_GYRO_CONFIG_REG, temp);
00164 }
00165 
00166 int MPU6050::getGyroRawX( void ) {
00167     short retval;
00168     char data[2];
00169     this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
00170     retval = (data[0]<<8) + data[1];
00171     return (int)retval;
00172 }
00173     
00174 int MPU6050::getGyroRawY( void ) {
00175     short retval;
00176     char data[2];
00177     this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
00178     retval = (data[0]<<8) + data[1];
00179     return (int)retval;
00180 }
00181 
00182 int MPU6050::getGyroRawZ( void ) {
00183     short retval;
00184     char data[2];
00185     this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
00186     retval = (data[0]<<8) + data[1];
00187     return (int)retval;
00188 }
00189 
00190 void MPU6050::getGyroRaw( int *data ) {
00191     char temp[6];
00192     this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
00193     data[0] = (int)(short)((temp[0]<<8) + temp[1]);
00194     data[1] = (int)(short)((temp[2]<<8) + temp[3]);
00195     data[2] = (int)(short)((temp[4]<<8) + temp[5]);
00196 }
00197 
00198 void MPU6050::getGyro( float *data ) {
00199     int temp[3];
00200     this->getGyroRaw(temp);
00201     if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
00202         data[0]=(float)temp[0] / 7505.7;
00203         data[1]=(float)temp[1] / 7505.7;
00204         data[2]=(float)temp[2] / 7505.7;
00205         }
00206     if (currentGyroRange == MPU6050_GYRO_RANGE_500){
00207         data[0]=(float)temp[0] / 3752.9;
00208         data[1]=(float)temp[1] / 3752.9;
00209         data[2]=(float)temp[2] / 3752.9;
00210         }
00211     if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
00212         data[0]=(float)temp[0] / 1879.3;;
00213         data[1]=(float)temp[1] / 1879.3;
00214         data[2]=(float)temp[2] / 1879.3;
00215         }
00216     if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
00217         data[0]=(float)temp[0] / 939.7;
00218         data[1]=(float)temp[1] / 939.7;
00219         data[2]=(float)temp[2] / 939.7;
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