syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

I2cPeripherals/I2cPeripherals.cpp@8:1db19b529b22, 2021-02-21 (annotated)

Committer:
komaida424
Date:
Sun Feb 21 05:14:57 2021 +0000
Revision:
8:1db19b529b22
Parent:
7:16bf0085d914 
rev 020

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