seungjae oh / LSM6DS3

LSM6DS3 Library by sj

Fork of LSM6DS3 by Sherry Yang

LSM6DS3_sj.cpp@3:5f90ed3ba2e2, 2017-05-02 (annotated)

Committer:
oreo329
Date:
Tue May 02 17:30:18 2017 +0000
Revision:
3:5f90ed3ba2e2
fixed accelerometer & gyro begin errors(both odr, scale); ; now its working at diff odrs &scales;

Who changed what in which revision?

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