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