Einstein Filho / Mbed 2 deprecated MANGUELOGGER_K64F

Data Logger Mangue Baja

Dependencies:   mbed

LSM6DS3/LSM6DS3.cpp@0:aef6b59caed0, 2019-07-05 (annotated)

Committer:
einsteingustavo
Date:
Fri Jul 05 00:02:13 2019 +0000
Revision:
0:aef6b59caed0
Datta Logger Mangue Baja 200Hz

Who changed what in which revision?

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