imu for l432kc

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MPU6050.cpp Source File

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     MPU6050_ADDRESS=0x68;
00013     //connection.frequency(400000); //mesures ne fonctionnent pas
00014     // test I2C connexion and update MPU6050_ADDRESS
00015     isFindMPU6050=I2C_finder();
00016     // if MPU6050 not found stop the program !!!!!
00017     if(!isFindMPU6050)
00018     {
00019          DigitalOut myled(LED1);
00020         printf("MPU6050 doesn't exist\n\r");
00021         printf("Check Connexion SCL and SDA and Power supply of your sensor\n\r");    
00022         while(1) {
00023             myled = 1;
00024             wait(0.2);
00025             myled = 0;
00026             wait(0.2);
00027         }
00028     }
00029 }
00030 
00031 //--------------------------------------------------
00032 //-------------------General------------------------
00033 //--------------------------------------------------
00034 
00035 bool MPU6050::I2C_finder(){
00036     int i,isOK=1;
00037     char data;
00038     bool isFindSlave=false;
00039     printf("test I2C\n\r");
00040     for(i=1;i<127;i++)
00041     {   wait(0.001);
00042         isOK=connection.read(i*2,&data,1,false);   
00043         if(isOK==0){
00044             printf("slave I2C find : 0X%X\n\r",i);
00045             if(i==MPU6050_ADDRESS || i==MPU6050_ADDRESS+1){
00046                 printf("MPU6050 has been found : 0x%X\n\r",i);
00047                 MPU6050_ADDRESS=i; // on met l'adresse du MPU6050 = 0x68 ou 0x69
00048                 isFindSlave=true;
00049             }
00050         }        
00051     } 
00052     return isFindSlave;  
00053 }
00054 
00055 void MPU6050::write(char address, char data) {
00056     char temp[2];
00057     temp[0]=address;
00058     temp[1]=data;
00059     
00060     connection.write(MPU6050_ADDRESS * 2,temp,2);
00061 }
00062 
00063 char MPU6050::read(char address) {
00064     char retval;
00065     connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
00066     connection.read(MPU6050_ADDRESS * 2, &retval, 1);
00067     return retval;
00068 }
00069 
00070 void MPU6050::read(char address, char *data, int length) {
00071     connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
00072     connection.read(MPU6050_ADDRESS * 2, data, length);
00073 }
00074 
00075 void MPU6050::setSleepMode(bool state) {
00076     char temp;
00077     temp = this->read(MPU6050_PWR_MGMT_1_REG);
00078     if (state == true)
00079         temp |= 1<<MPU6050_SLP_BIT;
00080     if (state == false)
00081         temp &= ~(1<<MPU6050_SLP_BIT);
00082     this->write(MPU6050_PWR_MGMT_1_REG, temp);
00083 }
00084 
00085 bool MPU6050::testConnection( void ) {
00086     char temp=0;
00087     temp = this->read(MPU6050_WHO_AM_I_REG);
00088     printf(" WHO AM I : 0X%X\n\r",temp);
00089     return (temp == (MPU6050_ADDRESS & 0xFE));
00090 }
00091 
00092 void MPU6050::setBW(char BW) {
00093     char temp;
00094     BW=BW & 0x07;
00095     temp = this->read(MPU6050_CONFIG_REG);
00096     temp &= 0xF8;
00097     temp = temp + BW;
00098     this->write(MPU6050_CONFIG_REG, temp);
00099 }
00100 
00101 void MPU6050::setI2CBypass(bool state) {
00102     char temp;
00103     temp = this->read(MPU6050_INT_PIN_CFG);
00104     if (state == true)
00105         temp |= 1<<MPU6050_BYPASS_BIT;
00106     if (state == false)
00107         temp &= ~(1<<MPU6050_BYPASS_BIT);
00108     this->write(MPU6050_INT_PIN_CFG, temp);
00109 }
00110 
00111 //--------------------------------------------------
00112 //----------------Accelerometer---------------------
00113 //--------------------------------------------------
00114 
00115 void MPU6050::setAcceleroRange( char range ) {
00116     char temp;
00117     range = range & 0x03;
00118     currentAcceleroRange = range;
00119     
00120     temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
00121     temp &= ~(3<<3);
00122     temp = temp + (range<<3);
00123     this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
00124 }
00125 
00126 int MPU6050::getAcceleroRawX( void ) {
00127     short retval;
00128     char data[2];
00129     this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
00130     retval = (data[0]<<8) + data[1];
00131     return (int)retval;
00132 }
00133     
00134 int MPU6050::getAcceleroRawY( void ) {
00135     short retval;
00136     char data[2];
00137     this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
00138     retval = (data[0]<<8) + data[1];
00139     return (int)retval;
00140 }
00141 
00142 int MPU6050::getAcceleroRawZ( void ) {
00143     short retval;
00144     char data[2];
00145     this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
00146     retval = (data[0]<<8) + data[1];
00147     return (int)retval;
00148 }
00149 
00150 void MPU6050::getAcceleroRaw( int *data ) {
00151     char temp[6];
00152     this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
00153     data[0] = (int)(short)((temp[0]<<8) + temp[1]);
00154     data[1] = (int)(short)((temp[2]<<8) + temp[3]);
00155     data[2] = (int)(short)((temp[4]<<8) + temp[5]);
00156 }
00157 
00158 void MPU6050::getAccelero( float *data ) {
00159     int temp[3];
00160     this->getAcceleroRaw(temp);
00161     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
00162         data[0]=(float)temp[0] / 16384.0 * 9.81;
00163         data[1]=(float)temp[1] / 16384.0 * 9.81;
00164         data[2]=(float)temp[2] / 16384.0 * 9.81;
00165         }
00166     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
00167         data[0]=(float)temp[0] / 8192.0 * 9.81;
00168         data[1]=(float)temp[1] / 8192.0 * 9.81;
00169         data[2]=(float)temp[2] / 8192.0 * 9.81;
00170         }
00171     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
00172         data[0]=(float)temp[0] / 4096.0 * 9.81;
00173         data[1]=(float)temp[1] / 4096.0 * 9.81;
00174         data[2]=(float)temp[2] / 4096.0 * 9.81;
00175         }
00176     if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
00177         data[0]=(float)temp[0] / 2048.0 * 9.81;
00178         data[1]=(float)temp[1] / 2048.0 * 9.81;
00179         data[2]=(float)temp[2] / 2048.0 * 9.81;
00180         }
00181     
00182     #ifdef DOUBLE_ACCELERO
00183         data[0]*=2;
00184         data[1]*=2;   
00185         data[2]*=2;
00186     #endif   
00187 }
00188 
00189 //--------------------------------------------------
00190 //------------------Gyroscope-----------------------
00191 //--------------------------------------------------
00192 void MPU6050::setGyroRange( char range ) {
00193     char temp;
00194     currentGyroRange = range;
00195     range = range & 0x03;
00196     temp = this->read(MPU6050_GYRO_CONFIG_REG);
00197     temp &= ~(3<<3);
00198     temp = temp + range<<3;
00199     this->write(MPU6050_GYRO_CONFIG_REG, temp);
00200 }
00201 
00202 int MPU6050::getGyroRawX( void ) {
00203     short retval;
00204     char data[2];
00205     this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
00206     retval = (data[0]<<8) + data[1];
00207     return (int)retval;
00208 }
00209     
00210 int MPU6050::getGyroRawY( void ) {
00211     short retval;
00212     char data[2];
00213     this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
00214     retval = (data[0]<<8) + data[1];
00215     return (int)retval;
00216 }
00217 
00218 int MPU6050::getGyroRawZ( void ) {
00219     short retval;
00220     char data[2];
00221     this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
00222     retval = (data[0]<<8) + data[1];
00223     return (int)retval;
00224 }
00225 
00226 void MPU6050::getGyroRaw( int *data ) {
00227     char temp[6];
00228     this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
00229     data[0] = (int)(short)((temp[0]<<8) + temp[1]);
00230     data[1] = (int)(short)((temp[2]<<8) + temp[3]);
00231     data[2] = (int)(short)((temp[4]<<8) + temp[5]);
00232 }
00233 
00234 void MPU6050::getGyro( float *data ) {
00235     int temp[3];
00236     this->getGyroRaw(temp);
00237     if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
00238         data[0]=(float)temp[0] / 7505.7;
00239         data[1]=(float)temp[1] / 7505.7;
00240         data[2]=(float)temp[2] / 7505.7;
00241         }
00242     if (currentGyroRange == MPU6050_GYRO_RANGE_500){
00243         data[0]=(float)temp[0] / 3752.9;
00244         data[1]=(float)temp[1] / 3752.9;
00245         data[2]=(float)temp[2] / 3752.9;
00246         }
00247     if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
00248         data[0]=(float)temp[0] / 1879.3;;
00249         data[1]=(float)temp[1] / 1879.3;
00250         data[2]=(float)temp[2] / 1879.3;
00251         }
00252     if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
00253         data[0]=(float)temp[0] / 939.7;
00254         data[1]=(float)temp[1] / 939.7;
00255         data[2]=(float)temp[2] / 939.7;
00256         }
00257 }
00258 //--------------------------------------------------
00259 //-------------------Temperature--------------------
00260 //--------------------------------------------------
00261 int MPU6050::getTempRaw( void ) {
00262     short retval;
00263     char data[2];
00264     this->read(MPU6050_TEMP_H_REG, data, 2);
00265     retval = (data[0]<<8) + data[1];
00266     return (int)retval;
00267 }
00268 
00269 float MPU6050::getTemp( void ) {
00270     float retval;
00271     retval=(float)this->getTempRaw();
00272     retval=(retval+521.0)/340.0+35.0;
00273     return retval;
00274 }
00275