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