imu for l432kc
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Tue Jul 12 2022 18:54:00 by
1.7.2