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