v61_0

Committer:
gr66
Date:
Fri Aug 06 14:52:00 2021 +0000
Revision:
7:164a2086348d
Parent:
6:798481567563
V60

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gr66 6:798481567563 1 #include "LSM6DS33_GR1.h"
gr66 6:798481567563 2
gr66 6:798481567563 3 LSM6DS33::LSM6DS33(PinName sda, PinName scl, uint8_t xgAddr) : i2c(sda, scl)
gr66 6:798481567563 4 {
gr66 6:798481567563 5 // xgAddress will store the 7-bit I2C address, if using I2C.
gr66 6:798481567563 6 xgAddress = xgAddr;
gr66 6:798481567563 7
gr66 6:798481567563 8 // init gyro offset
gr66 6:798481567563 9 gx_off=0;
gr66 6:798481567563 10 gy_off=0;
gr66 6:798481567563 11 gz_off=0;
gr66 6:798481567563 12 gxol=0;
gr66 6:798481567563 13 gxoh=0;
gr66 6:798481567563 14 gyol=0;
gr66 6:798481567563 15 gyoh=0;
gr66 6:798481567563 16 gzol=0;
gr66 6:798481567563 17 gzoh=0;
gr66 6:798481567563 18 }
gr66 6:798481567563 19
gr66 6:798481567563 20 uint16_t LSM6DS33::begin(gyro_scale gScl, accel_scale aScl,
gr66 6:798481567563 21 gyro_odr gODR, accel_odr aODR)
gr66 6:798481567563 22 {
gr66 6:798481567563 23
gr66 6:798481567563 24 // Store the given scales in class variables. These scale variables
gr66 6:798481567563 25 // are used throughout to calculate the actual g's, DPS,and Gs's.
gr66 6:798481567563 26 gScale = gScl;
gr66 6:798481567563 27 aScale = aScl;
gr66 6:798481567563 28
gr66 6:798481567563 29 // Once we have the scale values, we can calculate the resolution
gr66 6:798481567563 30 // of each sensor. That's what these functions are for. One for each sensor
gr66 6:798481567563 31 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
gr66 6:798481567563 32 calcaRes(); // Calculate g / ADC tick, stored in aRes variable
gr66 6:798481567563 33
gr66 6:798481567563 34
gr66 6:798481567563 35 // To verify communication, we can read from the WHO_AM_I register of
gr66 6:798481567563 36 // each device. Store those in a variable so we can return them.
gr66 6:798481567563 37 // The start of the addresses we want to read from
gr66 6:798481567563 38 char cmd[2] = {
gr66 6:798481567563 39 WHO_AM_I_REG,
gr66 6:798481567563 40 0
gr66 6:798481567563 41 };
gr66 6:798481567563 42
gr66 6:798481567563 43 // Write the address we are going to read from and don't end the transaction
gr66 6:798481567563 44 i2c.write(xgAddress, cmd, 1, true);
gr66 6:798481567563 45 // Read in all the 8 bits of data
gr66 6:798481567563 46 i2c.read(xgAddress, cmd+1, 1);
gr66 6:798481567563 47 uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I
gr66 6:798481567563 48
gr66 6:798481567563 49 // Gyro initialization stuff:
gr66 6:798481567563 50 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
gr66 6:798481567563 51 setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
gr66 6:798481567563 52 setGyroScale(gScale); // Set the gyro range
gr66 6:798481567563 53
gr66 6:798481567563 54 // Accelerometer initialization stuff:
gr66 6:798481567563 55 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
gr66 6:798481567563 56 setAccelODR(aODR); // Set the accel data rate.
gr66 6:798481567563 57 setAccelScale(aScale); // Set the accel range.
gr66 6:798481567563 58
gr66 6:798481567563 59 //set high res timestamp where LSB is 25us
gr66 6:798481567563 60 cmd[0] = WAKE_UP_DUR;
gr66 6:798481567563 61 cmd[1] = 0x10;
gr66 6:798481567563 62 i2c.write(xgAddress, cmd, 2);
gr66 6:798481567563 63
gr66 6:798481567563 64
gr66 6:798481567563 65 // Once everything is initialized, return the WHO_AM_I registers we read:
gr66 6:798481567563 66 return xgTest;
gr66 6:798481567563 67 }
gr66 6:798481567563 68
gr66 6:798481567563 69 void LSM6DS33::initGyro()
gr66 6:798481567563 70 {
gr66 6:798481567563 71 char cmd[2] = {
gr66 6:798481567563 72 CTRL2_G,
gr66 6:798481567563 73 gScale | G_ODR_104
gr66 6:798481567563 74 };
gr66 6:798481567563 75
gr66 6:798481567563 76 // Write the data to the gyro control registers
gr66 6:798481567563 77 i2c.write(xgAddress, cmd, 2);
gr66 6:798481567563 78 }
gr66 6:798481567563 79
gr66 6:798481567563 80 void LSM6DS33::initAccel()
gr66 6:798481567563 81 {
gr66 6:798481567563 82 char cmd[4] = {
gr66 6:798481567563 83 CTRL1_XL,
gr66 6:798481567563 84 0x30
gr66 6:798481567563 85 };
gr66 6:798481567563 86
gr66 6:798481567563 87 // Write the data to the accel control registers
gr66 6:798481567563 88 i2c.write(xgAddress, cmd, 2);
gr66 6:798481567563 89 }
gr66 6:798481567563 90
gr66 6:798481567563 91 void LSM6DS33::initIntr()
gr66 6:798481567563 92 {
gr66 6:798481567563 93
gr66 6:798481567563 94 }
gr66 6:798481567563 95 //modif gr1
gr66 6:798481567563 96 void LSM6DS33::readAllraw()
gr66 6:798481567563 97 {
gr66 6:798481567563 98 // The data we are going to read from the temp/gyr/acc/timestamp
gr66 6:798481567563 99 //char data[14];//from 0x20 to 0x42
gr66 6:798481567563 100
gr66 6:798481567563 101 char data[14];
gr66 6:798481567563 102 char tsdata[3];
gr66 6:798481567563 103
gr66 6:798481567563 104 i2c.start();
gr66 6:798481567563 105 i2c.write(xgAddress);
gr66 6:798481567563 106 i2c.write(OUT_TEMP_L);
gr66 6:798481567563 107 i2c.start();
gr66 6:798481567563 108 i2c.write(xgAddress | 0x1);
gr66 6:798481567563 109 for(int i =0; i<13; i++) {
gr66 6:798481567563 110 data[i]=i2c.read(1);
gr66 6:798481567563 111 }
gr66 6:798481567563 112 data[13]=i2c.read(0);
gr66 6:798481567563 113 i2c.stop();
gr66 6:798481567563 114
gr66 6:798481567563 115 // Temperature is a 12-bit signed integer
gr66 7:164a2086348d 116 temperature_raw = data[0] | (data[1] << 8);
gr66 6:798481567563 117 gxl = data[2] ;
gr66 6:798481567563 118 gxh =data[3] ;
gr66 6:798481567563 119 gyl = data[4] ;
gr66 6:798481567563 120 gyh= data[5] ;
gr66 6:798481567563 121 gzl = data[6] ;
gr66 6:798481567563 122 gzh=data[7] ;
gr66 6:798481567563 123 axl= data[8] ;
gr66 6:798481567563 124 axh=data[9] ;
gr66 6:798481567563 125 ayl = data[10];
gr66 6:798481567563 126 ayh=data[11] ;
gr66 6:798481567563 127 azl = data[12] ;
gr66 6:798481567563 128 azh=data[13] ;
gr66 6:798481567563 129
gr66 6:798481567563 130 //i2c.start();
gr66 6:798481567563 131 // i2c.write(xgAddress);
gr66 6:798481567563 132 // i2c.write(TIMESTAMP0_REG);
gr66 6:798481567563 133 // i2c.start();
gr66 6:798481567563 134 // i2c.write(xgAddress | 0x1);
gr66 6:798481567563 135 // for(int i =0; i<3; i++) {
gr66 6:798481567563 136 // tsdata[i]=i2c.read(1);
gr66 6:798481567563 137 // }
gr66 6:798481567563 138 // tsdata[3]=i2c.read(0);
gr66 6:798481567563 139 //i2c.stop();
gr66 6:798481567563 140
gr66 6:798481567563 141 // time_raw = tsdata[0] | (tsdata[1] << 8) | (tsdata[2] << 16);
gr66 6:798481567563 142
gr66 6:798481567563 143
gr66 7:164a2086348d 144 temperature_c = (float)temperature_raw / 16.0 + 25.0;
gr66 6:798481567563 145 // gx = gx_raw * gRes;
gr66 6:798481567563 146 // gy = gy_raw * gRes;
gr66 6:798481567563 147 // gz = gz_raw * gRes;
gr66 6:798481567563 148 // ax = ax_raw * aRes;
gr66 6:798481567563 149 // ay = ay_raw * aRes;
gr66 6:798481567563 150 // az = az_raw * aRes;
gr66 6:798481567563 151 // time = time_raw*(0.000025);
gr66 6:798481567563 152
gr66 6:798481567563 153
gr66 6:798481567563 154 }
gr66 6:798481567563 155 //fin modif gr1
gr66 6:798481567563 156 void LSM6DS33::readAll()
gr66 6:798481567563 157 {
gr66 6:798481567563 158 // The data we are going to read from the temp/gyr/acc/timestamp
gr66 6:798481567563 159 //char data[14];//from 0x20 to 0x42
gr66 6:798481567563 160
gr66 6:798481567563 161 char data[14];
gr66 6:798481567563 162 char tsdata[3];
gr66 6:798481567563 163
gr66 6:798481567563 164 i2c.start();
gr66 6:798481567563 165 i2c.write(xgAddress);
gr66 6:798481567563 166 i2c.write(OUT_TEMP_L);
gr66 6:798481567563 167 i2c.start();
gr66 6:798481567563 168 i2c.write(xgAddress | 0x1);
gr66 6:798481567563 169 for(int i =0; i<13; i++) {
gr66 6:798481567563 170 data[i]=i2c.read(1);
gr66 6:798481567563 171 }
gr66 6:798481567563 172 data[13]=i2c.read(0);
gr66 6:798481567563 173 i2c.stop();
gr66 6:798481567563 174
gr66 6:798481567563 175 // Temperature is a 12-bit signed integer
gr66 6:798481567563 176 temperature_raw = data[0] | (data[1] << 8);
gr66 6:798481567563 177 gx_raw = data[2] | (data[3] << 8);
gr66 6:798481567563 178 gy_raw = data[4] | (data[5] << 8);
gr66 6:798481567563 179 gz_raw = data[6] | (data[7] << 8);
gr66 6:798481567563 180 ax_raw = data[8] | (data[9] << 8);
gr66 6:798481567563 181 ay_raw = data[10] | (data[11] << 8);
gr66 6:798481567563 182 az_raw = data[12] | (data[13] << 8);
gr66 6:798481567563 183
gr66 6:798481567563 184 i2c.start();
gr66 6:798481567563 185 i2c.write(xgAddress);
gr66 6:798481567563 186 i2c.write(TIMESTAMP0_REG);
gr66 6:798481567563 187 i2c.start();
gr66 6:798481567563 188 i2c.write(xgAddress | 0x1);
gr66 6:798481567563 189 for(int i =0; i<3; i++) {
gr66 6:798481567563 190 tsdata[i]=i2c.read(1);
gr66 6:798481567563 191 }
gr66 6:798481567563 192 tsdata[3]=i2c.read(0);
gr66 6:798481567563 193 i2c.stop();
gr66 6:798481567563 194
gr66 6:798481567563 195 time_raw = tsdata[0] | (tsdata[1] << 8) | (tsdata[2] << 16);
gr66 6:798481567563 196
gr66 6:798481567563 197
gr66 6:798481567563 198 temperature_c = (float)temperature_raw / 16.0 + 25.0;
gr66 6:798481567563 199 gx = gx_raw * gRes;
gr66 6:798481567563 200 gy = gy_raw * gRes;
gr66 6:798481567563 201 gz = gz_raw * gRes;
gr66 6:798481567563 202 ax = ax_raw * aRes;
gr66 6:798481567563 203 ay = ay_raw * aRes;
gr66 6:798481567563 204 az = az_raw * aRes;
gr66 6:798481567563 205 time = time_raw*(0.000025);
gr66 6:798481567563 206
gr66 6:798481567563 207
gr66 6:798481567563 208 }
gr66 6:798481567563 209
gr66 6:798481567563 210
gr66 6:798481567563 211 void LSM6DS33::readAccel()
gr66 6:798481567563 212 {
gr66 6:798481567563 213 // The data we are going to read from the accel
gr66 6:798481567563 214 char data[6];
gr66 6:798481567563 215
gr66 6:798481567563 216 // Set addresses
gr66 6:798481567563 217 char subAddressXL = OUTX_L_XL;
gr66 6:798481567563 218 char subAddressXH = OUTX_H_XL;
gr66 6:798481567563 219 char subAddressYL = OUTY_L_XL;
gr66 6:798481567563 220 char subAddressYH = OUTY_H_XL;
gr66 6:798481567563 221 char subAddressZL = OUTZ_L_XL;
gr66 6:798481567563 222 char subAddressZH = OUTZ_H_XL;
gr66 6:798481567563 223
gr66 6:798481567563 224 // Write the address we are going to read from and don't end the transaction
gr66 6:798481567563 225 i2c.write(xgAddress, &subAddressXL, 1, true);
gr66 6:798481567563 226 // Read in register containing the axes data and alocated to the correct index
gr66 6:798481567563 227 i2c.read(xgAddress, data, 1);
gr66 6:798481567563 228
gr66 6:798481567563 229 i2c.write(xgAddress, &subAddressXH, 1, true);
gr66 6:798481567563 230 i2c.read(xgAddress, (data + 1), 1);
gr66 6:798481567563 231 i2c.write(xgAddress, &subAddressYL, 1, true);
gr66 6:798481567563 232 i2c.read(xgAddress, (data + 2), 1);
gr66 6:798481567563 233 i2c.write(xgAddress, &subAddressYH, 1, true);
gr66 6:798481567563 234 i2c.read(xgAddress, (data + 3), 1);
gr66 6:798481567563 235 i2c.write(xgAddress, &subAddressZL, 1, true);
gr66 6:798481567563 236 i2c.read(xgAddress, (data + 4), 1);
gr66 6:798481567563 237 i2c.write(xgAddress, &subAddressZH, 1, true);
gr66 6:798481567563 238 i2c.read(xgAddress, (data + 5), 1);
gr66 6:798481567563 239
gr66 6:798481567563 240 // Reassemble the data and convert to g
gr66 6:798481567563 241 ax_raw = data[0] | (data[1] << 8);
gr66 6:798481567563 242 ay_raw = data[2] | (data[3] << 8);
gr66 6:798481567563 243 az_raw = data[4] | (data[5] << 8);
gr66 6:798481567563 244 ax = ax_raw * aRes;
gr66 6:798481567563 245 ay = ay_raw * aRes;
gr66 6:798481567563 246 az = az_raw * aRes;
gr66 6:798481567563 247 //gr
gr66 6:798481567563 248 axl= data[0] ;
gr66 6:798481567563 249 axh=data[1] ;
gr66 6:798481567563 250 ayl = data[2];
gr66 6:798481567563 251 ayh=data[3] ;
gr66 6:798481567563 252 azl = data[4] ;
gr66 6:798481567563 253 azh=data[5] ;
gr66 6:798481567563 254 }
gr66 6:798481567563 255
gr66 6:798481567563 256 void LSM6DS33::readIntr()
gr66 6:798481567563 257 {
gr66 6:798481567563 258 char data[1];
gr66 6:798481567563 259 char subAddress = TAP_SRC;
gr66 6:798481567563 260
gr66 6:798481567563 261 i2c.write(xgAddress, &subAddress, 1, true);
gr66 6:798481567563 262 i2c.read(xgAddress, data, 1);
gr66 6:798481567563 263
gr66 6:798481567563 264 intr = (float)data[0];
gr66 6:798481567563 265 }
gr66 6:798481567563 266
gr66 6:798481567563 267 void LSM6DS33::readTemp()
gr66 6:798481567563 268 {
gr66 6:798481567563 269 // The data we are going to read from the temp
gr66 6:798481567563 270 char data[2];
gr66 6:798481567563 271
gr66 6:798481567563 272 // Set addresses
gr66 6:798481567563 273 char subAddressL = OUT_TEMP_L;
gr66 6:798481567563 274 char subAddressH = OUT_TEMP_H;
gr66 6:798481567563 275
gr66 6:798481567563 276 // Write the address we are going to read from and don't end the transaction
gr66 6:798481567563 277 i2c.write(xgAddress, &subAddressL, 1, true);
gr66 6:798481567563 278 // Read in register containing the temperature data and alocated to the correct index
gr66 6:798481567563 279 i2c.read(xgAddress, data, 1);
gr66 6:798481567563 280
gr66 6:798481567563 281 i2c.write(xgAddress, &subAddressH, 1, true);
gr66 6:798481567563 282 i2c.read(xgAddress, (data + 1), 1);
gr66 6:798481567563 283
gr66 6:798481567563 284 // Temperature is a 12-bit signed integer
gr66 6:798481567563 285 temperature_raw = data[0] | (data[1] << 8);
gr66 6:798481567563 286
gr66 6:798481567563 287 temperature_c = (float)temperature_raw / 16.0 + 25.0;
gr66 6:798481567563 288 temperature_f = temperature_c * 1.8 + 32.0;
gr66 6:798481567563 289 }
gr66 6:798481567563 290
gr66 6:798481567563 291
gr66 6:798481567563 292 void LSM6DS33::readGyro()
gr66 6:798481567563 293 {
gr66 6:798481567563 294 // The data we are going to read from the gyro
gr66 6:798481567563 295 char data[6];
gr66 6:798481567563 296
gr66 6:798481567563 297 // Set addresses
gr66 6:798481567563 298 char subAddressXL = OUTX_L_G;
gr66 6:798481567563 299 char subAddressXH = OUTX_H_G;
gr66 6:798481567563 300 char subAddressYL = OUTY_L_G;
gr66 6:798481567563 301 char subAddressYH = OUTY_H_G;
gr66 6:798481567563 302 char subAddressZL = OUTZ_L_G;
gr66 6:798481567563 303 char subAddressZH = OUTZ_H_G;
gr66 6:798481567563 304
gr66 6:798481567563 305 // Write the address we are going to read from and don't end the transaction
gr66 6:798481567563 306 i2c.write(xgAddress, &subAddressXL, 1, true);
gr66 6:798481567563 307 // Read in register containing the axes data and alocated to the correct index
gr66 6:798481567563 308 i2c.read(xgAddress, data, 1);
gr66 6:798481567563 309
gr66 6:798481567563 310 i2c.write(xgAddress, &subAddressXH, 1, true);
gr66 6:798481567563 311 i2c.read(xgAddress, (data + 1), 1);
gr66 6:798481567563 312 i2c.write(xgAddress, &subAddressYL, 1, true);
gr66 6:798481567563 313 i2c.read(xgAddress, (data + 2), 1);
gr66 6:798481567563 314 i2c.write(xgAddress, &subAddressYH, 1, true);
gr66 6:798481567563 315 i2c.read(xgAddress, (data + 3), 1);
gr66 6:798481567563 316 i2c.write(xgAddress, &subAddressZL, 1, true);
gr66 6:798481567563 317 i2c.read(xgAddress, (data + 4), 1);
gr66 6:798481567563 318 i2c.write(xgAddress, &subAddressZH, 1, true);
gr66 6:798481567563 319 i2c.read(xgAddress, (data + 5), 1);
gr66 6:798481567563 320
gr66 6:798481567563 321 // Reassemble the data and convert to degrees/sec
gr66 6:798481567563 322 gx_raw = data[0] | (data[1] << 8);
gr66 6:798481567563 323 gy_raw = data[2] | (data[3] << 8);
gr66 6:798481567563 324 gz_raw = data[4] | (data[5] << 8);
gr66 6:798481567563 325 gx = gx_raw * gRes;
gr66 6:798481567563 326 gy = gy_raw * gRes;
gr66 6:798481567563 327 gz = gz_raw * gRes;
gr66 6:798481567563 328 // gr
gr66 6:798481567563 329 gxl = data[0] ;
gr66 6:798481567563 330 gxh =data[1] ;
gr66 6:798481567563 331 gyl = data[2] ;
gr66 6:798481567563 332 gyh= data[3] ;
gr66 6:798481567563 333 gzl = data[4] ;
gr66 6:798481567563 334 gzh=data[5] ;
gr66 6:798481567563 335 }
gr66 6:798481567563 336
gr66 6:798481567563 337 void LSM6DS33::setGyroScale(gyro_scale gScl)
gr66 6:798481567563 338 {
gr66 6:798481567563 339 // The start of the addresses we want to read from
gr66 6:798481567563 340 char cmd[2] = {
gr66 6:798481567563 341 CTRL2_G,
gr66 6:798481567563 342 0
gr66 6:798481567563 343 };
gr66 6:798481567563 344
gr66 6:798481567563 345 // Write the address we are going to read from and don't end the transaction
gr66 6:798481567563 346 i2c.write(xgAddress, cmd, 1, true);
gr66 6:798481567563 347 // Read in all the 8 bits of data
gr66 6:798481567563 348 i2c.read(xgAddress, cmd+1, 1);
gr66 6:798481567563 349
gr66 6:798481567563 350 // Then mask out the gyro scale bits:
gr66 6:798481567563 351 cmd[1] &= 0xFF^(0x7 << 1); //// << 2 au lieu de 3
gr66 6:798481567563 352 // Then shift in our new scale bits:
gr66 6:798481567563 353 cmd[1] |= gScl << 1; //// << 0 au lieu de 3
gr66 6:798481567563 354
gr66 6:798481567563 355 // Write the gyroscale out to the gyro
gr66 6:798481567563 356 i2c.write(xgAddress, cmd, 2);
gr66 6:798481567563 357
gr66 6:798481567563 358 // We've updated the sensor, but we also need to update our class variables
gr66 6:798481567563 359 // First update gScale:
gr66 6:798481567563 360 gScale = gScl;
gr66 6:798481567563 361 // Then calculate a new gRes, which relies on gScale being set correctly:
gr66 6:798481567563 362 calcgRes();
gr66 6:798481567563 363 }
gr66 6:798481567563 364
gr66 6:798481567563 365 void LSM6DS33::setAccelScale(accel_scale aScl)
gr66 6:798481567563 366 {
gr66 6:798481567563 367 // The start of the addresses we want to read from
gr66 6:798481567563 368 char cmd[2] = {
gr66 6:798481567563 369 CTRL1_XL,
gr66 6:798481567563 370 0
gr66 6:798481567563 371 };
gr66 6:798481567563 372
gr66 6:798481567563 373 // Write the address we are going to read from and don't end the transaction
gr66 6:798481567563 374 i2c.write(xgAddress, cmd, 1, true);
gr66 6:798481567563 375 // Read in all the 8 bits of data
gr66 6:798481567563 376 i2c.read(xgAddress, cmd+1, 1);
gr66 6:798481567563 377
gr66 6:798481567563 378 // Then mask out the accel scale bits:
gr66 6:798481567563 379 cmd[1] &= 0xFF^(0x3 << 2); //// gr 2 au lieu de 3 mise a zero des bits 3 et 4
gr66 6:798481567563 380 // Then shift in our new scale bits:
gr66 6:798481567563 381 cmd[1] |= aScl << 2; //// gr 2 au lieu de 3
gr66 6:798481567563 382
gr66 6:798481567563 383 // Write the accelscale out to the accel
gr66 6:798481567563 384 i2c.write(xgAddress, cmd, 2);
gr66 6:798481567563 385
gr66 6:798481567563 386 // We've updated the sensor, but we also need to update our class variables
gr66 6:798481567563 387 // First update aScale:
gr66 6:798481567563 388 aScale = aScl;
gr66 6:798481567563 389 // Then calculate a new aRes, which relies on aScale being set correctly:
gr66 6:798481567563 390 calcaRes();
gr66 6:798481567563 391 }
gr66 6:798481567563 392
gr66 6:798481567563 393 void LSM6DS33::setGyroODR(gyro_odr gRate)
gr66 6:798481567563 394 {
gr66 6:798481567563 395 // The start of the addresses we want to read from
gr66 6:798481567563 396 char cmd[2] = {
gr66 6:798481567563 397 CTRL2_G,
gr66 6:798481567563 398 0
gr66 6:798481567563 399 };
gr66 6:798481567563 400
gr66 6:798481567563 401 // Set low power based on ODR, else keep sensor on high performance
gr66 6:798481567563 402 if(gRate == G_ODR_13_BW_0 | gRate == G_ODR_26_BW_2 | gRate == G_ODR_52_BW_16) {
gr66 6:798481567563 403 char cmdLow[2] = {
gr66 6:798481567563 404 CTRL7_G,
gr66 6:798481567563 405 1
gr66 6:798481567563 406 };
gr66 6:798481567563 407
gr66 6:798481567563 408 i2c.write(xgAddress, cmdLow, 2);
gr66 6:798481567563 409 } else {
gr66 6:798481567563 410 char cmdLow[2] = {
gr66 6:798481567563 411 CTRL7_G,
gr66 6:798481567563 412 0
gr66 6:798481567563 413 };
gr66 6:798481567563 414
gr66 6:798481567563 415 i2c.write(xgAddress, cmdLow, 2);
gr66 6:798481567563 416 }
gr66 6:798481567563 417
gr66 6:798481567563 418 // Write the address we are going to read from and don't end the transaction
gr66 6:798481567563 419 i2c.write(xgAddress, cmd, 1, true);
gr66 6:798481567563 420 // Read in all the 8 bits of data
gr66 6:798481567563 421 i2c.read(xgAddress, cmd+1, 1);
gr66 6:798481567563 422
gr66 6:798481567563 423 // Then mask out the gyro odr bits:
gr66 6:798481567563 424 cmd[1] &= 0xFF^(0xF << 4);
gr66 6:798481567563 425 // Then shift in our new odr bits:
gr66 6:798481567563 426 cmd[1] |= gRate;
gr66 6:798481567563 427
gr66 6:798481567563 428 // Write the gyroodr out to the gyro
gr66 6:798481567563 429 i2c.write(xgAddress, cmd, 2);
gr66 6:798481567563 430 }
gr66 6:798481567563 431
gr66 6:798481567563 432 void LSM6DS33::setAccelODR(accel_odr aRate)
gr66 6:798481567563 433 {
gr66 6:798481567563 434 // The start of the addresses we want to read from
gr66 6:798481567563 435 char cmd[2] = {
gr66 6:798481567563 436 CTRL1_XL,
gr66 6:798481567563 437 0
gr66 6:798481567563 438 };
gr66 6:798481567563 439
gr66 6:798481567563 440 // Set low power based on ODR, else keep sensor on high performance
gr66 6:798481567563 441 if(aRate == A_ODR_13 | aRate == A_ODR_26 | aRate == A_ODR_52) {
gr66 6:798481567563 442 char cmdLow[2] = {
gr66 6:798481567563 443 CTRL6_C,
gr66 6:798481567563 444 1
gr66 6:798481567563 445 };
gr66 6:798481567563 446
gr66 6:798481567563 447 i2c.write(xgAddress, cmdLow, 2);
gr66 6:798481567563 448 } else {
gr66 6:798481567563 449 char cmdLow[2] = {
gr66 6:798481567563 450 CTRL6_C,
gr66 6:798481567563 451 0
gr66 6:798481567563 452 };
gr66 6:798481567563 453
gr66 6:798481567563 454 i2c.write(xgAddress, cmdLow, 2);
gr66 6:798481567563 455 }
gr66 6:798481567563 456
gr66 6:798481567563 457 // Write the address we are going to read from and don't end the transaction
gr66 6:798481567563 458 i2c.write(xgAddress, cmd, 1, true);
gr66 6:798481567563 459 // Read in all the 8 bits of data
gr66 6:798481567563 460 i2c.read(xgAddress, cmd+1, 1);
gr66 6:798481567563 461
gr66 6:798481567563 462 // Then mask out the accel odr bits:
gr66 6:798481567563 463 cmd[1] &= 0xFF^(0xF << 4); // gr erreur ??
gr66 6:798481567563 464 // Then shift in our new odr bits:
gr66 6:798481567563 465 cmd[1] |= aRate << 4; // gr erreur
gr66 6:798481567563 466
gr66 6:798481567563 467 // Write the accelodr out to the accel
gr66 6:798481567563 468 i2c.write(xgAddress, cmd, 2);
gr66 6:798481567563 469 }
gr66 6:798481567563 470
gr66 6:798481567563 471 void LSM6DS33::calcgRes()
gr66 6:798481567563 472 {
gr66 6:798481567563 473 // Possible gyro scales (and their register bit settings) are:
gr66 6:798481567563 474 // 125 DPS , 245 DPS (00), 500 DPS (01), 2000 DPS (10).
gr66 6:798481567563 475 switch (gScale) {
gr66 6:798481567563 476 case G_SCALE_125DPS:
gr66 6:798481567563 477 gRes = 125.0 / 32768.0;
gr66 6:798481567563 478 break;
gr66 6:798481567563 479 case G_SCALE_250DPS:
gr66 6:798481567563 480 gRes = 250.0 / 32768.0;
gr66 6:798481567563 481 break;
gr66 6:798481567563 482 case G_SCALE_500DPS:
gr66 6:798481567563 483 gRes = 500.0 / 32768.0;
gr66 6:798481567563 484 break;
gr66 6:798481567563 485 case G_SCALE_1000DPS:
gr66 6:798481567563 486 gRes = 1000.0 / 32768.0;
gr66 6:798481567563 487 break;
gr66 6:798481567563 488 case G_SCALE_2000DPS:
gr66 6:798481567563 489 gRes = 2000.0 / 32768.0;
gr66 6:798481567563 490 break;
gr66 6:798481567563 491 }
gr66 6:798481567563 492 }
gr66 6:798481567563 493
gr66 6:798481567563 494 void LSM6DS33::calcaRes()
gr66 6:798481567563 495 {
gr66 6:798481567563 496 // Possible accelerometer scales (and their register bit settings) are:
gr66 6:798481567563 497 // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
gr66 6:798481567563 498 switch (aScale) {
gr66 6:798481567563 499 case A_SCALE_2G:
gr66 6:798481567563 500 aRes = 2.0 / 32768.0;
gr66 6:798481567563 501 break;
gr66 6:798481567563 502 case A_SCALE_4G:
gr66 6:798481567563 503 aRes = 4.0 / 32768.0;
gr66 6:798481567563 504 break;
gr66 6:798481567563 505 case A_SCALE_8G:
gr66 6:798481567563 506 aRes = 8.0 / 32768.0;
gr66 6:798481567563 507 break;
gr66 6:798481567563 508 case A_SCALE_16G:
gr66 6:798481567563 509 aRes = 16.0 / 32768.0;
gr66 6:798481567563 510 break;
gr66 6:798481567563 511 }
gr66 6:798481567563 512 }
gr66 6:798481567563 513 void LSM6DS33::calibration( int16_t iter)
gr66 6:798481567563 514 {
gr66 6:798481567563 515 int32_t gxoll=0,gyoll=0,gzoll=0;
gr66 6:798481567563 516 for(int ii=0; ii<iter; ii++) {
gr66 6:798481567563 517 this->readGyro();
gr66 6:798481567563 518 gx_off=gx_off+gx;
gr66 6:798481567563 519 gy_off=gy_off+gy;
gr66 6:798481567563 520 gz_off=gz_off+gz;
gr66 6:798481567563 521 //
gr66 6:798481567563 522
gr66 6:798481567563 523 gxoll=gxoll+(int32_t)gx_raw;
gr66 6:798481567563 524 gyoll=gyoll+(int32_t)gy_raw;
gr66 6:798481567563 525 gzoll=gzoll+(int32_t)gz_raw;
gr66 6:798481567563 526
gr66 6:798481567563 527
gr66 6:798481567563 528 //wait(0.01);
gr66 6:798481567563 529 }
gr66 6:798481567563 530 gx_off=gx_off/iter;
gr66 6:798481567563 531 gy_off=gy_off/iter;
gr66 6:798481567563 532 gz_off=gz_off/iter;
gr66 6:798481567563 533 //
gr66 6:798481567563 534 gxoll=gxoll/iter;
gr66 6:798481567563 535 gyoll=gyoll/iter;
gr66 6:798481567563 536 gzoll=gzoll/iter;
gr66 6:798481567563 537
gr66 6:798481567563 538 //
gr66 6:798481567563 539 gxol=(gxoll&0x00FF);
gr66 6:798481567563 540 gxoh=(gxoll>>8);
gr66 6:798481567563 541 gyol=(gyoll&0x00FF);
gr66 6:798481567563 542 gyoh=(gyoll>>8);
gr66 6:798481567563 543 gzol=(gzoll&0x00FF);
gr66 6:798481567563 544 gzoh=(gzoll>>8);
gr66 6:798481567563 545
gr66 6:798481567563 546 }
gr66 6:798481567563 547