Shingo FUKUYA / Mbed OS Airio-Base_Grove_6Axis_OS5

Airio-Base test program using Mbed OS 5 .

Dependencies:   SDFileSystem

LSM6DS3/LSM6DS3.cpp@1:2cfe30ab611c, 2019-07-16 (annotated)

Committer:
mbed_crane_elec
Date:
Tue Jul 16 21:29:03 2019 +0000
Revision:
1:2cfe30ab611c
modify

Who changed what in which revision?

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