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 bool MPU6050::testConnection( void ) {
00049     char temp;
00050     temp = this->read(MPU6050_WHO_AM_I_REG);
00051     return (temp == (MPU6050_ADDRESS & 0xFE));
00052 }
00053 
00054 void MPU6050::setBW(char BW) {
00055     char temp;
00056     BW=BW & 0x07;
00057     temp = this->read(MPU6050_CONFIG_REG);
00058     temp &= 0xF8;
00059     temp = temp + BW;
00060     this->write(MPU6050_CONFIG_REG, temp);
00061 }
00062 
00063 void MPU6050::setI2CBypass(bool state) {
00064     char temp;
00065     temp = this->read(MPU6050_INT_PIN_CFG);
00066     if (state == true)
00067         temp |= 1<<MPU6050_BYPASS_BIT;
00068     if (state == false)
00069         temp &= ~(1<<MPU6050_BYPASS_BIT);
00070     this->write(MPU6050_INT_PIN_CFG, temp);
00071 }
00072 
00073 //--------------------------------------------------
00074 //----------------Accelerometer---------------------
00075 //--------------------------------------------------
00076 
00077 void MPU6050::setAcceleroRange( char range ) {
00078     char temp;
00079     range = range & 0x03;
00080     currentAcceleroRange = range;
00081     
00082     temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
00083     temp &= ~(3<<3);
00084     temp = temp + (range<<3);
00085     this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
00086 }
00087 
00088 int MPU6050::getAcceleroRawX( void ) {
00089     short retval;
00090     char data[2];
00091     this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
00092     retval = (data[0]<<8) + data[1];
00093     return (int)retval;
00094 }
00095     
00096 int MPU6050::getAcceleroRawY( void ) {
00097     short retval;
00098     char data[2];
00099     this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
00100     retval = (data[0]<<8) + data[1];
00101     return (int)retval;
00102 }
00103 
00104 int MPU6050::getAcceleroRawZ( void ) {
00105     short retval;
00106     char data[2];
00107     this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
00108     retval = (data[0]<<8) + data[1];
00109     return (int)retval;
00110 }
00111 
00112 void MPU6050::getAcceleroRaw( int *data ) {
00113     char temp[6];
00114     this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
00115     data[0] = (int)(short)((temp[0]<<8) + temp[1]);
00116     data[1] = (int)(short)((temp[2]<<8) + temp[3]);
00117     data[2] = (int)(short)((temp[4]<<8) + temp[5]);
00118 }
00119 
00120 void MPU6050::getAccelero( float *data ) {
00121     int temp[3];
00122     this->getAcceleroRaw(temp);
00123     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
00124         data[0]=(float)temp[0] / 16384.0 * 9.81;
00125         data[1]=(float)temp[1] / 16384.0 * 9.81;
00126         data[2]=(float)temp[2] / 16384.0 * 9.81;
00127         }
00128     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
00129         data[0]=(float)temp[0] / 8192.0 * 9.81;
00130         data[1]=(float)temp[1] / 8192.0 * 9.81;
00131         data[2]=(float)temp[2] / 8192.0 * 9.81;
00132         }
00133     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
00134         data[0]=(float)temp[0] / 4096.0 * 9.81;
00135         data[1]=(float)temp[1] / 4096.0 * 9.81;
00136         data[2]=(float)temp[2] / 4096.0 * 9.81;
00137         }
00138     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
00139         data[0]=(float)temp[0] / 2048.0 * 9.81;
00140         data[1]=(float)temp[1] / 2048.0 * 9.81;
00141         data[2]=(float)temp[2] / 2048.0 * 9.81;
00142         }
00143     
00144     #ifdef DOUBLE_ACCELERO
00145         data[0]*=2;
00146         data[1]*=2;   
00147         data[2]*=2;
00148     #endif   
00149 }
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] / 7505.7;
00201         data[1]=(float)temp[1] / 7505.7;
00202         data[2]=(float)temp[2] / 7505.7;
00203         }
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     if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
00215         data[0]=(float)temp[0] / 939.7;
00216         data[1]=(float)temp[1] / 939.7;
00217         data[2]=(float)temp[2] / 939.7;
00218         }
00219 }
00220 //--------------------------------------------------
00221 //-------------------Temperature--------------------
00222 //--------------------------------------------------
00223 int MPU6050::getTempRaw( void ) {
00224     short retval;
00225     char data[2];
00226     this->read(MPU6050_TEMP_H_REG, data, 2);
00227     retval = (data[0]<<8) + data[1];
00228     return (int)retval;
00229 }
00230 
00231 float MPU6050::getTemp( void ) {
00232     float retval;
00233     retval=(float)this->getTempRaw();
00234     retval=(retval+521.0)/340.0+35.0;
00235     return retval;
00236 }
00237