brian claus / LSM6DS33

LSM6DS33 Library

Dependents:   teensyIMU LSM6DS33 ALTIMU_v6

Fork of LSM6DS3 by Sherry Yang

LSM6DS33.cpp@3:b1d064895178, 2016-10-06 (annotated)

Committer:
bclaus
Date:
Thu Oct 06 16:08:42 2016 +0000
Revision:
3:b1d064895178
Parent:
LSM6DS3.cpp@2:ed14e6196255
Child:
4:4e7d663e26bd
branch from lsm6ds3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bclaus 3:b1d064895178 1 #include "LSM6DS33.h"
5hel2l2y 0:46630122dec9 2
bclaus 3:b1d064895178 3 LSM6DS33::LSM6DS33(PinName sda, PinName scl, uint8_t xgAddr) : i2c(sda, scl)
5hel2l2y 0:46630122dec9 4 {
5hel2l2y 0:46630122dec9 5 // xgAddress will store the 7-bit I2C address, if using I2C.
5hel2l2y 0:46630122dec9 6 xgAddress = xgAddr;
5hel2l2y 0:46630122dec9 7 }
5hel2l2y 0:46630122dec9 8
bclaus 3:b1d064895178 9 uint16_t LSM6DS33::begin(gyro_scale gScl, accel_scale aScl,
5hel2l2y 0:46630122dec9 10 gyro_odr gODR, accel_odr aODR)
5hel2l2y 0:46630122dec9 11 {
5hel2l2y 0:46630122dec9 12 // Store the given scales in class variables. These scale variables
5hel2l2y 0:46630122dec9 13 // are used throughout to calculate the actual g's, DPS,and Gs's.
5hel2l2y 0:46630122dec9 14 gScale = gScl;
5hel2l2y 0:46630122dec9 15 aScale = aScl;
5hel2l2y 0:46630122dec9 16
5hel2l2y 0:46630122dec9 17 // Once we have the scale values, we can calculate the resolution
5hel2l2y 0:46630122dec9 18 // of each sensor. That's what these functions are for. One for each sensor
5hel2l2y 0:46630122dec9 19 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
5hel2l2y 0:46630122dec9 20 calcaRes(); // Calculate g / ADC tick, stored in aRes variable
5hel2l2y 0:46630122dec9 21
5hel2l2y 0:46630122dec9 22
5hel2l2y 0:46630122dec9 23 // To verify communication, we can read from the WHO_AM_I register of
5hel2l2y 0:46630122dec9 24 // each device. Store those in a variable so we can return them.
5hel2l2y 0:46630122dec9 25 // The start of the addresses we want to read from
5hel2l2y 0:46630122dec9 26 char cmd[2] = {
5hel2l2y 0:46630122dec9 27 WHO_AM_I_REG,
5hel2l2y 0:46630122dec9 28 0
5hel2l2y 0:46630122dec9 29 };
5hel2l2y 0:46630122dec9 30
5hel2l2y 0:46630122dec9 31 // Write the address we are going to read from and don't end the transaction
5hel2l2y 0:46630122dec9 32 i2c.write(xgAddress, cmd, 1, true);
5hel2l2y 0:46630122dec9 33 // Read in all the 8 bits of data
5hel2l2y 0:46630122dec9 34 i2c.read(xgAddress, cmd+1, 1);
5hel2l2y 0:46630122dec9 35 uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I
5hel2l2y 0:46630122dec9 36
5hel2l2y 0:46630122dec9 37 // Gyro initialization stuff:
5hel2l2y 0:46630122dec9 38 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
5hel2l2y 0:46630122dec9 39 setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
5hel2l2y 0:46630122dec9 40 setGyroScale(gScale); // Set the gyro range
5hel2l2y 0:46630122dec9 41
5hel2l2y 0:46630122dec9 42 // Accelerometer initialization stuff:
5hel2l2y 0:46630122dec9 43 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
5hel2l2y 0:46630122dec9 44 setAccelODR(aODR); // Set the accel data rate.
5hel2l2y 0:46630122dec9 45 setAccelScale(aScale); // Set the accel range.
5hel2l2y 0:46630122dec9 46
bclaus 3:b1d064895178 47 //set high res timestamp where LSB is 25us
bclaus 3:b1d064895178 48 cmd[0] = WAKE_UP_DUR;
bclaus 3:b1d064895178 49 cmd[1] = 0x10;
bclaus 3:b1d064895178 50 i2c.write(xgAddress, cmd, 2);
5hel2l2y 2:ed14e6196255 51
5hel2l2y 0:46630122dec9 52 // Once everything is initialized, return the WHO_AM_I registers we read:
5hel2l2y 0:46630122dec9 53 return xgTest;
5hel2l2y 0:46630122dec9 54 }
5hel2l2y 0:46630122dec9 55
bclaus 3:b1d064895178 56 void LSM6DS33::initGyro()
5hel2l2y 0:46630122dec9 57 {
5hel2l2y 0:46630122dec9 58 char cmd[4] = {
5hel2l2y 0:46630122dec9 59 CTRL2_G,
5hel2l2y 1:924c7dea286e 60 gScale | G_ODR_104,
5hel2l2y 0:46630122dec9 61 0, // Default data out and int out
5hel2l2y 0:46630122dec9 62 0 // Default power mode and high pass settings
5hel2l2y 0:46630122dec9 63 };
5hel2l2y 0:46630122dec9 64
5hel2l2y 0:46630122dec9 65 // Write the data to the gyro control registers
5hel2l2y 0:46630122dec9 66 i2c.write(xgAddress, cmd, 4);
5hel2l2y 0:46630122dec9 67 }
5hel2l2y 0:46630122dec9 68
bclaus 3:b1d064895178 69 void LSM6DS33::initAccel()
5hel2l2y 0:46630122dec9 70 {
5hel2l2y 0:46630122dec9 71 char cmd[4] = {
5hel2l2y 0:46630122dec9 72 CTRL1_XL,
5hel2l2y 0:46630122dec9 73 0x38, // Enable all axis and don't decimate data in out Registers
bclaus 3:b1d064895178 74 (A_ODR_104 << 5) | (aScale << 3) | (A_BW_400), // 119 Hz ODR, set scale, and auto BW
5hel2l2y 0:46630122dec9 75 0 // Default resolution mode and filtering settings
5hel2l2y 0:46630122dec9 76 };
5hel2l2y 0:46630122dec9 77
5hel2l2y 0:46630122dec9 78 // Write the data to the accel control registers
5hel2l2y 0:46630122dec9 79 i2c.write(xgAddress, cmd, 4);
5hel2l2y 0:46630122dec9 80 }
5hel2l2y 0:46630122dec9 81
bclaus 3:b1d064895178 82 void LSM6DS33::initIntr()
5hel2l2y 2:ed14e6196255 83 {
5hel2l2y 2:ed14e6196255 84 char cmd[2];
bclaus 3:b1d064895178 85
5hel2l2y 2:ed14e6196255 86 }
5hel2l2y 2:ed14e6196255 87
bclaus 3:b1d064895178 88 void LSM6DS33::readAll(){
bclaus 3:b1d064895178 89 // The data we are going to read from the temp/gyr/acc/timestamp
bclaus 3:b1d064895178 90 char data[14];//from 0x20 to 0x42
bclaus 3:b1d064895178 91 char tsdata[3];
bclaus 3:b1d064895178 92
bclaus 3:b1d064895178 93 // Set addresses
bclaus 3:b1d064895178 94 char subAddressLT = OUT_TEMP_L;
bclaus 3:b1d064895178 95 char subAddressHT = OUT_TEMP_H;
bclaus 3:b1d064895178 96 char subAddressXLG = OUTX_L_G;
bclaus 3:b1d064895178 97 char subAddressXHG = OUTX_H_G;
bclaus 3:b1d064895178 98 char subAddressYLG = OUTY_L_G;
bclaus 3:b1d064895178 99 char subAddressYHG = OUTY_H_G;
bclaus 3:b1d064895178 100 char subAddressZLG = OUTZ_L_G;
bclaus 3:b1d064895178 101 char subAddressZHG = OUTZ_H_G;
bclaus 3:b1d064895178 102 char subAddressXL = OUTX_L_XL;
bclaus 3:b1d064895178 103 char subAddressXH = OUTX_H_XL;
bclaus 3:b1d064895178 104 char subAddressYL = OUTY_L_XL;
bclaus 3:b1d064895178 105 char subAddressYH = OUTY_H_XL;
bclaus 3:b1d064895178 106 char subAddressZL = OUTZ_L_XL;
bclaus 3:b1d064895178 107 char subAddressZH = OUTZ_H_XL;
bclaus 3:b1d064895178 108 char subAddressTS0 = TIMESTAMP0_REG;
bclaus 3:b1d064895178 109 char subAddressTS1 = TIMESTAMP1_REG;
bclaus 3:b1d064895178 110 char subAddressTS2 = TIMESTAMP2_REG;
bclaus 3:b1d064895178 111
bclaus 3:b1d064895178 112 // Write the address we are going to read from and don't end the transaction
bclaus 3:b1d064895178 113 i2c.write(xgAddress, &subAddressLT, 1, true);
bclaus 3:b1d064895178 114 // Read in registers containing all the data and timestamp and don't end
bclaus 3:b1d064895178 115 i2c.read(xgAddress, data, 14,true);
bclaus 3:b1d064895178 116 i2c.write(xgAddress, &subAddressTS0, 1, true);
bclaus 3:b1d064895178 117 i2c.read(xgAddress, tsdata, 3);
bclaus 3:b1d064895178 118
bclaus 3:b1d064895178 119 // Temperature is a 12-bit signed integer
bclaus 3:b1d064895178 120 temperature_raw = data[0] | (data[1] << 8);
bclaus 3:b1d064895178 121 gx_raw = data[2] | (data[3] << 8);
bclaus 3:b1d064895178 122 gy_raw = data[4] | (data[5] << 8);
bclaus 3:b1d064895178 123 gz_raw = data[6] | (data[7] << 8);
bclaus 3:b1d064895178 124 ax_raw = data[8] | (data[9] << 8);
bclaus 3:b1d064895178 125 ay_raw = data[10] | (data[11] << 8);
bclaus 3:b1d064895178 126 az_raw = data[12] | (data[13] << 8);
bclaus 3:b1d064895178 127 time_raw = tsdata[0] | (tsdata[1] << 8) | (tsdata[2] << 16);
bclaus 3:b1d064895178 128
bclaus 3:b1d064895178 129
bclaus 3:b1d064895178 130 temperature_c = (float)temperature_raw / 16.0 + 25.0;
bclaus 3:b1d064895178 131 gx = gx_raw * gRes;
bclaus 3:b1d064895178 132 gy = gy_raw * gRes;
bclaus 3:b1d064895178 133 gz = gz_raw * gRes;
bclaus 3:b1d064895178 134 ax = ax_raw * aRes;
bclaus 3:b1d064895178 135 ay = ay_raw * aRes;
bclaus 3:b1d064895178 136 az = az_raw * aRes;
bclaus 3:b1d064895178 137 time = time_raw*(0.000025);
bclaus 3:b1d064895178 138
bclaus 3:b1d064895178 139
bclaus 3:b1d064895178 140 }
bclaus 3:b1d064895178 141
bclaus 3:b1d064895178 142
bclaus 3:b1d064895178 143 void LSM6DS33::readAccel()
5hel2l2y 0:46630122dec9 144 {
5hel2l2y 0:46630122dec9 145 // The data we are going to read from the accel
5hel2l2y 0:46630122dec9 146 char data[6];
5hel2l2y 0:46630122dec9 147
5hel2l2y 0:46630122dec9 148 // Set addresses
5hel2l2y 0:46630122dec9 149 char subAddressXL = OUTX_L_XL;
5hel2l2y 0:46630122dec9 150 char subAddressXH = OUTX_H_XL;
5hel2l2y 0:46630122dec9 151 char subAddressYL = OUTY_L_XL;
5hel2l2y 0:46630122dec9 152 char subAddressYH = OUTY_H_XL;
5hel2l2y 0:46630122dec9 153 char subAddressZL = OUTZ_L_XL;
5hel2l2y 0:46630122dec9 154 char subAddressZH = OUTZ_H_XL;
5hel2l2y 0:46630122dec9 155
5hel2l2y 0:46630122dec9 156 // Write the address we are going to read from and don't end the transaction
5hel2l2y 0:46630122dec9 157 i2c.write(xgAddress, &subAddressXL, 1, true);
5hel2l2y 0:46630122dec9 158 // Read in register containing the axes data and alocated to the correct index
5hel2l2y 0:46630122dec9 159 i2c.read(xgAddress, data, 1);
5hel2l2y 0:46630122dec9 160
5hel2l2y 0:46630122dec9 161 i2c.write(xgAddress, &subAddressXH, 1, true);
5hel2l2y 0:46630122dec9 162 i2c.read(xgAddress, (data + 1), 1);
5hel2l2y 0:46630122dec9 163 i2c.write(xgAddress, &subAddressYL, 1, true);
5hel2l2y 0:46630122dec9 164 i2c.read(xgAddress, (data + 2), 1);
5hel2l2y 0:46630122dec9 165 i2c.write(xgAddress, &subAddressYH, 1, true);
5hel2l2y 0:46630122dec9 166 i2c.read(xgAddress, (data + 3), 1);
5hel2l2y 0:46630122dec9 167 i2c.write(xgAddress, &subAddressZL, 1, true);
5hel2l2y 0:46630122dec9 168 i2c.read(xgAddress, (data + 4), 1);
5hel2l2y 0:46630122dec9 169 i2c.write(xgAddress, &subAddressZH, 1, true);
5hel2l2y 0:46630122dec9 170 i2c.read(xgAddress, (data + 5), 1);
5hel2l2y 0:46630122dec9 171
5hel2l2y 0:46630122dec9 172 // Reassemble the data and convert to g
5hel2l2y 0:46630122dec9 173 ax_raw = data[0] | (data[1] << 8);
5hel2l2y 0:46630122dec9 174 ay_raw = data[2] | (data[3] << 8);
5hel2l2y 0:46630122dec9 175 az_raw = data[4] | (data[5] << 8);
5hel2l2y 0:46630122dec9 176 ax = ax_raw * aRes;
5hel2l2y 0:46630122dec9 177 ay = ay_raw * aRes;
5hel2l2y 0:46630122dec9 178 az = az_raw * aRes;
5hel2l2y 0:46630122dec9 179 }
5hel2l2y 0:46630122dec9 180
bclaus 3:b1d064895178 181 void LSM6DS33::readIntr()
5hel2l2y 2:ed14e6196255 182 {
5hel2l2y 2:ed14e6196255 183 char data[1];
5hel2l2y 2:ed14e6196255 184 char subAddress = TAP_SRC;
5hel2l2y 2:ed14e6196255 185
5hel2l2y 2:ed14e6196255 186 i2c.write(xgAddress, &subAddress, 1, true);
5hel2l2y 2:ed14e6196255 187 i2c.read(xgAddress, data, 1);
5hel2l2y 2:ed14e6196255 188
5hel2l2y 2:ed14e6196255 189 intr = (float)data[0];
5hel2l2y 2:ed14e6196255 190 }
5hel2l2y 2:ed14e6196255 191
bclaus 3:b1d064895178 192 void LSM6DS33::readTemp()
5hel2l2y 0:46630122dec9 193 {
5hel2l2y 0:46630122dec9 194 // The data we are going to read from the temp
5hel2l2y 0:46630122dec9 195 char data[2];
5hel2l2y 0:46630122dec9 196
5hel2l2y 0:46630122dec9 197 // Set addresses
5hel2l2y 0:46630122dec9 198 char subAddressL = OUT_TEMP_L;
5hel2l2y 0:46630122dec9 199 char subAddressH = OUT_TEMP_H;
5hel2l2y 0:46630122dec9 200
5hel2l2y 0:46630122dec9 201 // Write the address we are going to read from and don't end the transaction
5hel2l2y 0:46630122dec9 202 i2c.write(xgAddress, &subAddressL, 1, true);
5hel2l2y 0:46630122dec9 203 // Read in register containing the temperature data and alocated to the correct index
5hel2l2y 0:46630122dec9 204 i2c.read(xgAddress, data, 1);
5hel2l2y 0:46630122dec9 205
5hel2l2y 0:46630122dec9 206 i2c.write(xgAddress, &subAddressH, 1, true);
5hel2l2y 0:46630122dec9 207 i2c.read(xgAddress, (data + 1), 1);
5hel2l2y 0:46630122dec9 208
5hel2l2y 0:46630122dec9 209 // Temperature is a 12-bit signed integer
5hel2l2y 0:46630122dec9 210 temperature_raw = data[0] | (data[1] << 8);
5hel2l2y 0:46630122dec9 211
5hel2l2y 0:46630122dec9 212 temperature_c = (float)temperature_raw / 16.0 + 25.0;
5hel2l2y 0:46630122dec9 213 temperature_f = temperature_c * 1.8 + 32.0;
5hel2l2y 0:46630122dec9 214 }
5hel2l2y 0:46630122dec9 215
5hel2l2y 0:46630122dec9 216
bclaus 3:b1d064895178 217 void LSM6DS33::readGyro()
5hel2l2y 0:46630122dec9 218 {
5hel2l2y 0:46630122dec9 219 // The data we are going to read from the gyro
5hel2l2y 0:46630122dec9 220 char data[6];
5hel2l2y 0:46630122dec9 221
5hel2l2y 0:46630122dec9 222 // Set addresses
5hel2l2y 0:46630122dec9 223 char subAddressXL = OUTX_L_G;
5hel2l2y 0:46630122dec9 224 char subAddressXH = OUTX_H_G;
5hel2l2y 0:46630122dec9 225 char subAddressYL = OUTY_L_G;
5hel2l2y 0:46630122dec9 226 char subAddressYH = OUTY_H_G;
5hel2l2y 0:46630122dec9 227 char subAddressZL = OUTZ_L_G;
5hel2l2y 0:46630122dec9 228 char subAddressZH = OUTZ_H_G;
5hel2l2y 0:46630122dec9 229
5hel2l2y 0:46630122dec9 230 // Write the address we are going to read from and don't end the transaction
5hel2l2y 0:46630122dec9 231 i2c.write(xgAddress, &subAddressXL, 1, true);
5hel2l2y 0:46630122dec9 232 // Read in register containing the axes data and alocated to the correct index
5hel2l2y 0:46630122dec9 233 i2c.read(xgAddress, data, 1);
5hel2l2y 0:46630122dec9 234
5hel2l2y 0:46630122dec9 235 i2c.write(xgAddress, &subAddressXH, 1, true);
5hel2l2y 0:46630122dec9 236 i2c.read(xgAddress, (data + 1), 1);
5hel2l2y 0:46630122dec9 237 i2c.write(xgAddress, &subAddressYL, 1, true);
5hel2l2y 0:46630122dec9 238 i2c.read(xgAddress, (data + 2), 1);
5hel2l2y 0:46630122dec9 239 i2c.write(xgAddress, &subAddressYH, 1, true);
5hel2l2y 0:46630122dec9 240 i2c.read(xgAddress, (data + 3), 1);
5hel2l2y 0:46630122dec9 241 i2c.write(xgAddress, &subAddressZL, 1, true);
5hel2l2y 0:46630122dec9 242 i2c.read(xgAddress, (data + 4), 1);
5hel2l2y 0:46630122dec9 243 i2c.write(xgAddress, &subAddressZH, 1, true);
5hel2l2y 0:46630122dec9 244 i2c.read(xgAddress, (data + 5), 1);
5hel2l2y 0:46630122dec9 245
5hel2l2y 0:46630122dec9 246 // Reassemble the data and convert to degrees/sec
5hel2l2y 0:46630122dec9 247 gx_raw = data[0] | (data[1] << 8);
5hel2l2y 0:46630122dec9 248 gy_raw = data[2] | (data[3] << 8);
5hel2l2y 0:46630122dec9 249 gz_raw = data[4] | (data[5] << 8);
5hel2l2y 0:46630122dec9 250 gx = gx_raw * gRes;
5hel2l2y 0:46630122dec9 251 gy = gy_raw * gRes;
5hel2l2y 0:46630122dec9 252 gz = gz_raw * gRes;
5hel2l2y 0:46630122dec9 253 }
5hel2l2y 0:46630122dec9 254
bclaus 3:b1d064895178 255 void LSM6DS33::setGyroScale(gyro_scale gScl)
5hel2l2y 0:46630122dec9 256 {
5hel2l2y 0:46630122dec9 257 // The start of the addresses we want to read from
5hel2l2y 0:46630122dec9 258 char cmd[2] = {
5hel2l2y 0:46630122dec9 259 CTRL2_G,
5hel2l2y 0:46630122dec9 260 0
5hel2l2y 0:46630122dec9 261 };
5hel2l2y 0:46630122dec9 262
5hel2l2y 0:46630122dec9 263 // Write the address we are going to read from and don't end the transaction
5hel2l2y 0:46630122dec9 264 i2c.write(xgAddress, cmd, 1, true);
5hel2l2y 0:46630122dec9 265 // Read in all the 8 bits of data
5hel2l2y 0:46630122dec9 266 i2c.read(xgAddress, cmd+1, 1);
5hel2l2y 0:46630122dec9 267
5hel2l2y 0:46630122dec9 268 // Then mask out the gyro scale bits:
5hel2l2y 0:46630122dec9 269 cmd[1] &= 0xFF^(0x3 << 3);
5hel2l2y 0:46630122dec9 270 // Then shift in our new scale bits:
5hel2l2y 0:46630122dec9 271 cmd[1] |= gScl << 3;
5hel2l2y 0:46630122dec9 272
5hel2l2y 0:46630122dec9 273 // Write the gyroscale out to the gyro
5hel2l2y 0:46630122dec9 274 i2c.write(xgAddress, cmd, 2);
5hel2l2y 0:46630122dec9 275
5hel2l2y 0:46630122dec9 276 // We've updated the sensor, but we also need to update our class variables
5hel2l2y 0:46630122dec9 277 // First update gScale:
5hel2l2y 0:46630122dec9 278 gScale = gScl;
5hel2l2y 0:46630122dec9 279 // Then calculate a new gRes, which relies on gScale being set correctly:
5hel2l2y 0:46630122dec9 280 calcgRes();
5hel2l2y 0:46630122dec9 281 }
5hel2l2y 0:46630122dec9 282
bclaus 3:b1d064895178 283 void LSM6DS33::setAccelScale(accel_scale aScl)
5hel2l2y 0:46630122dec9 284 {
5hel2l2y 0:46630122dec9 285 // The start of the addresses we want to read from
5hel2l2y 0:46630122dec9 286 char cmd[2] = {
5hel2l2y 0:46630122dec9 287 CTRL1_XL,
5hel2l2y 0:46630122dec9 288 0
5hel2l2y 0:46630122dec9 289 };
5hel2l2y 0:46630122dec9 290
5hel2l2y 0:46630122dec9 291 // Write the address we are going to read from and don't end the transaction
5hel2l2y 0:46630122dec9 292 i2c.write(xgAddress, cmd, 1, true);
5hel2l2y 0:46630122dec9 293 // Read in all the 8 bits of data
5hel2l2y 0:46630122dec9 294 i2c.read(xgAddress, cmd+1, 1);
5hel2l2y 0:46630122dec9 295
5hel2l2y 0:46630122dec9 296 // Then mask out the accel scale bits:
5hel2l2y 0:46630122dec9 297 cmd[1] &= 0xFF^(0x3 << 3);
5hel2l2y 0:46630122dec9 298 // Then shift in our new scale bits:
5hel2l2y 0:46630122dec9 299 cmd[1] |= aScl << 3;
5hel2l2y 0:46630122dec9 300
5hel2l2y 0:46630122dec9 301 // Write the accelscale out to the accel
5hel2l2y 0:46630122dec9 302 i2c.write(xgAddress, cmd, 2);
5hel2l2y 0:46630122dec9 303
5hel2l2y 0:46630122dec9 304 // We've updated the sensor, but we also need to update our class variables
5hel2l2y 0:46630122dec9 305 // First update aScale:
5hel2l2y 0:46630122dec9 306 aScale = aScl;
5hel2l2y 0:46630122dec9 307 // Then calculate a new aRes, which relies on aScale being set correctly:
5hel2l2y 0:46630122dec9 308 calcaRes();
5hel2l2y 0:46630122dec9 309 }
5hel2l2y 0:46630122dec9 310
bclaus 3:b1d064895178 311 void LSM6DS33::setGyroODR(gyro_odr gRate)
5hel2l2y 0:46630122dec9 312 {
5hel2l2y 0:46630122dec9 313 // The start of the addresses we want to read from
5hel2l2y 0:46630122dec9 314 char cmd[2] = {
5hel2l2y 0:46630122dec9 315 CTRL2_G,
5hel2l2y 0:46630122dec9 316 0
5hel2l2y 0:46630122dec9 317 };
5hel2l2y 1:924c7dea286e 318
5hel2l2y 1:924c7dea286e 319 // Set low power based on ODR, else keep sensor on high performance
5hel2l2y 1:924c7dea286e 320 if(gRate == G_ODR_13_BW_0 | gRate == G_ODR_26_BW_2 | gRate == G_ODR_52_BW_16) {
5hel2l2y 1:924c7dea286e 321 char cmdLow[2] ={
5hel2l2y 1:924c7dea286e 322 CTRL7_G,
5hel2l2y 1:924c7dea286e 323 1
5hel2l2y 1:924c7dea286e 324 };
5hel2l2y 1:924c7dea286e 325
5hel2l2y 1:924c7dea286e 326 i2c.write(xgAddress, cmdLow, 2);
5hel2l2y 1:924c7dea286e 327 }
5hel2l2y 1:924c7dea286e 328 else {
5hel2l2y 1:924c7dea286e 329 char cmdLow[2] ={
5hel2l2y 1:924c7dea286e 330 CTRL7_G,
5hel2l2y 1:924c7dea286e 331 0
5hel2l2y 1:924c7dea286e 332 };
5hel2l2y 1:924c7dea286e 333
5hel2l2y 1:924c7dea286e 334 i2c.write(xgAddress, cmdLow, 2);
5hel2l2y 1:924c7dea286e 335 }
5hel2l2y 0:46630122dec9 336
5hel2l2y 0:46630122dec9 337 // Write the address we are going to read from and don't end the transaction
5hel2l2y 0:46630122dec9 338 i2c.write(xgAddress, cmd, 1, true);
5hel2l2y 0:46630122dec9 339 // Read in all the 8 bits of data
5hel2l2y 0:46630122dec9 340 i2c.read(xgAddress, cmd+1, 1);
5hel2l2y 0:46630122dec9 341
5hel2l2y 0:46630122dec9 342 // Then mask out the gyro odr bits:
5hel2l2y 0:46630122dec9 343 cmd[1] &= (0x3 << 3);
5hel2l2y 0:46630122dec9 344 // Then shift in our new odr bits:
5hel2l2y 0:46630122dec9 345 cmd[1] |= gRate;
5hel2l2y 0:46630122dec9 346
5hel2l2y 0:46630122dec9 347 // Write the gyroodr out to the gyro
5hel2l2y 0:46630122dec9 348 i2c.write(xgAddress, cmd, 2);
5hel2l2y 0:46630122dec9 349 }
5hel2l2y 0:46630122dec9 350
bclaus 3:b1d064895178 351 void LSM6DS33::setAccelODR(accel_odr aRate)
5hel2l2y 0:46630122dec9 352 {
5hel2l2y 0:46630122dec9 353 // The start of the addresses we want to read from
5hel2l2y 0:46630122dec9 354 char cmd[2] = {
5hel2l2y 0:46630122dec9 355 CTRL1_XL,
5hel2l2y 0:46630122dec9 356 0
5hel2l2y 0:46630122dec9 357 };
5hel2l2y 1:924c7dea286e 358
5hel2l2y 1:924c7dea286e 359 // Set low power based on ODR, else keep sensor on high performance
5hel2l2y 1:924c7dea286e 360 if(aRate == A_ODR_13 | aRate == A_ODR_26 | aRate == A_ODR_52) {
5hel2l2y 1:924c7dea286e 361 char cmdLow[2] ={
5hel2l2y 1:924c7dea286e 362 CTRL6_C,
5hel2l2y 1:924c7dea286e 363 1
5hel2l2y 1:924c7dea286e 364 };
5hel2l2y 1:924c7dea286e 365
5hel2l2y 1:924c7dea286e 366 i2c.write(xgAddress, cmdLow, 2);
5hel2l2y 1:924c7dea286e 367 }
5hel2l2y 1:924c7dea286e 368 else {
5hel2l2y 1:924c7dea286e 369 char cmdLow[2] ={
5hel2l2y 1:924c7dea286e 370 CTRL6_C,
5hel2l2y 1:924c7dea286e 371 0
5hel2l2y 1:924c7dea286e 372 };
5hel2l2y 1:924c7dea286e 373
5hel2l2y 1:924c7dea286e 374 i2c.write(xgAddress, cmdLow, 2);
5hel2l2y 1:924c7dea286e 375 }
5hel2l2y 0:46630122dec9 376
5hel2l2y 0:46630122dec9 377 // Write the address we are going to read from and don't end the transaction
5hel2l2y 0:46630122dec9 378 i2c.write(xgAddress, cmd, 1, true);
5hel2l2y 0:46630122dec9 379 // Read in all the 8 bits of data
5hel2l2y 0:46630122dec9 380 i2c.read(xgAddress, cmd+1, 1);
5hel2l2y 0:46630122dec9 381
5hel2l2y 0:46630122dec9 382 // Then mask out the accel odr bits:
5hel2l2y 0:46630122dec9 383 cmd[1] &= 0xFF^(0x7 << 5);
5hel2l2y 0:46630122dec9 384 // Then shift in our new odr bits:
5hel2l2y 0:46630122dec9 385 cmd[1] |= aRate << 5;
5hel2l2y 0:46630122dec9 386
5hel2l2y 0:46630122dec9 387 // Write the accelodr out to the accel
5hel2l2y 0:46630122dec9 388 i2c.write(xgAddress, cmd, 2);
5hel2l2y 0:46630122dec9 389 }
5hel2l2y 0:46630122dec9 390
bclaus 3:b1d064895178 391 void LSM6DS33::calcgRes()
5hel2l2y 0:46630122dec9 392 {
5hel2l2y 0:46630122dec9 393 // Possible gyro scales (and their register bit settings) are:
5hel2l2y 0:46630122dec9 394 // 245 DPS (00), 500 DPS (01), 2000 DPS (10).
5hel2l2y 0:46630122dec9 395 switch (gScale)
5hel2l2y 0:46630122dec9 396 {
5hel2l2y 0:46630122dec9 397 case G_SCALE_245DPS:
5hel2l2y 0:46630122dec9 398 gRes = 245.0 / 32768.0;
5hel2l2y 0:46630122dec9 399 break;
5hel2l2y 0:46630122dec9 400 case G_SCALE_500DPS:
5hel2l2y 0:46630122dec9 401 gRes = 500.0 / 32768.0;
5hel2l2y 0:46630122dec9 402 break;
5hel2l2y 0:46630122dec9 403 case G_SCALE_2000DPS:
5hel2l2y 0:46630122dec9 404 gRes = 2000.0 / 32768.0;
5hel2l2y 0:46630122dec9 405 break;
5hel2l2y 0:46630122dec9 406 }
5hel2l2y 0:46630122dec9 407 }
5hel2l2y 0:46630122dec9 408
bclaus 3:b1d064895178 409 void LSM6DS33::calcaRes()
5hel2l2y 0:46630122dec9 410 {
5hel2l2y 0:46630122dec9 411 // Possible accelerometer scales (and their register bit settings) are:
5hel2l2y 0:46630122dec9 412 // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
5hel2l2y 0:46630122dec9 413 switch (aScale)
5hel2l2y 0:46630122dec9 414 {
5hel2l2y 0:46630122dec9 415 case A_SCALE_2G:
5hel2l2y 0:46630122dec9 416 aRes = 2.0 / 32768.0;
5hel2l2y 0:46630122dec9 417 break;
5hel2l2y 0:46630122dec9 418 case A_SCALE_4G:
5hel2l2y 0:46630122dec9 419 aRes = 4.0 / 32768.0;
5hel2l2y 0:46630122dec9 420 break;
5hel2l2y 0:46630122dec9 421 case A_SCALE_8G:
5hel2l2y 0:46630122dec9 422 aRes = 8.0 / 32768.0;
5hel2l2y 0:46630122dec9 423 break;
5hel2l2y 0:46630122dec9 424 case A_SCALE_16G:
5hel2l2y 0:46630122dec9 425 aRes = 16.0 / 32768.0;
5hel2l2y 0:46630122dec9 426 break;
5hel2l2y 0:46630122dec9 427 }
5hel2l2y 0:46630122dec9 428 }