LSM9DS1.cpp

00001 #include "LSM9DS1.h"
00002 
00003 LSM9DS1::LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr) : i2c(sda, scl)
00004 {
00005     // xgAddress and mAddress will store the 7-bit I2C address, if using I2C.
00006     xgAddress = xgAddr;
00007     mAddress = mAddr;
00008 }
00009 
00010 bool LSM9DS1::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
00011                         gyro_odr gODR, accel_odr aODR, mag_odr mODR)
00012 {
00013     // Store the given scales in class variables. These scale variables
00014     // are used throughout to calculate the actual g's, DPS,and Gs's.
00015     gScale = gScl;
00016     aScale = aScl;
00017     mScale = mScl;
00018 
00019     // Once we have the scale values, we can calculate the resolution
00020     // of each sensor. That's what these functions are for. One for each sensor
00021     calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
00022     calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
00023     calcaRes(); // Calculate g / ADC tick, stored in aRes variable
00024 
00025 
00026     // To verify communication, we can read from the WHO_AM_I register of
00027     // each device. Store those in a variable so we can return them.
00028     // The start of the addresses we want to read from
00029     char cmd[2] = {
00030         WHO_AM_I_XG,
00031         0
00032     };
00033 
00034     // Write the address we are going to read from and don't end the transaction
00035     i2c.write(xgAddress, cmd, 1, true);
00036     // Read in all the 8 bits of data
00037     i2c.read(xgAddress, cmd+1, 1);
00038     uint8_t xgTest = cmd[1];                    // Read the accel/gyro WHO_AM_I
00039 
00040     // Reset to the address of the mag who am i
00041     cmd[0] = WHO_AM_I_M;
00042     // Write the address we are going to read from and don't end the transaction
00043     i2c.write(mAddress, cmd, 1, true);
00044     // Read in all the 8 bits of data
00045     i2c.read(mAddress, cmd+1, 1);
00046     uint8_t mTest = cmd[1];      // Read the mag WHO_AM_I
00047 
00048     for(int ii = 0; ii < 3; ii++)
00049     {
00050         gBiasRaw[ii] = 0;
00051         aBiasRaw[ii] = 0;
00052         gBias[ii] = 0;
00053         aBias[ii] = 0;
00054     }
00055     autoCalib = false;
00056 
00057     // Accelerometer initialization stuff:
00058     initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
00059     setAccelODR(aODR); // Set the accel data rate.
00060     setAccelScale(aScale); // Set the accel range.
00061 
00062     // Gyro initialization stuff:
00063     initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
00064     setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
00065     setGyroScale(gScale); // Set the gyro range
00066 
00067     // Magnetometer initialization stuff:
00068     initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
00069     setMagODR(mODR); // Set the magnetometer output data rate.
00070     setMagScale(mScale); // Set the magnetometer's range.
00071 
00072     // Interrupt initialization stuff
00073     initIntr();
00074 
00075     // Once everything is initialized, return the WHO_AM_I registers we read:
00076     return ( ((xgTest << 8) | mTest) == (WHO_AM_I_AG_RSP << 8 | WHO_AM_I_M_RSP) );
00077 }
00078 
00079 void LSM9DS1::initGyro()
00080 {
00081     /*
00082     char cmd[4] = {
00083         CTRL_REG1_G,
00084         gScale | G_ODR_119_BW_14,
00085         0,          // Default data out and int out
00086         0           // Default power mode and high pass settings
00087     };
00088     */
00089 
00090     char cmd[4] = {
00091         CTRL_REG1_G,
00092         gScale | G_ODR_119_BW_14,
00093         0x03,          // Data pass througn HPF and LPF2, default int out
00094         0x80           // Low-power mode, disable high-pass filter, default cut-off frequency
00095     };
00096 
00097     // Write the data to the gyro control registers
00098     i2c.write(xgAddress, cmd, 4);
00099 }
00100 
00101 void LSM9DS1::initAccel()
00102 {
00103     char cmd[4] = {
00104         CTRL_REG5_XL,
00105         0x38,       // Enable all axis and don't decimate data in out Registers
00106         (A_ODR_119 << 5) | (aScale << 3) | (A_BW_AUTO_SCALE),   // 119 Hz ODR, set scale, and auto BW
00107         0           // Default resolution mode and filtering settings
00108     };
00109 
00110     // Write the data to the accel control registers
00111     i2c.write(xgAddress, cmd, 4);
00112 }
00113 
00114 void LSM9DS1::initMag()
00115 {
00116     char cmd[4] = {
00117         CTRL_REG1_M,
00118         0x10,       // Default data rate, xy axes mode, and temp comp
00119         mScale << 5, // Set mag scale
00120         0           // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
00121     };
00122 
00123     // Write the data to the mag control registers
00124     i2c.write(mAddress, cmd, 4);
00125 }
00126 
00127 void LSM9DS1::initIntr()
00128 {
00129     char cmd[2];
00130     uint16_t thresholdG = 500;
00131     uint8_t durationG = 10;
00132     uint8_t thresholdX = 20;
00133     uint8_t durationX = 1;
00134     uint16_t thresholdM = 10000;
00135 
00136     // 1. Configure the gyro interrupt generator
00137     cmd[0] = INT_GEN_CFG_G;
00138     cmd[1] = (1 << 5);
00139     i2c.write(xgAddress, cmd, 2);
00140     // 2. Configure the gyro threshold
00141     cmd[0] = INT_GEN_THS_ZH_G;
00142     cmd[1] = (thresholdG & 0x7F00) >> 8;
00143     i2c.write(xgAddress, cmd, 2);
00144     cmd[0] = INT_GEN_THS_ZL_G;
00145     cmd[1] = (thresholdG & 0x00FF);
00146     i2c.write(xgAddress, cmd, 2);
00147     cmd[0] = INT_GEN_DUR_G;
00148     cmd[1] = (durationG & 0x7F) | 0x80;
00149     i2c.write(xgAddress, cmd, 2);
00150 
00151     // 3. Configure accelerometer interrupt generator
00152     cmd[0] = INT_GEN_CFG_XL;
00153     cmd[1] = (1 << 1);
00154     i2c.write(xgAddress, cmd, 2);
00155     // 4. Configure accelerometer threshold
00156     cmd[0] = INT_GEN_THS_X_XL;
00157     cmd[1] = thresholdX;
00158     i2c.write(xgAddress, cmd, 2);
00159     cmd[0] = INT_GEN_DUR_XL;
00160     cmd[1] = (durationX & 0x7F);
00161     i2c.write(xgAddress, cmd, 2);
00162 
00163     // 5. Configure INT1 - assign it to gyro interrupt
00164     cmd[0] = INT1_CTRL;
00165 //    cmd[1] = 0xC0;
00166     cmd[1] = (1 << 7) | (1 << 6);
00167     i2c.write(xgAddress, cmd, 2);
00168     cmd[0] = CTRL_REG8;
00169 //    cmd[1] = 0x04;
00170     cmd[1] = (1 << 2) | (1 << 5) | (1 << 4);
00171     i2c.write(xgAddress, cmd, 2);
00172 
00173     // Configure interrupt 2 to fire whenever new accelerometer
00174     // or gyroscope data is available.
00175     cmd[0] = INT2_CTRL;
00176     cmd[1] = (1 << 0) | (1 << 1);
00177     i2c.write(xgAddress, cmd, 2);
00178     cmd[0] = CTRL_REG8;
00179     cmd[1] = (1 << 2) | (1 << 5) | (1 << 4);
00180     i2c.write(xgAddress, cmd, 2);
00181 
00182     // Configure magnetometer interrupt
00183     cmd[0] = INT_CFG_M;
00184     cmd[1] = (1 << 7) | (1 << 0);
00185     i2c.write(xgAddress, cmd, 2);
00186 
00187     // Configure magnetometer threshold
00188     cmd[0] = INT_THS_H_M;
00189     cmd[1] = uint8_t((thresholdM & 0x7F00) >> 8);
00190     i2c.write(xgAddress, cmd, 2);
00191     cmd[0] = INT_THS_L_M;
00192     cmd[1] = uint8_t(thresholdM & 0x00FF);
00193     i2c.write(xgAddress, cmd, 2);
00194 }
00195 
00196 void LSM9DS1::calibration()
00197 {
00198 
00199     uint16_t samples = 0;
00200     int32_t aBiasRawTemp[3] = {0, 0, 0};
00201     int32_t gBiasRawTemp[3] = {0, 0, 0};
00202     int32_t gDeltaBiasRaw[3] = {0, 0, 0};
00203 
00204     // Turn off the autoCalib first to get the raw data
00205     autoCalib = false;
00206 
00207     while(samples < 200)
00208     {
00209         readGyro();
00210         gBiasRawTemp[0] += gx_raw;
00211         gBiasRawTemp[1] += gy_raw;
00212         gBiasRawTemp[2] += gz_raw;
00213         /*
00214         readAccel();
00215         aBiasRawTemp[0] += ax_raw;
00216         aBiasRawTemp[1] += ay_raw;
00217         aBiasRawTemp[2] += az_raw;
00218         */
00219         wait_ms(1); // 1 ms
00220         samples++;
00221     }
00222 
00223     //
00224     for(int ii = 0; ii < 3; ii++)
00225     {
00226         gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
00227         // aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
00228         aBiasRaw[ii] = 0;
00229 
00230         /*
00231         gBias[ii] = gBiasRaw[ii] * gRes;
00232         aBias[ii] = aBiasRaw[ii] * aRes;
00233         */
00234         gBiasRawTemp[ii] = 0;
00235     }
00236 
00237     // Re-calculate the bias
00238     int bound_bias = 10;
00239     int32_t samples_axis[3] = {0, 0, 0};
00240     for (size_t i = 0; i < 500; ++i){
00241         readGyro();
00242         gDeltaBiasRaw[0] = gx_raw - gBiasRaw[0];
00243         gDeltaBiasRaw[1] = gy_raw - gBiasRaw[1];
00244         gDeltaBiasRaw[2] = gz_raw - gBiasRaw[2];
00245         for (size_t k = 0; k < 3; ++k){
00246             if (gDeltaBiasRaw[k] >= -bound_bias && gDeltaBiasRaw[k] <= bound_bias){
00247                 gBiasRawTemp[k] += gDeltaBiasRaw[k] + gBiasRaw[k];
00248                 samples_axis[k]++;
00249             }
00250         }
00251         //
00252         wait_ms(1); // 1 ms
00253     }
00254 
00255     //
00256     for(int ii = 0; ii < 3; ii++)
00257     {
00258         gBiasRaw[ii] = gBiasRawTemp[ii] / samples_axis[ii];
00259         // aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
00260         aBiasRaw[ii] = 0;
00261 
00262         gBias[ii] = gBiasRaw[ii] * gRes;
00263         aBias[ii] = aBiasRaw[ii] * aRes;
00264     }
00265 
00266     // Turn on the autoCalib
00267     autoCalib = true;
00268 }
00269 
00270 void LSM9DS1::readAccel()
00271 {
00272     // The data we are going to read from the accel
00273     char data[6];
00274 
00275     // The start of the addresses we want to read from
00276     char subAddress = OUT_X_L_XL;
00277 
00278     // Write the address we are going to read from and don't end the transaction
00279     i2c.write(xgAddress, &subAddress, 1, true);
00280     // Read in all 8 bit registers containing the axes data
00281     i2c.read(xgAddress, data, 6);
00282 
00283     // Reassemble the data and convert to g
00284     ax_raw = data[0] | (data[1] << 8);
00285     ay_raw = data[2] | (data[3] << 8);
00286     az_raw = data[4] | (data[5] << 8);
00287 
00288     //
00289     if(autoCalib)
00290     {
00291         ax_raw -= aBiasRaw[0];
00292         ay_raw -= aBiasRaw[1];
00293         az_raw -= aBiasRaw[2];
00294     }
00295     //
00296     ax = ax_raw * aRes;
00297     ay = ay_raw * aRes;
00298     az = az_raw * aRes;
00299 }
00300 
00301 void LSM9DS1::readMag()
00302 {
00303     // The data we are going to read from the mag
00304     char data[6];
00305 
00306     // The start of the addresses we want to read from
00307     char subAddress = OUT_X_L_M;
00308 
00309     // Write the address we are going to read from and don't end the transaction
00310     i2c.write(mAddress, &subAddress, 1, true);
00311     // Read in all 8 bit registers containing the axes data
00312     i2c.read(mAddress, data, 6);
00313 
00314     // Reassemble the data and convert to degrees
00315     mx_raw = data[0] | (data[1] << 8);
00316     my_raw = data[2] | (data[3] << 8);
00317     mz_raw = data[4] | (data[5] << 8);
00318     mx = mx_raw * mRes;
00319     my = my_raw * mRes;
00320     mz = mz_raw * mRes;
00321 }
00322 
00323 void LSM9DS1::readIntr()
00324 {
00325     char data[1];
00326     char subAddress = INT_GEN_SRC_G;
00327 
00328     i2c.write(xgAddress, &subAddress, 1, true);
00329     i2c.read(xgAddress, data, 1);
00330 
00331     intr = (float)data[0];
00332 }
00333 
00334 void LSM9DS1::readTemp()
00335 {
00336     // The data we are going to read from the temp
00337     char data[2];
00338 
00339     // The start of the addresses we want to read from
00340     char subAddress = OUT_TEMP_L;
00341 
00342     // Write the address we are going to read from and don't end the transaction
00343     i2c.write(xgAddress, &subAddress, 1, true);
00344     // Read in all 8 bit registers containing the axes data
00345     i2c.read(xgAddress, data, 2);
00346 
00347     // Temperature is a 12-bit signed integer
00348     temperature_raw = data[0] | (data[1] << 8);
00349 
00350     temperature_c = (float)temperature_raw / 8.0 + 25;
00351     temperature_f = temperature_c * 1.8 + 32;
00352 }
00353 
00354 void LSM9DS1::readGyro()
00355 {
00356     // The data we are going to read from the gyro
00357     char data[6];
00358 
00359     // The start of the addresses we want to read from
00360     char subAddress = OUT_X_L_G;
00361 
00362     // Write the address we are going to read from and don't end the transaction
00363     i2c.write(xgAddress, &subAddress, 1, true);
00364     // i2c.write(xgAddress, &subAddress, 1);
00365     // Read in all 8 bit registers containing the axes data
00366     i2c.read(xgAddress, data, 6);
00367 
00368     // Reassemble the data and convert to degrees/sec
00369     gx_raw = data[0] | (data[1] << 8);
00370     gy_raw = data[2] | (data[3] << 8);
00371     gz_raw = data[4] | (data[5] << 8);
00372 
00373     //
00374     if(autoCalib)
00375     {
00376         gx_raw -= gBiasRaw[0];
00377         gy_raw -= gBiasRaw[1];
00378         gz_raw -= gBiasRaw[2];
00379     }
00380     //
00381     gx = gx_raw * gRes;
00382     gy = gy_raw * gRes;
00383     gz = gz_raw * gRes;
00384 
00385 }
00386 
00387 void LSM9DS1::setGyroScale(gyro_scale gScl)
00388 {
00389     // The start of the addresses we want to read from
00390     char cmd[2] = {
00391         CTRL_REG1_G,
00392         0
00393     };
00394 
00395     // Write the address we are going to read from and don't end the transaction
00396     i2c.write(xgAddress, cmd, 1, true);
00397     // Read in all the 8 bits of data
00398     i2c.read(xgAddress, cmd+1, 1);
00399 
00400     // Then mask out the gyro scale bits:
00401     cmd[1] &= 0xFF^(0x3 << 3);
00402     // Then shift in our new scale bits:
00403     cmd[1] |= gScl << 3;
00404 
00405     // Write the gyroscale out to the gyro
00406     i2c.write(xgAddress, cmd, 2);
00407 
00408     // We've updated the sensor, but we also need to update our class variables
00409     // First update gScale:
00410     gScale = gScl;
00411     // Then calculate a new gRes, which relies on gScale being set correctly:
00412     calcgRes();
00413 }
00414 
00415 void LSM9DS1::setAccelScale(accel_scale aScl)
00416 {
00417     // The start of the addresses we want to read from
00418     char cmd[2] = {
00419         CTRL_REG6_XL,
00420         0
00421     };
00422 
00423     // Write the address we are going to read from and don't end the transaction
00424     i2c.write(xgAddress, cmd, 1, true);
00425     // Read in all the 8 bits of data
00426     i2c.read(xgAddress, cmd+1, 1);
00427 
00428     // Then mask out the accel scale bits:
00429     cmd[1] &= 0xFF^(0x3 << 3);
00430     // Then shift in our new scale bits:
00431     cmd[1] |= aScl << 3;
00432 
00433     // Write the accelscale out to the accel
00434     i2c.write(xgAddress, cmd, 2);
00435 
00436     // We've updated the sensor, but we also need to update our class variables
00437     // First update aScale:
00438     aScale = aScl;
00439     // Then calculate a new aRes, which relies on aScale being set correctly:
00440     calcaRes();
00441 }
00442 
00443 void LSM9DS1::setMagScale(mag_scale mScl)
00444 {
00445     // The start of the addresses we want to read from
00446     char cmd[2] = {
00447         CTRL_REG2_M,
00448         0
00449     };
00450 
00451     // Write the address we are going to read from and don't end the transaction
00452     i2c.write(mAddress, cmd, 1, true);
00453     // Read in all the 8 bits of data
00454     i2c.read(mAddress, cmd+1, 1);
00455 
00456     // Then mask out the mag scale bits:
00457     cmd[1] &= 0xFF^(0x3 << 5);
00458     // Then shift in our new scale bits:
00459     cmd[1] |= mScl << 5;
00460 
00461     // Write the magscale out to the mag
00462     i2c.write(mAddress, cmd, 2);
00463 
00464     // We've updated the sensor, but we also need to update our class variables
00465     // First update mScale:
00466     mScale = mScl;
00467     // Then calculate a new mRes, which relies on mScale being set correctly:
00468     calcmRes();
00469 }
00470 
00471 void LSM9DS1::setGyroODR(gyro_odr gRate)
00472 {
00473     char cmd[2];
00474     char cmdLow[2];
00475 
00476     // Enable the low-power mode
00477     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) {
00478         cmdLow[0] = CTRL_REG3_G;
00479         cmdLow[1] = 1<<7; // LP_mode
00480 
00481         i2c.write(xgAddress, cmdLow, 2);
00482     }
00483 
00484     // The start of the addresses we want to read from
00485     cmd[0] = CTRL_REG1_G;
00486     cmd[1] = 0;
00487 
00488     // Write the address we are going to read from and don't end the transaction
00489     i2c.write(xgAddress, cmd, 1, true);
00490     // Read in all the 8 bits of data
00491     i2c.read(xgAddress, cmd+1, 1);
00492 
00493     // Then mask out the gyro odr bits:
00494     cmd[1] &= (0x3 << 3);
00495     // Then shift in our new odr bits:
00496     cmd[1] |= gRate;
00497 
00498     // Write the gyroodr out to the gyro
00499     i2c.write(xgAddress, cmd, 2);
00500 }
00501 
00502 void LSM9DS1::setAccelODR(accel_odr aRate)
00503 {
00504     // The start of the addresses we want to read from
00505     char cmd[2] = {
00506         CTRL_REG6_XL,
00507         0
00508     };
00509 
00510     // Write the address we are going to read from and don't end the transaction
00511     i2c.write(xgAddress, cmd, 1, true);
00512     // Read in all the 8 bits of data
00513     i2c.read(xgAddress, cmd+1, 1);
00514 
00515     // Then mask out the accel odr bits:
00516     cmd[1] &= 0xFF^(0x7 << 5);
00517     // Then shift in our new odr bits:
00518     cmd[1] |= aRate << 5;
00519 
00520     // Write the accelodr out to the accel
00521     i2c.write(xgAddress, cmd, 2);
00522 }
00523 
00524 void LSM9DS1::setMagODR(mag_odr mRate)
00525 {
00526     // The start of the addresses we want to read from
00527     char cmd[2] = {
00528         CTRL_REG1_M,
00529         0
00530     };
00531 
00532     // Write the address we are going to read from and don't end the transaction
00533     i2c.write(mAddress, cmd, 1, true);
00534     // Read in all the 8 bits of data
00535     i2c.read(mAddress, cmd+1, 1);
00536 
00537     // Then mask out the mag odr bits:
00538     cmd[1] &= 0xFF^(0x7 << 2);
00539     // Then shift in our new odr bits:
00540     cmd[1] |= mRate << 2;
00541 
00542     // Write the magodr out to the mag
00543     i2c.write(mAddress, cmd, 2);
00544 }
00545 
00546 void LSM9DS1::calcgRes()
00547 {
00548     // Possible gyro scales (and their register bit settings) are:
00549     // 245 DPS (00), 500 DPS (01), 2000 DPS (10).
00550     switch (gScale)
00551     {
00552         case G_SCALE_245DPS:
00553             // gRes = 245.0 / 32768.0;
00554             gRes = 8.75*0.001; // deg./sec.
00555             break;
00556         case G_SCALE_500DPS:
00557             // gRes = 500.0 / 32768.0;
00558             gRes = 17.50*0.001; // deg./sec.
00559             break;
00560         case G_SCALE_2000DPS:
00561             // gRes = 2000.0 / 32768.0;
00562             gRes = 70.0*0.001; // deg./sec.
00563             break;
00564     }
00565 }
00566 
00567 void LSM9DS1::calcaRes()
00568 {
00569     // Possible accelerometer scales (and their register bit settings) are:
00570     // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
00571     switch (aScale)
00572     {
00573         case A_SCALE_2G:
00574             aRes = 2.0 / 32768.0;
00575             break;
00576         case A_SCALE_4G:
00577             aRes = 4.0 / 32768.0;
00578             break;
00579         case A_SCALE_8G:
00580             aRes = 8.0 / 32768.0;
00581             break;
00582         case A_SCALE_16G:
00583             // aRes = 16.0 / 32768.0;
00584             aRes = 0.732*0.001; // g (gravity)
00585             break;
00586     }
00587 }
00588 
00589 void LSM9DS1::calcmRes()
00590 {
00591     // Possible magnetometer scales (and their register bit settings) are:
00592     // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11).
00593     switch (mScale)
00594     {
00595         case M_SCALE_4GS:
00596             // mRes = 4.0 / 32768.0;
00597             mRes = 0.14*0.001; // gauss
00598             break;
00599         case M_SCALE_8GS:
00600             // mRes = 8.0 / 32768.0;
00601             mRes = 0.29*0.001; // gauss
00602             break;
00603         case M_SCALE_12GS:
00604             // mRes = 12.0 / 32768.0;
00605             mRes = 0.43*0.001; // gauss
00606             break;
00607         case M_SCALE_16GS:
00608             // mRes = 16.0 / 32768.0;
00609             mRes = 0.58*0.001; // gauss
00610             break;
00611     }
00612 }
00613 
00614 
00615 /*
00616 void LSM9DS1::enableXgFIFO(bool enable)
00617 {
00618     char cmd[2] = {CTRL_REG9, 0};
00619 
00620     i2c.write(xgAddress, cmd, 1);
00621     cmd[1] = i2c.read(CTRL_REG9);
00622 
00623     if (enable) cmd[1] |= (1<<1);
00624     else cmd[1] &= ~(1<<1);
00625 
00626     i2c.write(xgAddress, cmd, 2);
00627 }
00628 
00629 void LSM9DS1::setXgFIFO(uint8_t fifoMode, uint8_t fifoThs)
00630 {
00631     // Limit threshold - 0x1F (31) is the maximum. If more than that was asked
00632     // limit it to the maximum.
00633     char cmd[2] = {FIFO_CTRL, 0};
00634     uint8_t threshold = fifoThs <= 0x1F ? fifoThs : 0x1F;
00635     cmd[1] = ((fifoMode & 0x7) << 5) | (threshold & 0x1F);
00636     i2c.write(xgAddress, cmd, 2);
00637 }
00638 */