syouichi imamori
/
MulticopterQuadX
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
I2cPeripherals.cpp
00001 #include "mbed.h" 00002 #include "I2cPeripherals.h" 00003 void wait(float); 00004 //Serial pc(USBTX, USBRX); 00005 I2cPeripherals::I2cPeripherals(PinName sda, PinName scl): _i2c(sda,scl) 00006 { 00007 00008 _i2c.frequency(400000); 00009 // LCD_contrast = 60; 00010 wait(0.5); 00011 LCD_addr = 0; 00012 Gyro_addr = 0; 00013 Accel_addr = 0; 00014 barometor_addr = 0; 00015 ultrasonic_addr = 0; 00016 ultrasonic_distance = 0; 00017 //pc.printf("start"); 00018 find(); 00019 start(50); 00020 00021 } 00022 00023 void I2cPeripherals::start(int contrast) 00024 { 00025 char tx[2]; 00026 // find(); // I2C interface sensor detect 00027 #ifdef ST7032 00028 LCD_addr = 0x7c; 00029 LCD_data = 0x40; 00030 tx[0] = 0x00; 00031 tx[1] = 0x38; 00032 if ( _i2c.write(LCD_addr,tx,2) == 0 ) { 00033 tx[1] = 0x39; 00034 _i2c.write(LCD_addr,tx,2); 00035 tx[1] = 0x14; //1F 00036 _i2c.write(LCD_addr,tx,2); 00037 tx[1] = 0x70 | (contrast & 0x0F); 00038 _i2c.write(LCD_addr,tx,2); 00039 tx[1] = 0x5C | ((contrast >> 4) & 0x03); 00040 _i2c.write(LCD_addr,tx,2); 00041 tx[1] = 0x6C; 00042 _i2c.write(LCD_addr,tx,2); 00043 wait(0.3); 00044 tx[1] = 0x0C; 00045 _i2c.write(LCD_addr,tx,2); // display on 00046 tx[1] = 0x06; 00047 _i2c.write(LCD_addr,tx,2); 00048 // tx[0] = 0x40; 00049 // tx[1] = '*'; 00050 // _i2c.write(LCD_addr,tx,2); // Display clear 00051 return; 00052 } 00053 #endif 00054 #ifdef ACM1602 00055 LCD_addr = 0xA0; 00056 LCD_data = 0x80; 00057 tx[0] = 0x00; 00058 tx[1] = 0x01; 00059 if ( _i2c.write(LCD_addr,tx,2) == 0 ) { 00060 wait(0.005); 00061 tx[1] = 0x38; 00062 _i2c.write(LCD_addr,tx,2); 00063 wait(0.005); 00064 tx[1] = 0x0F; 00065 _i2c.write(LCD_addr,tx,2); 00066 wait(0.005); 00067 tx[1] = 0x06; 00068 _i2c.write(LCD_addr,tx,2); 00069 wait(0.005); 00070 return; 00071 } 00072 #endif 00073 LCD_addr = 0; 00074 } 00075 00076 void I2cPeripherals::find() 00077 { 00078 #define sensnum 11 00079 char tx[2]; 00080 char rx[1]; 00081 SensorInf sens[sensnum] = { 0xD0,0x68,0x00, //ITG3200 gyro 00082 0xD2,0x68,0x00, 00083 0xD0,0x68,0x75, //MPU6050 gyro/accel 00084 0xD2,0x68,0x75, 00085 0xD4,0xD4,0x0F, //L3GD20 gyro 00086 0xD6,0xD4,0x0F, 00087 0xA6,0xE5,0x00, //ADXL345 accel 00088 0x3A,0xE5,0x00, 00089 0xB8,0xBB,0x0F, //LPS331 baro 00090 0xBA,0xBB,0x0F, 00091 0xE0,0x18,0x01 //SRF02 ultrasonic 00092 }; 00093 00094 for ( int num = 0; num < sensnum; num++ ) { 00095 tx[0]= sens[num].whoaddr; 00096 if ( _i2c.write(sens[num].addr,tx,1,true) != 0 ) continue; 00097 _i2c.read (sens[num].addr,rx,1); 00098 if ( (rx[0]&0x7E) != (sens[num].who&0x7E) ) continue; 00099 //pc.printf("who=%2x",rx[0]); 00100 switch ( num ) { 00101 #ifdef ITG3200 00102 case 0: //ITG3200 00103 case 1: 00104 Gyro_addr = sens[num].addr; 00105 Gyro_data = 0x1D; 00106 tx[0] = 0x16; 00107 tx[1] = 0x18; // 0x1D 00108 _i2c.write(sens[num].addr,tx,2); 00109 tx[0] = 0x3E; 00110 tx[1] = 0x01; 00111 _i2c.write(sens[num].addr,tx,2); 00112 wait(0.001f); 00113 break; 00114 #endif 00115 #ifdef MPU6050 00116 case 2: //MPU6050 00117 case 3: 00118 Gyro_addr = sens[num].addr; 00119 Gyro_data = 0x43; 00120 Accel_addr = sens[num].addr; 00121 Accel_data = 0x3B; 00122 tx[0] = 0x6B; 00123 tx[1] = 0x00; // PWR on 00124 _i2c.write(sens[num].addr,tx,2); 00125 tx[0] = 0x1A; // DLPF(Digitel low pass filter) 00126 tx[1] = 0x02; // set 0 to 7 00127 _i2c.write(sens[num].addr,tx,2); 00128 wait(0.001); 00129 tx[0] = 0x1B; 00130 tx[1] = 0x18; // +-2000deg 00131 _i2c.write(sens[num].addr,tx,2); 00132 wait(0.001); 00133 tx[0] = 0x1C; 00134 tx[1] = 0x18; // 00:2g,08:4g,10:8g,18:+-16g 00135 _i2c.write(sens[num].addr,tx,2); 00136 wait(0.001); 00137 break; 00138 #endif 00139 #ifdef L3GD20 00140 case 4: //L3GD20 00141 case 5: 00142 Gyro_addr = sens[num].addr; 00143 Gyro_data = 0x28; 00144 tx[0] = 0x20; 00145 tx[1] = 0xBF; //rate 400Hz 0x8F-0xBF 00146 _i2c.write(sens[num].addr,tx,2); 00147 tx[0] = 0x21; 00148 tx[1] = 0x09; 00149 _i2c.write(sens[num].addr,tx,2); 00150 tx[0] = 0x23; 00151 tx[1] = 0xF0; 00152 _i2c.write(sens[num].addr,tx,2); 00153 tx[0] = 0x24; 00154 tx[1] = 0x10; 00155 _i2c.write(sens[num].addr,tx,2); 00156 break; 00157 #endif 00158 #ifdef ADXL345 00159 case 6: //ADXL345 00160 case 7: 00161 Accel_addr = sens[num].addr; 00162 Accel_data = 0x32; 00163 tx[0] = 0x2D; 00164 tx[1] = 0x00; 00165 _i2c.write(sens[num].addr,tx,2); 00166 tx[0] = 0x31; 00167 tx[1] = 0x0B; //full range 08:2g 09:4g 0A:8g 0B:16g 00168 _i2c.write(sens[num].addr,tx,2); 00169 tx[0] = 0x2C; 00170 tx[1] = 0x09; //out rate 0xC:400Hz 0xD:800Hz 00171 _i2c.write(sens[num].addr,tx,2); 00172 tx[0] = 0x2D; 00173 tx[1] = 0x08; 00174 _i2c.write(sens[num].addr,tx,2); 00175 break; 00176 #endif 00177 #ifdef LPS331AP 00178 case 8: //barometor 00179 case 9: 00180 barometor_addr = sens[num].addr; 00181 barometor_data = 0x2B; 00182 tx[0] = 0x10; //RES_CNF 00183 tx[1] = 0x7A; 00184 _i2c.write(sens[num].addr,tx,2); 00185 tx[0] = 0x20; // CTL_REG1 00186 tx[1] = 0xB0; // power on 00187 _i2c.write(sens[num].addr,tx,2); 00188 // tx[0]= 0x21; // CTL_REG2 00189 // tx[1]= 0x01; // one shot start 00190 // _i2c.write(sens[num].addr,tx,2); 00191 wait(0.01); 00192 break; 00193 #endif 00194 case 10: //SFR02 ultrasonic 00195 ultrasonic_addr = sens[num].addr; 00196 ultrasonic_data = 0x02; 00197 tx[0] = 0x00; 00198 tx[1] = 0x52; 00199 _i2c.write(sens[num].addr,tx,2); 00200 wait(0.01); 00201 break; 00202 } 00203 } 00204 } 00205 00206 //int I2cPeripherals::_putc(int value) 00207 int I2cPeripherals::write_lcd(const char* value) 00208 { 00209 if ( LCD_addr == 0 ) return -1; 00210 _i2c.start(); 00211 _i2c.write(LCD_addr); 00212 _i2c.write(LCD_data); 00213 _i2c.write(LCD_addr,value,strlen(value)); 00214 _i2c.stop(); 00215 return 0; 00216 } 00217 /* 00218 int I2cPeripherals::_getc() 00219 { 00220 return -1; 00221 } 00222 */ 00223 void I2cPeripherals::write_reg(int I2cAddr,char reg_addr,char* data,int len) 00224 { 00225 char tx[17]; 00226 if ( len >16 ) len = 16; 00227 tx[0] = reg_addr; 00228 if ( len > 1 ) tx[0] |= 0x80; 00229 for (int i=0; i<len; i++) tx[i+1] = data[i]; 00230 _i2c.write(I2cAddr,tx,len+1); 00231 } 00232 00233 void I2cPeripherals::read_reg(int I2cAddr,char reg_addr,char* data,int len) 00234 { 00235 char tx[1]; 00236 tx[0] = reg_addr | 0x80; 00237 _i2c.write(I2cAddr,tx,1,true); 00238 _i2c.read(I2cAddr,data,len); 00239 } 00240 int I2cPeripherals::write_EEPROM(short reg_addr,char* data,int len) 00241 { 00242 char tx[3]; 00243 int rc=0,i; 00244 int I2cAddr = I2C_EEPROM_ADDR; 00245 for (i=0; i<len; i++) { 00246 tx[0] = reg_addr >> 8; 00247 tx[1] = reg_addr & 0xFF; 00248 tx[2] = data[i]; 00249 rc = _i2c.write(I2cAddr,tx,3); 00250 if ( rc ) break; 00251 wait(0.01); 00252 reg_addr ++; 00253 } 00254 return rc; 00255 } 00256 00257 int I2cPeripherals::read_EEPROM(short reg_addr,char* data,int len) 00258 { 00259 int I2cAddr = I2C_EEPROM_ADDR; 00260 int rc = _i2c.write(I2cAddr,(char*)& reg_addr,2,true); 00261 if ( rc ) return rc; 00262 rc = _i2c.read(I2cAddr,data,len); 00263 return rc; 00264 } 00265 00266 void I2cPeripherals::cls() 00267 { 00268 if ( LCD_addr == 0 ) return; 00269 char tx[2] = { 0x00, 0x01 }; 00270 _i2c.write(LCD_addr,tx,2); 00271 wait(0.001); 00272 } 00273 00274 void I2cPeripherals::locate(int clm,int row) 00275 { 00276 char tx[2]; 00277 00278 if ( LCD_addr == 0 ) return; 00279 tx[0] = 0x00; 00280 tx[1] = 0x80 + (row * 0x40) + clm; 00281 _i2c.write(LCD_addr,tx,2); 00282 wait(0.001); 00283 } 00284 00285 int I2cPeripherals::angular(float *x,float *y,float *z) 00286 { 00287 char rx[6]; 00288 char tx[1]; 00289 float i = 0; 00290 tx[0] = Gyro_data | 0x80; 00291 if ( _i2c.write(Gyro_addr,tx,1,true) != 0 ) { 00292 *x=*y=*z=0; 00293 return Gyro_addr; 00294 } 00295 switch ( Gyro_addr ) { 00296 case 0xD0: 00297 case 0xD2: 00298 i = 0.06098; 00299 break; 00300 case 0xD4: 00301 case 0xD6: 00302 i = 0.06957; 00303 } 00304 _i2c.read(Gyro_addr,rx,6); 00305 *x = ( short(rx[0] << 8 | (uint8_t)rx[1]) ) * i; 00306 *y = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * i; 00307 *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * i; 00308 return true; 00309 } 00310 00311 int I2cPeripherals::Acceleration(float *x,float *y,float *z) 00312 { 00313 //AXDL345 Data Read 00314 char rx[6]; 00315 char tx[1]; 00316 float lsb; 00317 tx[0] = Accel_data | 0x80; 00318 if ( _i2c.write(Accel_addr,tx,1,true) != 0 ) { 00319 *x=*y=0; 00320 *z=1; 00321 return false; 00322 } 00323 _i2c.read(Accel_addr,rx,6); 00324 switch ( Accel_addr ) { 00325 case 0xD0: 00326 case 0xD2: 00327 #ifdef MPU6050 00328 lsb = 0.000488; //16g 00329 // lsb = 0.000244; //8g 00330 // lsb = 0.000122; //4g 00331 // lsb = 0.000061; //2g 00332 *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb;//re 00333 *x = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * lsb; 00334 *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * lsb; 00335 #endif 00336 break; 00337 case 0xA6: 00338 case 0x3A: 00339 #ifdef ADXL345 00340 lsb = 0.004; 00341 *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * lsb;//re 00342 *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * lsb; 00343 *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * lsb; 00344 break; 00345 #endif 00346 } 00347 return true; 00348 } 00349 00350 int I2cPeripherals::pressure() 00351 { 00352 char tx[2]; 00353 char rx[3]; 00354 int press = 0; 00355 00356 tx[0]= 0x28 | 0x80; 00357 if ( _i2c.write(barometor_addr,tx,1,true) != 0 ) 00358 return 0; 00359 _i2c.read (barometor_addr,rx,3); 00360 press =int( rx[2]<<16 | rx[1]<<8 | rx[0] ); 00361 return press; 00362 } 00363 00364 int I2cPeripherals::temperature() 00365 { 00366 char tx[1]; 00367 char rx[2]; 00368 int temp; 00369 00370 tx[0]= 0x2B | 0x80; 00371 if ( _i2c.write(barometor_addr,tx,1,true) != 0 ) 00372 return 0; 00373 _i2c.read (barometor_addr,rx,2); 00374 00375 temp = short(rx[1]<<8 | rx[0]); 00376 return temp; 00377 } 00378 00379 float I2cPeripherals::height_cm() 00380 { 00381 return (float)ultrasonic(0x52)*0.0175f; 00382 } 00383 float I2cPeripherals::height_mm() 00384 { 00385 return (float)ultrasonic(0x52)*0.175f; 00386 } 00387 00388 float I2cPeripherals::height_us() 00389 { 00390 return (float)ultrasonic(0x52)/1000000; 00391 } 00392 00393 int I2cPeripherals::ultrasonic(char unit) 00394 { 00395 char rx[2],tx[2]; 00396 tx[0] = 0x00; 00397 if ( ultrasonic_addr == 0 ) return -1; 00398 _i2c.write(ultrasonic_addr,tx,1,true); 00399 _i2c.read (ultrasonic_addr,rx,1); 00400 if ( rx[0] != 0xFF ) { 00401 tx[0]= 0x02; 00402 _i2c.write(ultrasonic_addr,tx,1,true); 00403 _i2c.read (ultrasonic_addr,rx,2); 00404 ultrasonic_distance = short(rx[0] << 8 | (uint8_t)rx[1]); 00405 tx[0] = 0x00; 00406 tx[1] = unit; 00407 _i2c.write(ultrasonic_addr,tx,2); 00408 } 00409 return ultrasonic_distance; 00410 00411 } 00412 00413 void I2cPeripherals::i2c_write(int i2caddr,char* tx,int len) 00414 { 00415 char rx[1]; 00416 rx[0] = 0; 00417 if ( i2caddr == 0 ) return; 00418 while( 1 ) { 00419 _i2c.write(i2caddr,tx,len); 00420 _i2c.write(i2caddr,tx,1,true); 00421 _i2c.read (i2caddr,rx,1); 00422 if ( tx[1] == rx[0] ) return; 00423 } 00424 } 00425 00426 int I2cPeripherals::GetAddr(int type) 00427 { 00428 switch ( type ) { 00429 case GYRO_ADDR: 00430 return Gyro_addr; 00431 case ACCEL_ADDR: 00432 return Accel_addr; 00433 case LCD_ADDR: 00434 return LCD_addr; 00435 case BARO_ADDR: 00436 return barometor_addr; 00437 case ULTRASONIC_ADDR: 00438 return ultrasonic_addr; 00439 } 00440 return 0; 00441 } 00442 ; 00443 00444 00445 00446 00447 00448 00449 00450
Generated on Wed Jul 13 2022 09:20:34 by
1.7.2