Raynaud Gilles / LSM9DS1

VGR

Dependents:   VITI_motor_angle_1 VITI_motor_angle_2 VITI_motor_angle_3

LSM9DS1.cpp@9:33258ebdb817, 2020-05-21 (annotated)

Committer:
gr66
Date:
Thu May 21 17:42:32 2020 +0000
Revision:
9:33258ebdb817
Parent:
8:16e88babd42a 
V1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beanmachine44 0:622e8874902e 1 #include "LSM9DS1.h"
beanmachine44 0:622e8874902e 2
beanmachine44 0:622e8874902e 3 LSM9DS1::LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr) : i2c(sda, scl)
beanmachine44 0:622e8874902e 4 {
beanmachine44 0:622e8874902e 5 // xgAddress and mAddress will store the 7-bit I2C address, if using I2C.
beanmachine44 0:622e8874902e 6 xgAddress = xgAddr;
beanmachine44 0:622e8874902e 7 mAddress = mAddr;
beanmachine44 0:622e8874902e 8 }
beanmachine44 0:622e8874902e 9
benson516 8:16e88babd42a 10 bool LSM9DS1::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
beanmachine44 0:622e8874902e 11 gyro_odr gODR, accel_odr aODR, mag_odr mODR)
beanmachine44 0:622e8874902e 12 {
beanmachine44 0:622e8874902e 13 // Store the given scales in class variables. These scale variables
beanmachine44 0:622e8874902e 14 // are used throughout to calculate the actual g's, DPS,and Gs's.
beanmachine44 0:622e8874902e 15 gScale = gScl;
beanmachine44 0:622e8874902e 16 aScale = aScl;
beanmachine44 0:622e8874902e 17 mScale = mScl;
benson516 8:16e88babd42a 18
beanmachine44 0:622e8874902e 19 // Once we have the scale values, we can calculate the resolution
beanmachine44 0:622e8874902e 20 // of each sensor. That's what these functions are for. One for each sensor
beanmachine44 0:622e8874902e 21 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
beanmachine44 0:622e8874902e 22 calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
beanmachine44 0:622e8874902e 23 calcaRes(); // Calculate g / ADC tick, stored in aRes variable
benson516 8:16e88babd42a 24
benson516 8:16e88babd42a 25
beanmachine44 0:622e8874902e 26 // To verify communication, we can read from the WHO_AM_I register of
beanmachine44 0:622e8874902e 27 // each device. Store those in a variable so we can return them.
beanmachine44 0:622e8874902e 28 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 29 char cmd[2] = {
beanmachine44 0:622e8874902e 30 WHO_AM_I_XG,
beanmachine44 0:622e8874902e 31 0
beanmachine44 0:622e8874902e 32 };
beanmachine44 0:622e8874902e 33
beanmachine44 0:622e8874902e 34 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 35 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 36 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 37 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 38 uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I
benson516 8:16e88babd42a 39
beanmachine44 0:622e8874902e 40 // Reset to the address of the mag who am i
ChangYuHsuan 6:28c4b3c8b43d 41 cmd[0] = WHO_AM_I_M;
beanmachine44 0:622e8874902e 42 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 43 i2c.write(mAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 44 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 45 i2c.read(mAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 46 uint8_t mTest = cmd[1]; // Read the mag WHO_AM_I
benson516 8:16e88babd42a 47
ChangYuHsuan 6:28c4b3c8b43d 48 for(int ii = 0; ii < 3; ii++)
ChangYuHsuan 6:28c4b3c8b43d 49 {
ChangYuHsuan 6:28c4b3c8b43d 50 gBiasRaw[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 51 aBiasRaw[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 52 gBias[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 53 aBias[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 54 }
benson516 8:16e88babd42a 55 autoCalib = false;
benson516 8:16e88babd42a 56
beanmachine44 0:622e8874902e 57 // Accelerometer initialization stuff:
beanmachine44 0:622e8874902e 58 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
beanmachine44 0:622e8874902e 59 setAccelODR(aODR); // Set the accel data rate.
beanmachine44 0:622e8874902e 60 setAccelScale(aScale); // Set the accel range.
benson516 8:16e88babd42a 61
benson516 8:16e88babd42a 62 // Gyro initialization stuff:
benson516 8:16e88babd42a 63 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
benson516 8:16e88babd42a 64 setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
benson516 8:16e88babd42a 65 setGyroScale(gScale); // Set the gyro range
benson516 8:16e88babd42a 66
beanmachine44 0:622e8874902e 67 // Magnetometer initialization stuff:
beanmachine44 0:622e8874902e 68 initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
beanmachine44 0:622e8874902e 69 setMagODR(mODR); // Set the magnetometer output data rate.
beanmachine44 0:622e8874902e 70 setMagScale(mScale); // Set the magnetometer's range.
benson516 8:16e88babd42a 71
5hel2l2y 4:7ffcb378cfd4 72 // Interrupt initialization stuff
5hel2l2y 3:f96b287c0bf7 73 initIntr();
benson516 8:16e88babd42a 74
beanmachine44 0:622e8874902e 75 // Once everything is initialized, return the WHO_AM_I registers we read:
benson516 8:16e88babd42a 76 return ( ((xgTest << 8) | mTest) == (WHO_AM_I_AG_RSP << 8 | WHO_AM_I_M_RSP) );
beanmachine44 0:622e8874902e 77 }
beanmachine44 0:622e8874902e 78
beanmachine44 0:622e8874902e 79 void LSM9DS1::initGyro()
beanmachine44 0:622e8874902e 80 {
benson516 8:16e88babd42a 81 /*
beanmachine44 0:622e8874902e 82 char cmd[4] = {
beanmachine44 0:622e8874902e 83 CTRL_REG1_G,
beanmachine44 0:622e8874902e 84 gScale | G_ODR_119_BW_14,
beanmachine44 0:622e8874902e 85 0, // Default data out and int out
beanmachine44 0:622e8874902e 86 0 // Default power mode and high pass settings
beanmachine44 0:622e8874902e 87 };
benson516 8:16e88babd42a 88 */
benson516 8:16e88babd42a 89
benson516 8:16e88babd42a 90 char cmd[4] = {
benson516 8:16e88babd42a 91 CTRL_REG1_G,
gr66 9:33258ebdb817 92 (char)(gScale | G_ODR_119_BW_14),
benson516 8:16e88babd42a 93 0x03, // Data pass througn HPF and LPF2, default int out
benson516 8:16e88babd42a 94 0x80 // Low-power mode, disable high-pass filter, default cut-off frequency
benson516 8:16e88babd42a 95 };
beanmachine44 0:622e8874902e 96
beanmachine44 0:622e8874902e 97 // Write the data to the gyro control registers
beanmachine44 0:622e8874902e 98 i2c.write(xgAddress, cmd, 4);
beanmachine44 0:622e8874902e 99 }
beanmachine44 0:622e8874902e 100
beanmachine44 0:622e8874902e 101 void LSM9DS1::initAccel()
beanmachine44 0:622e8874902e 102 {
beanmachine44 0:622e8874902e 103 char cmd[4] = {
beanmachine44 0:622e8874902e 104 CTRL_REG5_XL,
beanmachine44 0:622e8874902e 105 0x38, // Enable all axis and don't decimate data in out Registers
gr66 9:33258ebdb817 106 //gr
gr66 9:33258ebdb817 107 (char)((A_ODR_119 << 5) | (aScale << 3) | (A_BW_AUTO_SCALE)), // 119 Hz ODR, set scale, and auto BW
beanmachine44 0:622e8874902e 108 0 // Default resolution mode and filtering settings
beanmachine44 0:622e8874902e 109 };
beanmachine44 0:622e8874902e 110
beanmachine44 0:622e8874902e 111 // Write the data to the accel control registers
beanmachine44 0:622e8874902e 112 i2c.write(xgAddress, cmd, 4);
beanmachine44 0:622e8874902e 113 }
beanmachine44 0:622e8874902e 114
beanmachine44 0:622e8874902e 115 void LSM9DS1::initMag()
benson516 8:16e88babd42a 116 {
beanmachine44 0:622e8874902e 117 char cmd[4] = {
beanmachine44 0:622e8874902e 118 CTRL_REG1_M,
beanmachine44 0:622e8874902e 119 0x10, // Default data rate, xy axes mode, and temp comp
gr66 9:33258ebdb817 120 // gr
gr66 9:33258ebdb817 121 (char)(mScale << 5), // Set mag scale
beanmachine44 0:622e8874902e 122 0 // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
beanmachine44 0:622e8874902e 123 };
beanmachine44 0:622e8874902e 124
beanmachine44 0:622e8874902e 125 // Write the data to the mag control registers
beanmachine44 0:622e8874902e 126 i2c.write(mAddress, cmd, 4);
beanmachine44 0:622e8874902e 127 }
beanmachine44 0:622e8874902e 128
5hel2l2y 3:f96b287c0bf7 129 void LSM9DS1::initIntr()
benson516 8:16e88babd42a 130 {
5hel2l2y 3:f96b287c0bf7 131 char cmd[2];
5hel2l2y 4:7ffcb378cfd4 132 uint16_t thresholdG = 500;
5hel2l2y 4:7ffcb378cfd4 133 uint8_t durationG = 10;
5hel2l2y 4:7ffcb378cfd4 134 uint8_t thresholdX = 20;
5hel2l2y 4:7ffcb378cfd4 135 uint8_t durationX = 1;
5hel2l2y 4:7ffcb378cfd4 136 uint16_t thresholdM = 10000;
benson516 8:16e88babd42a 137
5hel2l2y 4:7ffcb378cfd4 138 // 1. Configure the gyro interrupt generator
5hel2l2y 3:f96b287c0bf7 139 cmd[0] = INT_GEN_CFG_G;
5hel2l2y 3:f96b287c0bf7 140 cmd[1] = (1 << 5);
5hel2l2y 3:f96b287c0bf7 141 i2c.write(xgAddress, cmd, 2);
5hel2l2y 4:7ffcb378cfd4 142 // 2. Configure the gyro threshold
5hel2l2y 4:7ffcb378cfd4 143 cmd[0] = INT_GEN_THS_ZH_G;
5hel2l2y 4:7ffcb378cfd4 144 cmd[1] = (thresholdG & 0x7F00) >> 8;
5hel2l2y 3:f96b287c0bf7 145 i2c.write(xgAddress, cmd, 2);
5hel2l2y 4:7ffcb378cfd4 146 cmd[0] = INT_GEN_THS_ZL_G;
5hel2l2y 4:7ffcb378cfd4 147 cmd[1] = (thresholdG & 0x00FF);
5hel2l2y 3:f96b287c0bf7 148 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 149 cmd[0] = INT_GEN_DUR_G;
5hel2l2y 4:7ffcb378cfd4 150 cmd[1] = (durationG & 0x7F) | 0x80;
5hel2l2y 3:f96b287c0bf7 151 i2c.write(xgAddress, cmd, 2);
benson516 8:16e88babd42a 152
5hel2l2y 4:7ffcb378cfd4 153 // 3. Configure accelerometer interrupt generator
5hel2l2y 3:f96b287c0bf7 154 cmd[0] = INT_GEN_CFG_XL;
5hel2l2y 3:f96b287c0bf7 155 cmd[1] = (1 << 1);
5hel2l2y 3:f96b287c0bf7 156 i2c.write(xgAddress, cmd, 2);
5hel2l2y 4:7ffcb378cfd4 157 // 4. Configure accelerometer threshold
5hel2l2y 4:7ffcb378cfd4 158 cmd[0] = INT_GEN_THS_X_XL;
5hel2l2y 4:7ffcb378cfd4 159 cmd[1] = thresholdX;
5hel2l2y 3:f96b287c0bf7 160 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 161 cmd[0] = INT_GEN_DUR_XL;
5hel2l2y 4:7ffcb378cfd4 162 cmd[1] = (durationX & 0x7F);
5hel2l2y 3:f96b287c0bf7 163 i2c.write(xgAddress, cmd, 2);
benson516 8:16e88babd42a 164
5hel2l2y 4:7ffcb378cfd4 165 // 5. Configure INT1 - assign it to gyro interrupt
5hel2l2y 3:f96b287c0bf7 166 cmd[0] = INT1_CTRL;
5hel2l2y 4:7ffcb378cfd4 167 // cmd[1] = 0xC0;
5hel2l2y 3:f96b287c0bf7 168 cmd[1] = (1 << 7) | (1 << 6);
5hel2l2y 3:f96b287c0bf7 169 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 170 cmd[0] = CTRL_REG8;
5hel2l2y 4:7ffcb378cfd4 171 // cmd[1] = 0x04;
5hel2l2y 4:7ffcb378cfd4 172 cmd[1] = (1 << 2) | (1 << 5) | (1 << 4);
5hel2l2y 3:f96b287c0bf7 173 i2c.write(xgAddress, cmd, 2);
benson516 8:16e88babd42a 174
5hel2l2y 4:7ffcb378cfd4 175 // Configure interrupt 2 to fire whenever new accelerometer
5hel2l2y 4:7ffcb378cfd4 176 // or gyroscope data is available.
5hel2l2y 3:f96b287c0bf7 177 cmd[0] = INT2_CTRL;
5hel2l2y 3:f96b287c0bf7 178 cmd[1] = (1 << 0) | (1 << 1);
5hel2l2y 3:f96b287c0bf7 179 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 180 cmd[0] = CTRL_REG8;
5hel2l2y 4:7ffcb378cfd4 181 cmd[1] = (1 << 2) | (1 << 5) | (1 << 4);
5hel2l2y 3:f96b287c0bf7 182 i2c.write(xgAddress, cmd, 2);
benson516 8:16e88babd42a 183
5hel2l2y 4:7ffcb378cfd4 184 // Configure magnetometer interrupt
5hel2l2y 3:f96b287c0bf7 185 cmd[0] = INT_CFG_M;
5hel2l2y 4:7ffcb378cfd4 186 cmd[1] = (1 << 7) | (1 << 0);
5hel2l2y 3:f96b287c0bf7 187 i2c.write(xgAddress, cmd, 2);
benson516 8:16e88babd42a 188
5hel2l2y 4:7ffcb378cfd4 189 // Configure magnetometer threshold
5hel2l2y 3:f96b287c0bf7 190 cmd[0] = INT_THS_H_M;
5hel2l2y 4:7ffcb378cfd4 191 cmd[1] = uint8_t((thresholdM & 0x7F00) >> 8);
5hel2l2y 3:f96b287c0bf7 192 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 193 cmd[0] = INT_THS_L_M;
5hel2l2y 4:7ffcb378cfd4 194 cmd[1] = uint8_t(thresholdM & 0x00FF);
5hel2l2y 3:f96b287c0bf7 195 i2c.write(xgAddress, cmd, 2);
5hel2l2y 3:f96b287c0bf7 196 }
5hel2l2y 3:f96b287c0bf7 197
ChangYuHsuan 6:28c4b3c8b43d 198 void LSM9DS1::calibration()
ChangYuHsuan 6:28c4b3c8b43d 199 {
benson516 8:16e88babd42a 200
ChangYuHsuan 6:28c4b3c8b43d 201 uint16_t samples = 0;
ChangYuHsuan 6:28c4b3c8b43d 202 int32_t aBiasRawTemp[3] = {0, 0, 0};
ChangYuHsuan 6:28c4b3c8b43d 203 int32_t gBiasRawTemp[3] = {0, 0, 0};
benson516 8:16e88babd42a 204 int32_t gDeltaBiasRaw[3] = {0, 0, 0};
benson516 8:16e88babd42a 205
benson516 8:16e88babd42a 206 // Turn off the autoCalib first to get the raw data
benson516 8:16e88babd42a 207 autoCalib = false;
benson516 8:16e88babd42a 208
benson516 8:16e88babd42a 209 while(samples < 200)
ChangYuHsuan 6:28c4b3c8b43d 210 {
ChangYuHsuan 6:28c4b3c8b43d 211 readGyro();
ChangYuHsuan 6:28c4b3c8b43d 212 gBiasRawTemp[0] += gx_raw;
ChangYuHsuan 6:28c4b3c8b43d 213 gBiasRawTemp[1] += gy_raw;
ChangYuHsuan 6:28c4b3c8b43d 214 gBiasRawTemp[2] += gz_raw;
benson516 8:16e88babd42a 215 /*
ChangYuHsuan 6:28c4b3c8b43d 216 readAccel();
ChangYuHsuan 6:28c4b3c8b43d 217 aBiasRawTemp[0] += ax_raw;
ChangYuHsuan 6:28c4b3c8b43d 218 aBiasRawTemp[1] += ay_raw;
ChangYuHsuan 6:28c4b3c8b43d 219 aBiasRawTemp[2] += az_raw;
benson516 8:16e88babd42a 220 */
benson516 8:16e88babd42a 221 wait_ms(1); // 1 ms
benson516 8:16e88babd42a 222 samples++;
ChangYuHsuan 6:28c4b3c8b43d 223 }
benson516 8:16e88babd42a 224
benson516 8:16e88babd42a 225 //
ChangYuHsuan 6:28c4b3c8b43d 226 for(int ii = 0; ii < 3; ii++)
ChangYuHsuan 6:28c4b3c8b43d 227 {
ChangYuHsuan 6:28c4b3c8b43d 228 gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
benson516 8:16e88babd42a 229 // aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
benson516 8:16e88babd42a 230 aBiasRaw[ii] = 0;
benson516 8:16e88babd42a 231
benson516 8:16e88babd42a 232 /*
ChangYuHsuan 6:28c4b3c8b43d 233 gBias[ii] = gBiasRaw[ii] * gRes;
benson516 8:16e88babd42a 234 aBias[ii] = aBiasRaw[ii] * aRes;
benson516 8:16e88babd42a 235 */
benson516 8:16e88babd42a 236 gBiasRawTemp[ii] = 0;
ChangYuHsuan 6:28c4b3c8b43d 237 }
benson516 8:16e88babd42a 238
benson516 8:16e88babd42a 239 // Re-calculate the bias
benson516 8:16e88babd42a 240 int bound_bias = 10;
benson516 8:16e88babd42a 241 int32_t samples_axis[3] = {0, 0, 0};
benson516 8:16e88babd42a 242 for (size_t i = 0; i < 500; ++i){
benson516 8:16e88babd42a 243 readGyro();
benson516 8:16e88babd42a 244 gDeltaBiasRaw[0] = gx_raw - gBiasRaw[0];
benson516 8:16e88babd42a 245 gDeltaBiasRaw[1] = gy_raw - gBiasRaw[1];
benson516 8:16e88babd42a 246 gDeltaBiasRaw[2] = gz_raw - gBiasRaw[2];
benson516 8:16e88babd42a 247 for (size_t k = 0; k < 3; ++k){
benson516 8:16e88babd42a 248 if (gDeltaBiasRaw[k] >= -bound_bias && gDeltaBiasRaw[k] <= bound_bias){
benson516 8:16e88babd42a 249 gBiasRawTemp[k] += gDeltaBiasRaw[k] + gBiasRaw[k];
benson516 8:16e88babd42a 250 samples_axis[k]++;
benson516 8:16e88babd42a 251 }
benson516 8:16e88babd42a 252 }
benson516 8:16e88babd42a 253 //
benson516 8:16e88babd42a 254 wait_ms(1); // 1 ms
benson516 8:16e88babd42a 255 }
benson516 8:16e88babd42a 256
benson516 8:16e88babd42a 257 //
benson516 8:16e88babd42a 258 for(int ii = 0; ii < 3; ii++)
benson516 8:16e88babd42a 259 {
benson516 8:16e88babd42a 260 gBiasRaw[ii] = gBiasRawTemp[ii] / samples_axis[ii];
benson516 8:16e88babd42a 261 // aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
benson516 8:16e88babd42a 262 aBiasRaw[ii] = 0;
benson516 8:16e88babd42a 263
benson516 8:16e88babd42a 264 gBias[ii] = gBiasRaw[ii] * gRes;
benson516 8:16e88babd42a 265 aBias[ii] = aBiasRaw[ii] * aRes;
benson516 8:16e88babd42a 266 }
benson516 8:16e88babd42a 267
benson516 8:16e88babd42a 268 // Turn on the autoCalib
ChangYuHsuan 6:28c4b3c8b43d 269 autoCalib = true;
ChangYuHsuan 6:28c4b3c8b43d 270 }
ChangYuHsuan 6:28c4b3c8b43d 271
beanmachine44 0:622e8874902e 272 void LSM9DS1::readAccel()
beanmachine44 0:622e8874902e 273 {
beanmachine44 0:622e8874902e 274 // The data we are going to read from the accel
beanmachine44 0:622e8874902e 275 char data[6];
beanmachine44 0:622e8874902e 276
beanmachine44 0:622e8874902e 277 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 278 char subAddress = OUT_X_L_XL;
beanmachine44 0:622e8874902e 279
beanmachine44 0:622e8874902e 280 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 281 i2c.write(xgAddress, &subAddress, 1, true);
beanmachine44 0:622e8874902e 282 // Read in all 8 bit registers containing the axes data
beanmachine44 0:622e8874902e 283 i2c.read(xgAddress, data, 6);
beanmachine44 0:622e8874902e 284
beanmachine44 0:622e8874902e 285 // Reassemble the data and convert to g
beanmachine44 0:622e8874902e 286 ax_raw = data[0] | (data[1] << 8);
beanmachine44 0:622e8874902e 287 ay_raw = data[2] | (data[3] << 8);
beanmachine44 0:622e8874902e 288 az_raw = data[4] | (data[5] << 8);
benson516 8:16e88babd42a 289
benson516 8:16e88babd42a 290 //
benson516 8:16e88babd42a 291 if(autoCalib)
ChangYuHsuan 6:28c4b3c8b43d 292 {
ChangYuHsuan 6:28c4b3c8b43d 293 ax_raw -= aBiasRaw[0];
ChangYuHsuan 6:28c4b3c8b43d 294 ay_raw -= aBiasRaw[1];
ChangYuHsuan 6:28c4b3c8b43d 295 az_raw -= aBiasRaw[2];
ChangYuHsuan 6:28c4b3c8b43d 296 }
benson516 8:16e88babd42a 297 //
benson516 8:16e88babd42a 298 ax = ax_raw * aRes;
benson516 8:16e88babd42a 299 ay = ay_raw * aRes;
benson516 8:16e88babd42a 300 az = az_raw * aRes;
beanmachine44 0:622e8874902e 301 }
beanmachine44 0:622e8874902e 302
beanmachine44 0:622e8874902e 303 void LSM9DS1::readMag()
beanmachine44 0:622e8874902e 304 {
beanmachine44 0:622e8874902e 305 // The data we are going to read from the mag
beanmachine44 0:622e8874902e 306 char data[6];
beanmachine44 0:622e8874902e 307
beanmachine44 0:622e8874902e 308 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 309 char subAddress = OUT_X_L_M;
beanmachine44 0:622e8874902e 310
beanmachine44 0:622e8874902e 311 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 312 i2c.write(mAddress, &subAddress, 1, true);
beanmachine44 0:622e8874902e 313 // Read in all 8 bit registers containing the axes data
beanmachine44 0:622e8874902e 314 i2c.read(mAddress, data, 6);
beanmachine44 0:622e8874902e 315
beanmachine44 0:622e8874902e 316 // Reassemble the data and convert to degrees
beanmachine44 0:622e8874902e 317 mx_raw = data[0] | (data[1] << 8);
beanmachine44 0:622e8874902e 318 my_raw = data[2] | (data[3] << 8);
beanmachine44 0:622e8874902e 319 mz_raw = data[4] | (data[5] << 8);
beanmachine44 0:622e8874902e 320 mx = mx_raw * mRes;
beanmachine44 0:622e8874902e 321 my = my_raw * mRes;
beanmachine44 0:622e8874902e 322 mz = mz_raw * mRes;
beanmachine44 0:622e8874902e 323 }
beanmachine44 0:622e8874902e 324
5hel2l2y 3:f96b287c0bf7 325 void LSM9DS1::readIntr()
5hel2l2y 3:f96b287c0bf7 326 {
5hel2l2y 3:f96b287c0bf7 327 char data[1];
5hel2l2y 3:f96b287c0bf7 328 char subAddress = INT_GEN_SRC_G;
5hel2l2y 3:f96b287c0bf7 329
5hel2l2y 3:f96b287c0bf7 330 i2c.write(xgAddress, &subAddress, 1, true);
5hel2l2y 3:f96b287c0bf7 331 i2c.read(xgAddress, data, 1);
5hel2l2y 3:f96b287c0bf7 332
5hel2l2y 3:f96b287c0bf7 333 intr = (float)data[0];
5hel2l2y 3:f96b287c0bf7 334 }
5hel2l2y 3:f96b287c0bf7 335
beanmachine44 0:622e8874902e 336 void LSM9DS1::readTemp()
beanmachine44 0:622e8874902e 337 {
beanmachine44 0:622e8874902e 338 // The data we are going to read from the temp
beanmachine44 0:622e8874902e 339 char data[2];
beanmachine44 0:622e8874902e 340
beanmachine44 0:622e8874902e 341 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 342 char subAddress = OUT_TEMP_L;
beanmachine44 0:622e8874902e 343
beanmachine44 0:622e8874902e 344 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 345 i2c.write(xgAddress, &subAddress, 1, true);
beanmachine44 0:622e8874902e 346 // Read in all 8 bit registers containing the axes data
beanmachine44 0:622e8874902e 347 i2c.read(xgAddress, data, 2);
beanmachine44 0:622e8874902e 348
benson516 8:16e88babd42a 349 // Temperature is a 12-bit signed integer
beanmachine44 0:622e8874902e 350 temperature_raw = data[0] | (data[1] << 8);
beanmachine44 0:622e8874902e 351
beanmachine44 0:622e8874902e 352 temperature_c = (float)temperature_raw / 8.0 + 25;
beanmachine44 0:622e8874902e 353 temperature_f = temperature_c * 1.8 + 32;
beanmachine44 0:622e8874902e 354 }
beanmachine44 0:622e8874902e 355
beanmachine44 0:622e8874902e 356 void LSM9DS1::readGyro()
beanmachine44 0:622e8874902e 357 {
beanmachine44 0:622e8874902e 358 // The data we are going to read from the gyro
beanmachine44 0:622e8874902e 359 char data[6];
beanmachine44 0:622e8874902e 360
beanmachine44 0:622e8874902e 361 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 362 char subAddress = OUT_X_L_G;
beanmachine44 0:622e8874902e 363
beanmachine44 0:622e8874902e 364 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 365 i2c.write(xgAddress, &subAddress, 1, true);
ChangYuHsuan 7:e6e3d320eb6c 366 // i2c.write(xgAddress, &subAddress, 1);
beanmachine44 0:622e8874902e 367 // Read in all 8 bit registers containing the axes data
beanmachine44 0:622e8874902e 368 i2c.read(xgAddress, data, 6);
beanmachine44 0:622e8874902e 369
beanmachine44 0:622e8874902e 370 // Reassemble the data and convert to degrees/sec
beanmachine44 0:622e8874902e 371 gx_raw = data[0] | (data[1] << 8);
beanmachine44 0:622e8874902e 372 gy_raw = data[2] | (data[3] << 8);
beanmachine44 0:622e8874902e 373 gz_raw = data[4] | (data[5] << 8);
benson516 8:16e88babd42a 374
benson516 8:16e88babd42a 375 //
benson516 8:16e88babd42a 376 if(autoCalib)
ChangYuHsuan 6:28c4b3c8b43d 377 {
ChangYuHsuan 6:28c4b3c8b43d 378 gx_raw -= gBiasRaw[0];
ChangYuHsuan 6:28c4b3c8b43d 379 gy_raw -= gBiasRaw[1];
ChangYuHsuan 6:28c4b3c8b43d 380 gz_raw -= gBiasRaw[2];
ChangYuHsuan 6:28c4b3c8b43d 381 }
benson516 8:16e88babd42a 382 //
benson516 8:16e88babd42a 383 gx = gx_raw * gRes;
benson516 8:16e88babd42a 384 gy = gy_raw * gRes;
benson516 8:16e88babd42a 385 gz = gz_raw * gRes;
benson516 8:16e88babd42a 386
beanmachine44 0:622e8874902e 387 }
beanmachine44 0:622e8874902e 388
beanmachine44 0:622e8874902e 389 void LSM9DS1::setGyroScale(gyro_scale gScl)
beanmachine44 0:622e8874902e 390 {
beanmachine44 0:622e8874902e 391 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 392 char cmd[2] = {
beanmachine44 0:622e8874902e 393 CTRL_REG1_G,
beanmachine44 0:622e8874902e 394 0
beanmachine44 0:622e8874902e 395 };
beanmachine44 0:622e8874902e 396
beanmachine44 0:622e8874902e 397 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 398 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 399 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 400 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 401
beanmachine44 0:622e8874902e 402 // Then mask out the gyro scale bits:
beanmachine44 0:622e8874902e 403 cmd[1] &= 0xFF^(0x3 << 3);
beanmachine44 0:622e8874902e 404 // Then shift in our new scale bits:
beanmachine44 0:622e8874902e 405 cmd[1] |= gScl << 3;
beanmachine44 0:622e8874902e 406
beanmachine44 0:622e8874902e 407 // Write the gyroscale out to the gyro
beanmachine44 0:622e8874902e 408 i2c.write(xgAddress, cmd, 2);
benson516 8:16e88babd42a 409
beanmachine44 0:622e8874902e 410 // We've updated the sensor, but we also need to update our class variables
beanmachine44 0:622e8874902e 411 // First update gScale:
beanmachine44 0:622e8874902e 412 gScale = gScl;
beanmachine44 0:622e8874902e 413 // Then calculate a new gRes, which relies on gScale being set correctly:
beanmachine44 0:622e8874902e 414 calcgRes();
beanmachine44 0:622e8874902e 415 }
beanmachine44 0:622e8874902e 416
beanmachine44 0:622e8874902e 417 void LSM9DS1::setAccelScale(accel_scale aScl)
beanmachine44 0:622e8874902e 418 {
beanmachine44 0:622e8874902e 419 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 420 char cmd[2] = {
beanmachine44 0:622e8874902e 421 CTRL_REG6_XL,
beanmachine44 0:622e8874902e 422 0
beanmachine44 0:622e8874902e 423 };
beanmachine44 0:622e8874902e 424
beanmachine44 0:622e8874902e 425 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 426 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 427 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 428 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 429
beanmachine44 0:622e8874902e 430 // Then mask out the accel scale bits:
beanmachine44 0:622e8874902e 431 cmd[1] &= 0xFF^(0x3 << 3);
beanmachine44 0:622e8874902e 432 // Then shift in our new scale bits:
beanmachine44 0:622e8874902e 433 cmd[1] |= aScl << 3;
beanmachine44 0:622e8874902e 434
beanmachine44 0:622e8874902e 435 // Write the accelscale out to the accel
beanmachine44 0:622e8874902e 436 i2c.write(xgAddress, cmd, 2);
benson516 8:16e88babd42a 437
beanmachine44 0:622e8874902e 438 // We've updated the sensor, but we also need to update our class variables
beanmachine44 0:622e8874902e 439 // First update aScale:
beanmachine44 0:622e8874902e 440 aScale = aScl;
beanmachine44 0:622e8874902e 441 // Then calculate a new aRes, which relies on aScale being set correctly:
beanmachine44 0:622e8874902e 442 calcaRes();
beanmachine44 0:622e8874902e 443 }
beanmachine44 0:622e8874902e 444
beanmachine44 0:622e8874902e 445 void LSM9DS1::setMagScale(mag_scale mScl)
beanmachine44 0:622e8874902e 446 {
beanmachine44 0:622e8874902e 447 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 448 char cmd[2] = {
beanmachine44 0:622e8874902e 449 CTRL_REG2_M,
beanmachine44 0:622e8874902e 450 0
beanmachine44 0:622e8874902e 451 };
beanmachine44 0:622e8874902e 452
beanmachine44 0:622e8874902e 453 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 454 i2c.write(mAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 455 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 456 i2c.read(mAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 457
beanmachine44 0:622e8874902e 458 // Then mask out the mag scale bits:
beanmachine44 0:622e8874902e 459 cmd[1] &= 0xFF^(0x3 << 5);
beanmachine44 0:622e8874902e 460 // Then shift in our new scale bits:
beanmachine44 0:622e8874902e 461 cmd[1] |= mScl << 5;
beanmachine44 0:622e8874902e 462
beanmachine44 0:622e8874902e 463 // Write the magscale out to the mag
beanmachine44 0:622e8874902e 464 i2c.write(mAddress, cmd, 2);
benson516 8:16e88babd42a 465
beanmachine44 0:622e8874902e 466 // We've updated the sensor, but we also need to update our class variables
beanmachine44 0:622e8874902e 467 // First update mScale:
beanmachine44 0:622e8874902e 468 mScale = mScl;
beanmachine44 0:622e8874902e 469 // Then calculate a new mRes, which relies on mScale being set correctly:
beanmachine44 0:622e8874902e 470 calcmRes();
beanmachine44 0:622e8874902e 471 }
beanmachine44 0:622e8874902e 472
beanmachine44 0:622e8874902e 473 void LSM9DS1::setGyroODR(gyro_odr gRate)
beanmachine44 0:622e8874902e 474 {
5hel2l2y 2:ac3b69ccd3dd 475 char cmd[2];
5hel2l2y 2:ac3b69ccd3dd 476 char cmdLow[2];
benson516 8:16e88babd42a 477
benson516 8:16e88babd42a 478 // Enable the low-power mode
5hel2l2y 2:ac3b69ccd3dd 479 if(gRate == G_ODR_15_BW_0 | gRate == G_ODR_60_BW_16 | gRate == G_ODR_119_BW_14 | gRate == G_ODR_119_BW_31) {
5hel2l2y 2:ac3b69ccd3dd 480 cmdLow[0] = CTRL_REG3_G;
benson516 8:16e88babd42a 481 cmdLow[1] = 1<<7; // LP_mode
benson516 8:16e88babd42a 482
5hel2l2y 2:ac3b69ccd3dd 483 i2c.write(xgAddress, cmdLow, 2);
5hel2l2y 2:ac3b69ccd3dd 484 }
benson516 8:16e88babd42a 485
beanmachine44 0:622e8874902e 486 // The start of the addresses we want to read from
5hel2l2y 2:ac3b69ccd3dd 487 cmd[0] = CTRL_REG1_G;
5hel2l2y 2:ac3b69ccd3dd 488 cmd[1] = 0;
beanmachine44 0:622e8874902e 489
beanmachine44 0:622e8874902e 490 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 491 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 492 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 493 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 494
beanmachine44 0:622e8874902e 495 // Then mask out the gyro odr bits:
beanmachine44 0:622e8874902e 496 cmd[1] &= (0x3 << 3);
beanmachine44 0:622e8874902e 497 // Then shift in our new odr bits:
beanmachine44 0:622e8874902e 498 cmd[1] |= gRate;
beanmachine44 0:622e8874902e 499
beanmachine44 0:622e8874902e 500 // Write the gyroodr out to the gyro
beanmachine44 0:622e8874902e 501 i2c.write(xgAddress, cmd, 2);
beanmachine44 0:622e8874902e 502 }
beanmachine44 0:622e8874902e 503
beanmachine44 0:622e8874902e 504 void LSM9DS1::setAccelODR(accel_odr aRate)
beanmachine44 0:622e8874902e 505 {
beanmachine44 0:622e8874902e 506 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 507 char cmd[2] = {
beanmachine44 0:622e8874902e 508 CTRL_REG6_XL,
beanmachine44 0:622e8874902e 509 0
beanmachine44 0:622e8874902e 510 };
beanmachine44 0:622e8874902e 511
beanmachine44 0:622e8874902e 512 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 513 i2c.write(xgAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 514 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 515 i2c.read(xgAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 516
beanmachine44 0:622e8874902e 517 // Then mask out the accel odr bits:
beanmachine44 0:622e8874902e 518 cmd[1] &= 0xFF^(0x7 << 5);
beanmachine44 0:622e8874902e 519 // Then shift in our new odr bits:
beanmachine44 0:622e8874902e 520 cmd[1] |= aRate << 5;
beanmachine44 0:622e8874902e 521
beanmachine44 0:622e8874902e 522 // Write the accelodr out to the accel
beanmachine44 0:622e8874902e 523 i2c.write(xgAddress, cmd, 2);
beanmachine44 0:622e8874902e 524 }
beanmachine44 0:622e8874902e 525
beanmachine44 0:622e8874902e 526 void LSM9DS1::setMagODR(mag_odr mRate)
beanmachine44 0:622e8874902e 527 {
beanmachine44 0:622e8874902e 528 // The start of the addresses we want to read from
beanmachine44 0:622e8874902e 529 char cmd[2] = {
beanmachine44 0:622e8874902e 530 CTRL_REG1_M,
beanmachine44 0:622e8874902e 531 0
beanmachine44 0:622e8874902e 532 };
beanmachine44 0:622e8874902e 533
beanmachine44 0:622e8874902e 534 // Write the address we are going to read from and don't end the transaction
beanmachine44 0:622e8874902e 535 i2c.write(mAddress, cmd, 1, true);
beanmachine44 0:622e8874902e 536 // Read in all the 8 bits of data
beanmachine44 0:622e8874902e 537 i2c.read(mAddress, cmd+1, 1);
beanmachine44 0:622e8874902e 538
beanmachine44 0:622e8874902e 539 // Then mask out the mag odr bits:
beanmachine44 0:622e8874902e 540 cmd[1] &= 0xFF^(0x7 << 2);
beanmachine44 0:622e8874902e 541 // Then shift in our new odr bits:
beanmachine44 0:622e8874902e 542 cmd[1] |= mRate << 2;
beanmachine44 0:622e8874902e 543
beanmachine44 0:622e8874902e 544 // Write the magodr out to the mag
beanmachine44 0:622e8874902e 545 i2c.write(mAddress, cmd, 2);
beanmachine44 0:622e8874902e 546 }
beanmachine44 0:622e8874902e 547
beanmachine44 0:622e8874902e 548 void LSM9DS1::calcgRes()
beanmachine44 0:622e8874902e 549 {
beanmachine44 0:622e8874902e 550 // Possible gyro scales (and their register bit settings) are:
beanmachine44 0:622e8874902e 551 // 245 DPS (00), 500 DPS (01), 2000 DPS (10).
beanmachine44 0:622e8874902e 552 switch (gScale)
beanmachine44 0:622e8874902e 553 {
beanmachine44 0:622e8874902e 554 case G_SCALE_245DPS:
benson516 8:16e88babd42a 555 // gRes = 245.0 / 32768.0;
benson516 8:16e88babd42a 556 gRes = 8.75*0.001; // deg./sec.
beanmachine44 0:622e8874902e 557 break;
beanmachine44 0:622e8874902e 558 case G_SCALE_500DPS:
benson516 8:16e88babd42a 559 // gRes = 500.0 / 32768.0;
benson516 8:16e88babd42a 560 gRes = 17.50*0.001; // deg./sec.
beanmachine44 0:622e8874902e 561 break;
beanmachine44 0:622e8874902e 562 case G_SCALE_2000DPS:
benson516 8:16e88babd42a 563 // gRes = 2000.0 / 32768.0;
benson516 8:16e88babd42a 564 gRes = 70.0*0.001; // deg./sec.
beanmachine44 0:622e8874902e 565 break;
beanmachine44 0:622e8874902e 566 }
beanmachine44 0:622e8874902e 567 }
beanmachine44 0:622e8874902e 568
beanmachine44 0:622e8874902e 569 void LSM9DS1::calcaRes()
beanmachine44 0:622e8874902e 570 {
beanmachine44 0:622e8874902e 571 // Possible accelerometer scales (and their register bit settings) are:
beanmachine44 0:622e8874902e 572 // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
beanmachine44 0:622e8874902e 573 switch (aScale)
beanmachine44 0:622e8874902e 574 {
beanmachine44 0:622e8874902e 575 case A_SCALE_2G:
beanmachine44 0:622e8874902e 576 aRes = 2.0 / 32768.0;
beanmachine44 0:622e8874902e 577 break;
beanmachine44 0:622e8874902e 578 case A_SCALE_4G:
beanmachine44 0:622e8874902e 579 aRes = 4.0 / 32768.0;
beanmachine44 0:622e8874902e 580 break;
beanmachine44 0:622e8874902e 581 case A_SCALE_8G:
beanmachine44 0:622e8874902e 582 aRes = 8.0 / 32768.0;
beanmachine44 0:622e8874902e 583 break;
beanmachine44 0:622e8874902e 584 case A_SCALE_16G:
benson516 8:16e88babd42a 585 // aRes = 16.0 / 32768.0;
benson516 8:16e88babd42a 586 aRes = 0.732*0.001; // g (gravity)
beanmachine44 0:622e8874902e 587 break;
beanmachine44 0:622e8874902e 588 }
beanmachine44 0:622e8874902e 589 }
beanmachine44 0:622e8874902e 590
beanmachine44 0:622e8874902e 591 void LSM9DS1::calcmRes()
beanmachine44 0:622e8874902e 592 {
beanmachine44 0:622e8874902e 593 // Possible magnetometer scales (and their register bit settings) are:
benson516 8:16e88babd42a 594 // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11).
beanmachine44 0:622e8874902e 595 switch (mScale)
beanmachine44 0:622e8874902e 596 {
beanmachine44 0:622e8874902e 597 case M_SCALE_4GS:
benson516 8:16e88babd42a 598 // mRes = 4.0 / 32768.0;
benson516 8:16e88babd42a 599 mRes = 0.14*0.001; // gauss
beanmachine44 0:622e8874902e 600 break;
beanmachine44 0:622e8874902e 601 case M_SCALE_8GS:
benson516 8:16e88babd42a 602 // mRes = 8.0 / 32768.0;
benson516 8:16e88babd42a 603 mRes = 0.29*0.001; // gauss
beanmachine44 0:622e8874902e 604 break;
beanmachine44 0:622e8874902e 605 case M_SCALE_12GS:
benson516 8:16e88babd42a 606 // mRes = 12.0 / 32768.0;
benson516 8:16e88babd42a 607 mRes = 0.43*0.001; // gauss
beanmachine44 0:622e8874902e 608 break;
beanmachine44 0:622e8874902e 609 case M_SCALE_16GS:
benson516 8:16e88babd42a 610 // mRes = 16.0 / 32768.0;
benson516 8:16e88babd42a 611 mRes = 0.58*0.001; // gauss
beanmachine44 0:622e8874902e 612 break;
beanmachine44 0:622e8874902e 613 }
ChangYuHsuan 6:28c4b3c8b43d 614 }
ChangYuHsuan 6:28c4b3c8b43d 615
ChangYuHsuan 6:28c4b3c8b43d 616
ChangYuHsuan 6:28c4b3c8b43d 617 /*
ChangYuHsuan 6:28c4b3c8b43d 618 void LSM9DS1::enableXgFIFO(bool enable)
ChangYuHsuan 6:28c4b3c8b43d 619 {
ChangYuHsuan 6:28c4b3c8b43d 620 char cmd[2] = {CTRL_REG9, 0};
benson516 8:16e88babd42a 621
ChangYuHsuan 6:28c4b3c8b43d 622 i2c.write(xgAddress, cmd, 1);
ChangYuHsuan 6:28c4b3c8b43d 623 cmd[1] = i2c.read(CTRL_REG9);
benson516 8:16e88babd42a 624
ChangYuHsuan 6:28c4b3c8b43d 625 if (enable) cmd[1] |= (1<<1);
ChangYuHsuan 6:28c4b3c8b43d 626 else cmd[1] &= ~(1<<1);
benson516 8:16e88babd42a 627
ChangYuHsuan 6:28c4b3c8b43d 628 i2c.write(xgAddress, cmd, 2);
ChangYuHsuan 6:28c4b3c8b43d 629 }
ChangYuHsuan 6:28c4b3c8b43d 630
ChangYuHsuan 6:28c4b3c8b43d 631 void LSM9DS1::setXgFIFO(uint8_t fifoMode, uint8_t fifoThs)
ChangYuHsuan 6:28c4b3c8b43d 632 {
ChangYuHsuan 6:28c4b3c8b43d 633 // Limit threshold - 0x1F (31) is the maximum. If more than that was asked
ChangYuHsuan 6:28c4b3c8b43d 634 // limit it to the maximum.
ChangYuHsuan 6:28c4b3c8b43d 635 char cmd[2] = {FIFO_CTRL, 0};
ChangYuHsuan 6:28c4b3c8b43d 636 uint8_t threshold = fifoThs <= 0x1F ? fifoThs : 0x1F;
ChangYuHsuan 6:28c4b3c8b43d 637 cmd[1] = ((fifoMode & 0x7) << 5) | (threshold & 0x1F);
ChangYuHsuan 6:28c4b3c8b43d 638 i2c.write(xgAddress, cmd, 2);
ChangYuHsuan 6:28c4b3c8b43d 639 }
benson516 8:16e88babd42a 640 */