Einstein Filho / Mbed OS MANGUEBAJA2019_FRONT2

Mangue Baja team's code to frontal ECU

LSM6DS3/LSM6DS3.cpp@0:12fb9cbcabcc, 2019-07-24 (annotated)

Committer:
einsteingustavo
Date:
Wed Jul 24 20:03:52 2019 +0000
Revision:
0:12fb9cbcabcc
Mangue Baja team's code to frontal ECU

Who changed what in which revision?

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