Diego Hamilton / LSM6DS3

Fork of Eugene Gonzalez's version of LSM9DS1_Demo, Modified by Sherry Yang.

Fork of LSM6DS3 by Sherry Yang

LSM6DS3.cpp@3:b705b62fbc1a, 2018-07-05 (annotated)

Committer:
sid26
Date:
Thu Jul 05 01:31:06 2018 +0000
Revision:
3:b705b62fbc1a
Parent:
2:ed14e6196255 
Set I2c frequency to 400kHz

Who changed what in which revision?

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