E&R S3 prime / LSM6DS33_GR1

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.

LSM6DS33_GR1.cpp@7:704790280bbe, 2021-09-30 (annotated)

Committer:
gr66
Date:
Thu Sep 30 09:37:35 2021 +0000
Revision:
7:704790280bbe
Parent:
6:132a63741359 
Fusion3

Who changed what in which revision?

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