syouichi imamori / Mbed OS 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.

Dependencies:   IAP

I2cPeripherals/I2cPeripherals.cpp@7:16bf0085d914, 2015-04-28 (annotated)

Committer:
komaida424
Date:
Tue Apr 28 01:48:21 2015 +0000
Revision:
7:16bf0085d914
Parent:
4:4060309b9cc0
Child:
8:1db19b529b22
rev.3.11   bug fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komaida424 0:cca1c4e84da4 1 #include "mbed.h"
komaida424 0:cca1c4e84da4 2 #include "I2cPeripherals.h"
komaida424 2:59ac9df97701 3 //Serial pc(USBTX, USBRX);
komaida424 0:cca1c4e84da4 4 I2cPeripherals::I2cPeripherals(PinName sda, PinName scl): _i2c(sda,scl)
komaida424 2:59ac9df97701 5 {
komaida424 2:59ac9df97701 6
komaida424 3:27407c4984cf 7 _i2c.frequency(200000);
komaida424 2:59ac9df97701 8 // LCD_contrast = 60;
komaida424 2:59ac9df97701 9 wait(0.5);
komaida424 2:59ac9df97701 10 LCD_addr = 0;
komaida424 2:59ac9df97701 11 Gyro_addr = 0;
komaida424 2:59ac9df97701 12 Accel_addr = 0;
komaida424 2:59ac9df97701 13 barometor_addr = 0;
komaida424 4:4060309b9cc0 14 ultrasonic_addr = 0;
komaida424 4:4060309b9cc0 15 ultrasonic_distance = 0;
komaida424 4:4060309b9cc0 16 //pc.printf("start");
komaida424 2:59ac9df97701 17 find();
komaida424 2:59ac9df97701 18 start();
komaida424 2:59ac9df97701 19
komaida424 0:cca1c4e84da4 20 }
komaida424 0:cca1c4e84da4 21
komaida424 2:59ac9df97701 22 void I2cPeripherals::start(int contrast)
komaida424 0:cca1c4e84da4 23 {
komaida424 2:59ac9df97701 24 char tx[2];
komaida424 2:59ac9df97701 25 // find(); // I2C interface sensor detect
komaida424 2:59ac9df97701 26 #ifdef ST7032
komaida424 2:59ac9df97701 27 LCD_addr = 0x7c;
komaida424 2:59ac9df97701 28 LCD_data = 0x40;
komaida424 2:59ac9df97701 29 tx[0] = 0x00;
komaida424 2:59ac9df97701 30 tx[1] = 0x38;
komaida424 2:59ac9df97701 31 if ( _i2c.write(LCD_addr,tx,2) == 0 ) {
komaida424 2:59ac9df97701 32 tx[1] = 0x39;
komaida424 2:59ac9df97701 33 _i2c.write(LCD_addr,tx,2);
komaida424 2:59ac9df97701 34 tx[1] = 0x14; //1F
komaida424 2:59ac9df97701 35 _i2c.write(LCD_addr,tx,2);
komaida424 2:59ac9df97701 36 tx[1] = 0x70 | (contrast & 0x0F);
komaida424 2:59ac9df97701 37 _i2c.write(LCD_addr,tx,2);
komaida424 2:59ac9df97701 38 tx[1] = 0x5C | ((contrast >> 4) & 0x03);
komaida424 2:59ac9df97701 39 _i2c.write(LCD_addr,tx,2);
komaida424 7:16bf0085d914 40 tx[1] = 0x6C;
komaida424 2:59ac9df97701 41 _i2c.write(LCD_addr,tx,2);
komaida424 2:59ac9df97701 42 wait(0.3);
komaida424 2:59ac9df97701 43 tx[1] = 0x0C;
komaida424 2:59ac9df97701 44 _i2c.write(LCD_addr,tx,2); // display on
komaida424 2:59ac9df97701 45 tx[1] = 0x06;
komaida424 2:59ac9df97701 46 _i2c.write(LCD_addr,tx,2);
komaida424 2:59ac9df97701 47 // tx[0] = 0x40;
komaida424 2:59ac9df97701 48 // tx[1] = '*';
komaida424 2:59ac9df97701 49 // _i2c.write(LCD_addr,tx,2); // Display clear
komaida424 2:59ac9df97701 50 return;
komaida424 2:59ac9df97701 51 }
komaida424 2:59ac9df97701 52 #endif
komaida424 2:59ac9df97701 53 #ifdef ACM1602
komaida424 2:59ac9df97701 54 LCD_addr = 0xA0;
komaida424 2:59ac9df97701 55 LCD_data = 0x80;
komaida424 2:59ac9df97701 56 tx[0] = 0x00;
komaida424 2:59ac9df97701 57 tx[1] = 0x01;
komaida424 2:59ac9df97701 58 if ( _i2c.write(LCD_addr,tx,2) == 0 ) {
komaida424 2:59ac9df97701 59 wait(0.005);
komaida424 2:59ac9df97701 60 tx[1] = 0x38;
komaida424 2:59ac9df97701 61 _i2c.write(LCD_addr,tx,2);
komaida424 2:59ac9df97701 62 wait(0.005);
komaida424 2:59ac9df97701 63 tx[1] = 0x0F;
komaida424 2:59ac9df97701 64 _i2c.write(LCD_addr,tx,2);
komaida424 2:59ac9df97701 65 wait(0.005);
komaida424 2:59ac9df97701 66 tx[1] = 0x06;
komaida424 2:59ac9df97701 67 _i2c.write(LCD_addr,tx,2);
komaida424 2:59ac9df97701 68 wait(0.005);
komaida424 2:59ac9df97701 69 return;
komaida424 2:59ac9df97701 70 }
komaida424 2:59ac9df97701 71 #endif
komaida424 2:59ac9df97701 72 LCD_addr = 0;
komaida424 0:cca1c4e84da4 73 }
komaida424 0:cca1c4e84da4 74
komaida424 2:59ac9df97701 75 void I2cPeripherals::find()
komaida424 0:cca1c4e84da4 76 {
komaida424 4:4060309b9cc0 77 #define sensnum 11
komaida424 0:cca1c4e84da4 78 char tx[2];
komaida424 2:59ac9df97701 79 char rx[1];
komaida424 4:4060309b9cc0 80 SensorInf sens[sensnum] = { 0xD0,0x68,0x00, //ITG3200 gyro
komaida424 2:59ac9df97701 81 0xD2,0x68,0x00,
komaida424 4:4060309b9cc0 82 0xD0,0x68,0x75, //MPU6050 gyro/accel
komaida424 2:59ac9df97701 83 0xD2,0x68,0x75,
komaida424 4:4060309b9cc0 84 0xD4,0xD4,0x0F, //L3GD20 gyro
komaida424 2:59ac9df97701 85 0xD6,0xD4,0x0F,
komaida424 4:4060309b9cc0 86 0xA6,0xE5,0x00, //ADXL345 accel
komaida424 2:59ac9df97701 87 0x3A,0xE5,0x00,
komaida424 4:4060309b9cc0 88 0xB8,0xBB,0x0F, //LPS331 baro
komaida424 4:4060309b9cc0 89 0xBA,0xBB,0x0F,
komaida424 4:4060309b9cc0 90 0xE0,0x18,0x01 //SRF02 ultrasonic
komaida424 4:4060309b9cc0 91 };
komaida424 0:cca1c4e84da4 92
komaida424 4:4060309b9cc0 93 for ( int num = 0; num < sensnum; num++ ) {
komaida424 2:59ac9df97701 94 tx[0]= sens[num].whoaddr;
komaida424 2:59ac9df97701 95 if ( _i2c.write(sens[num].addr,tx,1,true) != 0 ) continue;
komaida424 2:59ac9df97701 96 _i2c.read (sens[num].addr,rx,1);
komaida424 2:59ac9df97701 97 if ( (rx[0]&0x7E) != (sens[num].who&0x7E) ) continue;
komaida424 4:4060309b9cc0 98 //pc.printf("who=%2x",rx[0]);
komaida424 2:59ac9df97701 99 switch ( num ) {
komaida424 2:59ac9df97701 100 #ifdef ITG3200
komaida424 2:59ac9df97701 101 case 0: //ITG3200
komaida424 2:59ac9df97701 102 case 1:
komaida424 2:59ac9df97701 103 Gyro_addr = sens[num].addr;
komaida424 2:59ac9df97701 104 Gyro_data = 0x1D;
komaida424 2:59ac9df97701 105 tx[0] = 0x16;
komaida424 2:59ac9df97701 106 tx[1] = 0x18; // 0x1D
komaida424 2:59ac9df97701 107 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 108 tx[0] = 0x3E;
komaida424 2:59ac9df97701 109 tx[1] = 0x01;
komaida424 2:59ac9df97701 110 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 111 wait_ms(10);
komaida424 2:59ac9df97701 112 break;
komaida424 2:59ac9df97701 113 #endif
komaida424 2:59ac9df97701 114 #ifdef MPU6050
komaida424 2:59ac9df97701 115 case 2: //MPU6050
komaida424 2:59ac9df97701 116 case 3:
komaida424 2:59ac9df97701 117 Gyro_addr = sens[num].addr;
komaida424 2:59ac9df97701 118 Gyro_data = 0x43;
komaida424 2:59ac9df97701 119 Accel_addr = sens[num].addr;
komaida424 2:59ac9df97701 120 Accel_data = 0x3B;
komaida424 2:59ac9df97701 121 tx[0] = 0x6B;
komaida424 2:59ac9df97701 122 tx[1] = 0x00; // PWR on
komaida424 2:59ac9df97701 123 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 124 tx[0] = 0x1A; // DLPF(Digitel low pass filter)
komaida424 2:59ac9df97701 125 tx[1] = 0x02; // set 0 to 7
komaida424 2:59ac9df97701 126 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 127 wait(0.01);
komaida424 2:59ac9df97701 128 tx[0] = 0x1B;
komaida424 2:59ac9df97701 129 tx[1] = 0x18; // +-2000deg
komaida424 2:59ac9df97701 130 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 131 wait(0.01);
komaida424 2:59ac9df97701 132 tx[0] = 0x1C;
komaida424 3:27407c4984cf 133 tx[1] = 0x18; // 00:2g,08:4g,10:8g,18:+-16g
komaida424 2:59ac9df97701 134 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 135 wait_ms(10);
komaida424 2:59ac9df97701 136 break;
komaida424 2:59ac9df97701 137 #endif
komaida424 2:59ac9df97701 138 #ifdef L3GD20
komaida424 2:59ac9df97701 139 case 4: //L3GD20
komaida424 2:59ac9df97701 140 case 5:
komaida424 2:59ac9df97701 141 Gyro_addr = sens[num].addr;
komaida424 2:59ac9df97701 142 Gyro_data = 0x28;
komaida424 2:59ac9df97701 143 tx[0] = 0x20;
komaida424 2:59ac9df97701 144 tx[1] = 0xBF; //rate 400Hz 0x8F-0xBF
komaida424 2:59ac9df97701 145 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 146 tx[0] = 0x21;
komaida424 2:59ac9df97701 147 tx[1] = 0x09;
komaida424 2:59ac9df97701 148 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 149 tx[0] = 0x23;
komaida424 2:59ac9df97701 150 tx[1] = 0xF0;
komaida424 2:59ac9df97701 151 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 152 tx[0] = 0x24;
komaida424 2:59ac9df97701 153 tx[1] = 0x10;
komaida424 2:59ac9df97701 154 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 155 break;
komaida424 2:59ac9df97701 156 #endif
komaida424 2:59ac9df97701 157 #ifdef ADXL345
komaida424 2:59ac9df97701 158 case 6: //ADXL345
komaida424 2:59ac9df97701 159 case 7:
komaida424 2:59ac9df97701 160 Accel_addr = sens[num].addr;
komaida424 2:59ac9df97701 161 Accel_data = 0x32;
komaida424 2:59ac9df97701 162 tx[0] = 0x2D;
komaida424 2:59ac9df97701 163 tx[1] = 0x00;
komaida424 2:59ac9df97701 164 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 165 tx[0] = 0x31;
komaida424 2:59ac9df97701 166 tx[1] = 0x0B; //full range 08:2g 09:4g 0A:8g 0B:16g
komaida424 2:59ac9df97701 167 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 168 tx[0] = 0x2C;
komaida424 2:59ac9df97701 169 tx[1] = 0x09; //out rate 0xC:400Hz 0xD:800Hz
komaida424 2:59ac9df97701 170 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 171 tx[0] = 0x2D;
komaida424 2:59ac9df97701 172 tx[1] = 0x08;
komaida424 2:59ac9df97701 173 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 174 break;
komaida424 2:59ac9df97701 175 #endif
komaida424 2:59ac9df97701 176 #ifdef LPS331AP
komaida424 2:59ac9df97701 177 case 8: //barometor
komaida424 2:59ac9df97701 178 case 9:
komaida424 2:59ac9df97701 179 barometor_addr = sens[num].addr;
komaida424 2:59ac9df97701 180 barometor_data = 0x2B;
komaida424 2:59ac9df97701 181 tx[0] = 0x10; //RES_CNF
komaida424 2:59ac9df97701 182 tx[1] = 0x7A;
komaida424 2:59ac9df97701 183 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 184 tx[0] = 0x20; // CTL_REG1
komaida424 2:59ac9df97701 185 tx[1] = 0xB0; // power on
komaida424 2:59ac9df97701 186 _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 187 // tx[0]= 0x21; // CTL_REG2
komaida424 2:59ac9df97701 188 // tx[1]= 0x01; // one shot start
komaida424 2:59ac9df97701 189 // _i2c.write(sens[num].addr,tx,2);
komaida424 2:59ac9df97701 190 wait(1);
komaida424 2:59ac9df97701 191 break;
komaida424 2:59ac9df97701 192 #endif
komaida424 4:4060309b9cc0 193 case 10: //SFR02 ultrasonic
komaida424 4:4060309b9cc0 194 ultrasonic_addr = sens[num].addr;
komaida424 4:4060309b9cc0 195 ultrasonic_data = 0x02;
komaida424 4:4060309b9cc0 196 tx[0] = 0x00;
komaida424 4:4060309b9cc0 197 tx[1] = 0x52;
komaida424 4:4060309b9cc0 198 _i2c.write(sens[num].addr,tx,2);
komaida424 4:4060309b9cc0 199 wait(0.01);
komaida424 4:4060309b9cc0 200 break;
komaida424 2:59ac9df97701 201 }
komaida424 2:59ac9df97701 202 }
komaida424 0:cca1c4e84da4 203 }
komaida424 0:cca1c4e84da4 204
komaida424 2:59ac9df97701 205 int I2cPeripherals::_putc(int value)
komaida424 2:59ac9df97701 206 {
komaida424 2:59ac9df97701 207 if ( LCD_addr == 0 ) return value;
komaida424 0:cca1c4e84da4 208 _i2c.start();
komaida424 0:cca1c4e84da4 209 _i2c.write(LCD_addr);
komaida424 0:cca1c4e84da4 210 _i2c.write(LCD_data);
komaida424 0:cca1c4e84da4 211 _i2c.write(value);
komaida424 0:cca1c4e84da4 212 _i2c.stop();
komaida424 0:cca1c4e84da4 213 return value;
komaida424 0:cca1c4e84da4 214 }
komaida424 0:cca1c4e84da4 215
komaida424 2:59ac9df97701 216 int I2cPeripherals::_getc()
komaida424 2:59ac9df97701 217 {
komaida424 0:cca1c4e84da4 218 return -1;
komaida424 0:cca1c4e84da4 219 }
komaida424 0:cca1c4e84da4 220
komaida424 4:4060309b9cc0 221 void I2cPeripherals::write_reg(int I2cAddr,char reg_addr,char* data,int len)
komaida424 2:59ac9df97701 222 {
komaida424 2:59ac9df97701 223 char tx[17];
komaida424 2:59ac9df97701 224 if ( len >16 ) len = 16;
komaida424 2:59ac9df97701 225 tx[0] = reg_addr;
komaida424 2:59ac9df97701 226 if ( len > 1 ) tx[0] |= 0x80;
komaida424 2:59ac9df97701 227 for (int i=0; i<len; i++) tx[i+1] = data[i];
komaida424 2:59ac9df97701 228 _i2c.write(I2cAddr,tx,len+1);
komaida424 2:59ac9df97701 229 }
komaida424 2:59ac9df97701 230
komaida424 4:4060309b9cc0 231 void I2cPeripherals::read_reg(int I2cAddr,char reg_addr,char* data,int len)
komaida424 2:59ac9df97701 232 {
komaida424 2:59ac9df97701 233 char tx[1];
komaida424 2:59ac9df97701 234 tx[0] = reg_addr | 0x80;
komaida424 2:59ac9df97701 235 _i2c.write(I2cAddr,tx,1,true);
komaida424 2:59ac9df97701 236 _i2c.read(I2cAddr,data,len);
komaida424 2:59ac9df97701 237 }
komaida424 4:4060309b9cc0 238 int I2cPeripherals::write_EEPROM(short reg_addr,char* data,int len)
komaida424 4:4060309b9cc0 239 {
komaida424 4:4060309b9cc0 240 char tx[3];
komaida424 4:4060309b9cc0 241 int rc=0,i;
komaida424 4:4060309b9cc0 242 int I2cAddr = I2C_EEPROM_ADDR;
komaida424 4:4060309b9cc0 243 for (i=0; i<len; i++) {
komaida424 4:4060309b9cc0 244 tx[0] = reg_addr >> 8;
komaida424 4:4060309b9cc0 245 tx[1] = reg_addr & 0xFF;
komaida424 4:4060309b9cc0 246 tx[2] = data[i];
komaida424 4:4060309b9cc0 247 rc = _i2c.write(I2cAddr,tx,3);
komaida424 4:4060309b9cc0 248 if ( rc ) break;
komaida424 4:4060309b9cc0 249 wait(0.01);
komaida424 4:4060309b9cc0 250 reg_addr ++;
komaida424 4:4060309b9cc0 251 }
komaida424 4:4060309b9cc0 252 return rc;
komaida424 4:4060309b9cc0 253 }
komaida424 4:4060309b9cc0 254
komaida424 4:4060309b9cc0 255 int I2cPeripherals::read_EEPROM(short reg_addr,char* data,int len)
komaida424 4:4060309b9cc0 256 {
komaida424 4:4060309b9cc0 257 int I2cAddr = I2C_EEPROM_ADDR;
komaida424 4:4060309b9cc0 258 int rc = _i2c.write(I2cAddr,(char*)& reg_addr,2,true);
komaida424 4:4060309b9cc0 259 if ( rc ) return rc;
komaida424 4:4060309b9cc0 260 rc = _i2c.read(I2cAddr,data,len);
komaida424 4:4060309b9cc0 261 return rc;
komaida424 4:4060309b9cc0 262 }
komaida424 2:59ac9df97701 263
komaida424 0:cca1c4e84da4 264 void I2cPeripherals::cls()
komaida424 0:cca1c4e84da4 265 {
komaida424 2:59ac9df97701 266 if ( LCD_addr == 0 ) return;
komaida424 0:cca1c4e84da4 267 char tx[2] = { 0x00, 0x01 };
komaida424 0:cca1c4e84da4 268 _i2c.write(LCD_addr,tx,2);
komaida424 4:4060309b9cc0 269 wait(0.001);
komaida424 0:cca1c4e84da4 270 }
komaida424 0:cca1c4e84da4 271
komaida424 0:cca1c4e84da4 272 void I2cPeripherals::locate(int clm,int row)
komaida424 0:cca1c4e84da4 273 {
komaida424 0:cca1c4e84da4 274 char tx[2];
komaida424 2:59ac9df97701 275
komaida424 2:59ac9df97701 276 if ( LCD_addr == 0 ) return;
komaida424 0:cca1c4e84da4 277 tx[0] = 0x00;
komaida424 0:cca1c4e84da4 278 tx[1] = 0x80 + (row * 0x40) + clm;
komaida424 0:cca1c4e84da4 279 _i2c.write(LCD_addr,tx,2);
komaida424 4:4060309b9cc0 280 wait(0.001);
komaida424 0:cca1c4e84da4 281 }
komaida424 0:cca1c4e84da4 282
komaida424 2:59ac9df97701 283 int I2cPeripherals::angular(float *x,float *y,float *z)
komaida424 2:59ac9df97701 284 {
komaida424 0:cca1c4e84da4 285 char rx[6];
komaida424 0:cca1c4e84da4 286 char tx[1];
komaida424 2:59ac9df97701 287 float i = 0;
komaida424 0:cca1c4e84da4 288 tx[0] = Gyro_data | 0x80;
komaida424 0:cca1c4e84da4 289 if ( _i2c.write(Gyro_addr,tx,1,true) != 0 ) {
komaida424 2:59ac9df97701 290 *x=*y=*z=0;
komaida424 2:59ac9df97701 291 return Gyro_addr;
komaida424 2:59ac9df97701 292 }
komaida424 2:59ac9df97701 293 switch ( Gyro_addr ) {
komaida424 2:59ac9df97701 294 case 0xD0:
komaida424 2:59ac9df97701 295 case 0xD2:
komaida424 2:59ac9df97701 296 i = 0.06098;
komaida424 2:59ac9df97701 297 break;
komaida424 2:59ac9df97701 298 case 0xD4:
komaida424 2:59ac9df97701 299 case 0xD6:
komaida424 2:59ac9df97701 300 i = 0.06957;
komaida424 0:cca1c4e84da4 301 }
komaida424 0:cca1c4e84da4 302 _i2c.read(Gyro_addr,rx,6);
komaida424 2:59ac9df97701 303 *x = ( short(rx[0] << 8 | (uint8_t)rx[1]) ) * i;
komaida424 2:59ac9df97701 304 *y = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * i;
komaida424 2:59ac9df97701 305 *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * i;
komaida424 0:cca1c4e84da4 306 return true;
komaida424 0:cca1c4e84da4 307 }
komaida424 2:59ac9df97701 308
komaida424 2:59ac9df97701 309 int I2cPeripherals::Acceleration(float *x,float *y,float *z)
komaida424 0:cca1c4e84da4 310 {
komaida424 2:59ac9df97701 311 //AXDL345 Data Read
komaida424 0:cca1c4e84da4 312 char rx[6];
komaida424 0:cca1c4e84da4 313 char tx[1];
komaida424 3:27407c4984cf 314 float lsb;
komaida424 0:cca1c4e84da4 315 tx[0] = Accel_data | 0x80;
komaida424 0:cca1c4e84da4 316 if ( _i2c.write(Accel_addr,tx,1,true) != 0 ) {
komaida424 2:59ac9df97701 317 *x=*y=0;
komaida424 4:4060309b9cc0 318 *z=1;
komaida424 0:cca1c4e84da4 319 return false;
komaida424 0:cca1c4e84da4 320 }
komaida424 0:cca1c4e84da4 321 _i2c.read(Accel_addr,rx,6);
komaida424 2:59ac9df97701 322 switch ( Accel_addr ) {
komaida424 2:59ac9df97701 323 case 0xD0:
komaida424 2:59ac9df97701 324 case 0xD2:
komaida424 2:59ac9df97701 325 #ifdef MPU6050
komaida424 3:27407c4984cf 326 lsb = 0.000488; //16g
komaida424 4:4060309b9cc0 327 // lsb = 0.000244; //8g
komaida424 4:4060309b9cc0 328 // lsb = 0.000122; //4g
komaida424 4:4060309b9cc0 329 // lsb = 0.000061; //2g
komaida424 4:4060309b9cc0 330 *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb;//re
komaida424 3:27407c4984cf 331 *x = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * lsb;
komaida424 3:27407c4984cf 332 *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * lsb;
komaida424 2:59ac9df97701 333 #endif
komaida424 2:59ac9df97701 334 break;
komaida424 2:59ac9df97701 335 case 0xA6:
komaida424 2:59ac9df97701 336 case 0x3A:
komaida424 2:59ac9df97701 337 #ifdef ADXL345
komaida424 4:4060309b9cc0 338 lsb = 0.004;
komaida424 4:4060309b9cc0 339 *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * lsb;//re
komaida424 4:4060309b9cc0 340 *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * lsb;
komaida424 4:4060309b9cc0 341 *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * lsb;
komaida424 3:27407c4984cf 342 break;
komaida424 2:59ac9df97701 343 #endif
komaida424 2:59ac9df97701 344 }
komaida424 0:cca1c4e84da4 345 return true;
komaida424 0:cca1c4e84da4 346 }
komaida424 2:59ac9df97701 347
komaida424 0:cca1c4e84da4 348 int I2cPeripherals::pressure()
komaida424 0:cca1c4e84da4 349 {
komaida424 0:cca1c4e84da4 350 char tx[2];
komaida424 0:cca1c4e84da4 351 char rx[3];
komaida424 2:59ac9df97701 352 int press = 0;
komaida424 2:59ac9df97701 353
komaida424 2:59ac9df97701 354 tx[0]= 0x28 | 0x80;
komaida424 0:cca1c4e84da4 355 if ( _i2c.write(barometor_addr,tx,1,true) != 0 )
komaida424 0:cca1c4e84da4 356 return 0;
komaida424 2:59ac9df97701 357 _i2c.read (barometor_addr,rx,3);
komaida424 0:cca1c4e84da4 358 press =int( rx[2]<<16 | rx[1]<<8 | rx[0] );
komaida424 0:cca1c4e84da4 359 return press;
komaida424 0:cca1c4e84da4 360 }
komaida424 0:cca1c4e84da4 361
komaida424 0:cca1c4e84da4 362 int I2cPeripherals::temperature()
komaida424 0:cca1c4e84da4 363 {
komaida424 0:cca1c4e84da4 364 char tx[1];
komaida424 0:cca1c4e84da4 365 char rx[2];
komaida424 0:cca1c4e84da4 366 int temp;
komaida424 2:59ac9df97701 367
komaida424 0:cca1c4e84da4 368 tx[0]= 0x2B | 0x80;
komaida424 0:cca1c4e84da4 369 if ( _i2c.write(barometor_addr,tx,1,true) != 0 )
komaida424 0:cca1c4e84da4 370 return 0;
komaida424 2:59ac9df97701 371 _i2c.read (barometor_addr,rx,2);
komaida424 2:59ac9df97701 372
komaida424 0:cca1c4e84da4 373 temp = short(rx[1]<<8 | rx[0]);
komaida424 0:cca1c4e84da4 374 return temp;
komaida424 2:59ac9df97701 375 }
komaida424 2:59ac9df97701 376
komaida424 4:4060309b9cc0 377 float I2cPeripherals::height_cm()
komaida424 4:4060309b9cc0 378 {
komaida424 4:4060309b9cc0 379 return (float)ultrasonic(0x52)*0.0175f;
komaida424 4:4060309b9cc0 380 }
komaida424 4:4060309b9cc0 381 float I2cPeripherals::height_mm()
komaida424 4:4060309b9cc0 382 {
komaida424 4:4060309b9cc0 383 return (float)ultrasonic(0x52)*0.175f;
komaida424 4:4060309b9cc0 384 }
komaida424 4:4060309b9cc0 385
komaida424 4:4060309b9cc0 386 float I2cPeripherals::height_us()
komaida424 4:4060309b9cc0 387 {
komaida424 4:4060309b9cc0 388 return (float)ultrasonic(0x52)/1000000;
komaida424 4:4060309b9cc0 389 }
komaida424 4:4060309b9cc0 390
komaida424 4:4060309b9cc0 391 int I2cPeripherals::ultrasonic(char unit)
komaida424 4:4060309b9cc0 392 {
komaida424 4:4060309b9cc0 393 char rx[2],tx[2];
komaida424 4:4060309b9cc0 394 tx[0] = 0x00;
komaida424 4:4060309b9cc0 395 if ( ultrasonic_addr == 0 ) return -1;
komaida424 4:4060309b9cc0 396 _i2c.write(ultrasonic_addr,tx,1,true);
komaida424 4:4060309b9cc0 397 _i2c.read (ultrasonic_addr,rx,1);
komaida424 4:4060309b9cc0 398 if ( rx[0] != 0xFF ) {
komaida424 4:4060309b9cc0 399 tx[0]= 0x02;
komaida424 4:4060309b9cc0 400 _i2c.write(ultrasonic_addr,tx,1,true);
komaida424 4:4060309b9cc0 401 _i2c.read (ultrasonic_addr,rx,2);
komaida424 4:4060309b9cc0 402 ultrasonic_distance = short(rx[0] << 8 | (uint8_t)rx[1]);
komaida424 4:4060309b9cc0 403 tx[0] = 0x00;
komaida424 4:4060309b9cc0 404 tx[1] = unit;
komaida424 4:4060309b9cc0 405 _i2c.write(ultrasonic_addr,tx,2);
komaida424 4:4060309b9cc0 406 }
komaida424 4:4060309b9cc0 407 return ultrasonic_distance;
komaida424 4:4060309b9cc0 408
komaida424 4:4060309b9cc0 409 }
komaida424 4:4060309b9cc0 410
komaida424 2:59ac9df97701 411 void I2cPeripherals::i2c_write(int i2caddr,char* tx,int len)
komaida424 2:59ac9df97701 412 {
komaida424 2:59ac9df97701 413 char rx[1];
komaida424 2:59ac9df97701 414 rx[0] = 0;
komaida424 2:59ac9df97701 415 while( 1 ) {
komaida424 2:59ac9df97701 416 _i2c.write(i2caddr,tx,len);
komaida424 2:59ac9df97701 417 _i2c.write(i2caddr,tx,1,true);
komaida424 2:59ac9df97701 418 _i2c.read (i2caddr,rx,1);
komaida424 2:59ac9df97701 419 if ( tx[1] == rx[0] ) return;
komaida424 2:59ac9df97701 420 }
komaida424 2:59ac9df97701 421 }
komaida424 2:59ac9df97701 422
komaida424 2:59ac9df97701 423 int I2cPeripherals::GetAddr(int type)
komaida424 2:59ac9df97701 424 {
komaida424 2:59ac9df97701 425 switch ( type ) {
komaida424 2:59ac9df97701 426 case GYRO_ADDR:
komaida424 2:59ac9df97701 427 return Gyro_addr;
komaida424 2:59ac9df97701 428 case ACCEL_ADDR:
komaida424 2:59ac9df97701 429 return Accel_addr;
komaida424 2:59ac9df97701 430 case LCD_ADDR:
komaida424 2:59ac9df97701 431 return LCD_addr;
komaida424 2:59ac9df97701 432 case BARO_ADDR:
komaida424 2:59ac9df97701 433 return barometor_addr;
komaida424 4:4060309b9cc0 434 case ULTRASONIC_ADDR:
komaida424 4:4060309b9cc0 435 return ultrasonic_addr;
komaida424 2:59ac9df97701 436 }
komaida424 2:59ac9df97701 437 return 0;
komaida424 2:59ac9df97701 438 }
komaida424 2:59ac9df97701 439 ;
komaida424 0:cca1c4e84da4 440
komaida424 0:cca1c4e84da4 441
komaida424 2:59ac9df97701 442
komaida424 2:59ac9df97701 443
komaida424 2:59ac9df97701 444
komaida424 4:4060309b9cc0 445
komaida424 4:4060309b9cc0 446
komaida424 4:4060309b9cc0 447