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 char MPU6050::testConnection( void ) {
00049     char temp;
00050     temp = this->read(MPU6050_WHO_AM_I_REG);
00051     //return (temp == (MPU6050_ADDRESS & 0xFE));
00052     return temp;
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 //--------------------------------------------------
00153 //------------------Gyroscope-----------------------
00154 //--------------------------------------------------
00155 void MPU6050::setGyroRange( char range ) {
00156     char temp;
00157     currentGyroRange = range;
00158     range = range & 0x03;
00159     temp = this->read(MPU6050_GYRO_CONFIG_REG);
00160     temp &= ~(3<<3);
00161     temp = temp + range<<3;
00162     this->write(MPU6050_GYRO_CONFIG_REG, temp);
00163 }
00164 
00165 int MPU6050::getGyroRawX( void ) {
00166     short retval;
00167     char data[2];
00168     this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
00169     retval = (data[0]<<8) + data[1];
00170     return (int)retval;
00171 }
00172     
00173 int MPU6050::getGyroRawY( void ) {
00174     short retval;
00175     char data[2];
00176     this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
00177     retval = (data[0]<<8) + data[1];
00178     return (int)retval;
00179 }
00180 
00181 int MPU6050::getGyroRawZ( void ) {
00182     short retval;
00183     char data[2];
00184     this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
00185     retval = (data[0]<<8) + data[1];
00186     return (int)retval;
00187 }
00188 
00189 void MPU6050::getGyroRaw( int *data ) {
00190     char temp[6];
00191     this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
00192     data[0] = (int)(short)((temp[0]<<8) + temp[1]);
00193     data[1] = (int)(short)((temp[2]<<8) + temp[3]);
00194     data[2] = (int)(short)((temp[4]<<8) + temp[5]);
00195 }
00196 
00197 void MPU6050::getGyro( float *data ) {
00198     int temp[3];
00199     this->getGyroRaw(temp);
00200     if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
00201         data[0]=(float)temp[0] / 7505.7;
00202         data[1]=(float)temp[1] / 7505.7;
00203         data[2]=(float)temp[2] / 7505.7;
00204         }
00205     if (currentGyroRange == MPU6050_GYRO_RANGE_500){
00206         data[0]=(float)temp[0] / 3752.9;
00207         data[1]=(float)temp[1] / 3752.9;
00208         data[2]=(float)temp[2] / 3752.9;
00209         }
00210     if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
00211         data[0]=(float)temp[0] / 1879.3;;
00212         data[1]=(float)temp[1] / 1879.3;
00213         data[2]=(float)temp[2] / 1879.3;
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 //-------------------Temperature--------------------
00223 //--------------------------------------------------
00224 int MPU6050::getTempRaw( void ) {
00225     short retval;
00226     char data[2];
00227     this->read(MPU6050_TEMP_H_REG, data, 2);
00228     retval = (data[0]<<8) + data[1];
00229     return (int)retval;
00230 }
00231 
00232 float MPU6050::getTemp( void ) {
00233     float retval;
00234     retval=(float)this->getTempRaw();
00235     retval=(retval+521.0)/340.0+35.0;
00236     return retval;
00237 }