LSM9DS0.cpp

00001 #include "LSM9DS0.h"
00002 
00003 LSM9DS0::LSM9DS0(PinName sda, PinName scl, uint8_t gAddr, uint8_t xmAddr)
00004 {
00005     // xmAddress and gAddress will store the 7-bit I2C address, if using I2C.
00006     xmAddress = xmAddr;
00007     gAddress = gAddr;
00008     
00009     i2c_ = new I2Cdev(sda, scl);
00010 }
00011 
00012 uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, 
00013                         gyro_odr gODR, accel_odr aODR, mag_odr mODR)
00014 {
00015     // Store the given scales in class variables. These scale variables
00016     // are used throughout to calculate the actual g's, DPS,and Gs's.
00017     gScale = gScl;
00018     aScale = aScl;
00019     mScale = mScl;
00020     
00021     // Once we have the scale values, we can calculate the resolution
00022     // of each sensor. That's what these functions are for. One for each sensor
00023     calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
00024     calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
00025     calcaRes(); // Calculate g / ADC tick, stored in aRes variable
00026     
00027     
00028     // To verify communication, we can read from the WHO_AM_I register of
00029     // each device. Store those in a variable so we can return them.
00030     uint8_t gTest = gReadByte(WHO_AM_I_G);      // Read the gyro WHO_AM_I
00031     uint8_t xmTest = xmReadByte(WHO_AM_I_XM);   // Read the accel/mag WHO_AM_I
00032     
00033     // Gyro initialization stuff:
00034     initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
00035     setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
00036     setGyroScale(gScale); // Set the gyro range
00037     
00038     // Accelerometer initialization stuff:
00039     initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
00040     setAccelODR(aODR); // Set the accel data rate.
00041     setAccelScale(aScale); // Set the accel range.
00042     
00043     // Magnetometer initialization stuff:
00044     initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
00045     setMagODR(mODR); // Set the magnetometer output data rate.
00046     setMagScale(mScale); // Set the magnetometer's range.
00047     
00048     // Once everything is initialized, return the WHO_AM_I registers we read:
00049     return (xmTest << 8) | gTest;
00050 }
00051 
00052 void LSM9DS0::initGyro()
00053 {
00054 
00055     gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
00056     gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency
00057     gWriteByte(CTRL_REG3_G, 0x88);  //Interrupt enabled on both INT_G  and I2_DRDY
00058     gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps
00059     gWriteByte(CTRL_REG5_G, 0x00); //Init default values
00060     
00061 }
00062 
00063 void LSM9DS0::initAccel()
00064 {
00065     xmWriteByte(CTRL_REG0_XM, 0x00);  
00066     xmWriteByte(CTRL_REG1_XM, 0x57); // 50Hz data rate, x/y/z all enabled                                                
00067     xmWriteByte(CTRL_REG2_XM, 0x00); // Set scale to 2g
00068     xmWriteByte(CTRL_REG3_XM, 0x04); // Accelerometer data ready on INT1_XM (0x04)
00069 
00070 }
00071 
00072 void LSM9DS0::initMag()
00073 {   
00074     xmWriteByte(CTRL_REG5_XM, 0x94); // Mag data rate - 100 Hz, enable temperature sensor
00075     xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS
00076     xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
00077     xmWriteByte(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08)
00078     xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
00079 }
00080 
00081 void LSM9DS0::calLSM9DS0(float * gbias, float * abias)
00082 {  
00083   uint8_t data[6] = {0, 0, 0, 0, 0, 0};
00084   int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
00085   int samples, ii;
00086   
00087   // First get gyro bias
00088   uint8_t c = gReadByte(CTRL_REG5_G);
00089   gWriteByte(CTRL_REG5_G, c | 0x40);         // Enable gyro FIFO  
00090   wait_ms(20);                                 // Wait for change to take effect
00091   gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
00092   wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
00093   
00094   samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
00095 
00096   for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
00097 
00098     data[0] = gReadByte(OUT_X_L_G);
00099     data[1] = gReadByte(OUT_X_H_G);
00100     data[2] = gReadByte(OUT_Y_L_G);
00101     data[3] = gReadByte(OUT_Y_H_G);
00102     data[4] = gReadByte(OUT_Z_L_G);
00103     data[5] = gReadByte(OUT_Z_H_G);
00104   
00105     gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
00106     gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
00107     gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
00108   }  
00109 
00110   gyro_bias[0] /= samples; // average the data
00111   gyro_bias[1] /= samples; 
00112   gyro_bias[2] /= samples; 
00113   
00114   gbias[0] = (float)gyro_bias[0]*gRes;  // Properly scale the data to get deg/s
00115   gbias[1] = (float)gyro_bias[1]*gRes;
00116   gbias[2] = (float)gyro_bias[2]*gRes;
00117   
00118   c = gReadByte(CTRL_REG5_G);
00119   gWriteByte(CTRL_REG5_G, c & ~0x40);  // Disable gyro FIFO  
00120   wait_ms(20);
00121   gWriteByte(FIFO_CTRL_REG_G, 0x00);   // Enable gyro bypass mode
00122   
00123   //  Now get the accelerometer biases
00124   c = xmReadByte(CTRL_REG0_XM);
00125   xmWriteByte(CTRL_REG0_XM, c | 0x40);      // Enable accelerometer FIFO  
00126   wait_ms(20);                                // Wait for change to take effect
00127   xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F);  // Enable accelerometer FIFO stream mode and set watermark at 32 samples
00128   wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
00129 
00130   samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
00131 
00132    for(ii = 0; ii < samples ; ii++) {          // Read the accelerometer data stored in the FIFO
00133    
00134     data[0] = xmReadByte(OUT_X_L_A);
00135     data[1] = xmReadByte(OUT_X_H_A);
00136     data[2] = xmReadByte(OUT_Y_L_A);
00137     data[3] = xmReadByte(OUT_Y_H_A);
00138     data[4] = xmReadByte(OUT_Z_L_A);
00139     data[5] = xmReadByte(OUT_Z_H_A);
00140     accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
00141     accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
00142     accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1./aRes); // Assumes sensor facing up!
00143   }  
00144 
00145   accel_bias[0] /= samples; // average the data
00146   accel_bias[1] /= samples; 
00147   accel_bias[2] /= samples; 
00148   
00149   abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
00150   abias[1] = (float)accel_bias[1]*aRes;
00151   abias[2] = (float)accel_bias[2]*aRes;
00152 
00153   c = xmReadByte(CTRL_REG0_XM);
00154   xmWriteByte(CTRL_REG0_XM, c & ~0x40);    // Disable accelerometer FIFO  
00155   wait_ms(20);
00156   xmWriteByte(FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
00157   
00158 }
00159 void LSM9DS0::readAccel()
00160 {
00161   uint16_t Temp = 0;
00162   
00163   //Get x
00164   Temp = xmReadByte(OUT_X_H_A);
00165   Temp = Temp<<8;
00166   Temp |= xmReadByte(OUT_X_L_A);
00167   ax = Temp;
00168   
00169   
00170   //Get y
00171   Temp=0;
00172   Temp = xmReadByte(OUT_Y_H_A);
00173   Temp = Temp<<8;
00174   Temp |= xmReadByte(OUT_Y_L_A);
00175   ay = Temp;
00176   
00177   //Get z
00178   Temp=0;
00179   Temp = xmReadByte(OUT_Z_H_A);
00180   Temp = Temp<<8;
00181   Temp |= xmReadByte(OUT_Z_L_A);
00182   az = Temp;
00183   
00184 }
00185 
00186 void LSM9DS0::readMag()
00187 {
00188   uint16_t Temp = 0;  
00189 
00190   //Get x
00191   Temp = xmReadByte(OUT_X_H_M);
00192   Temp = Temp<<8;
00193   Temp |= xmReadByte(OUT_X_L_M);
00194   mx = Temp;
00195   
00196   
00197   //Get y
00198   Temp=0;
00199   Temp = xmReadByte(OUT_Y_H_M);
00200   Temp = Temp<<8;
00201   Temp |= xmReadByte(OUT_Y_L_M);
00202   my = Temp;
00203   
00204   //Get z
00205   Temp=0;
00206   Temp = xmReadByte(OUT_Z_H_M);
00207   Temp = Temp<<8;
00208   Temp |= xmReadByte(OUT_Z_L_M);
00209   mz = Temp;
00210 }
00211 
00212 void LSM9DS0::readTemp()
00213 {
00214     uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
00215     
00216     temp[0] = xmReadByte(OUT_TEMP_L_XM);
00217     temp[1] = xmReadByte(OUT_TEMP_H_XM);
00218     
00219     temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
00220 }
00221 
00222 
00223 void LSM9DS0::readGyro()
00224 {   
00225   uint16_t Temp = 0;
00226 
00227   //Get x
00228   Temp = gReadByte(OUT_X_H_G);
00229   Temp = Temp<<8;
00230   Temp |= gReadByte(OUT_X_L_G);
00231   gx = Temp;
00232   
00233   
00234   //Get y
00235   Temp=0;
00236   Temp = gReadByte(OUT_Y_H_G);
00237   Temp = Temp<<8;
00238   Temp |= gReadByte(OUT_Y_L_G);
00239   gy = Temp;
00240   
00241   //Get z
00242   Temp=0;
00243   Temp = gReadByte(OUT_Z_H_G);
00244   Temp = Temp<<8;
00245   Temp |= gReadByte(OUT_Z_L_G);
00246   gz = Temp;
00247 }
00248 
00249 float LSM9DS0::calcGyro(int16_t gyro)
00250 {
00251     // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
00252     return gRes * gyro; 
00253 }
00254 
00255 float LSM9DS0::calcAccel(int16_t accel)
00256 {
00257     // Return the accel raw reading times our pre-calculated g's / (ADC tick):
00258     return aRes * accel;
00259 }
00260 
00261 float LSM9DS0::calcMag(int16_t mag)
00262 {
00263     // Return the mag raw reading times our pre-calculated Gs / (ADC tick):
00264     return mRes * mag;
00265 }
00266 
00267 void LSM9DS0::setGyroScale(gyro_scale gScl)
00268 {
00269     // We need to preserve the other bytes in CTRL_REG4_G. So, first read it:
00270     uint8_t temp = gReadByte(CTRL_REG4_G);
00271     // Then mask out the gyro scale bits:
00272     temp &= 0xFF^(0x3 << 4);
00273     // Then shift in our new scale bits:
00274     temp |= gScl << 4;
00275     // And write the new register value back into CTRL_REG4_G:
00276     gWriteByte(CTRL_REG4_G, temp);
00277     
00278     // We've updated the sensor, but we also need to update our class variables
00279     // First update gScale:
00280     gScale = gScl;
00281     // Then calculate a new gRes, which relies on gScale being set correctly:
00282     calcgRes();
00283 }
00284 
00285 void LSM9DS0::setAccelScale(accel_scale aScl)
00286 {
00287     // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
00288     uint8_t temp = xmReadByte(CTRL_REG2_XM);
00289     // Then mask out the accel scale bits:
00290     temp &= 0xFF^(0x3 << 3);
00291     // Then shift in our new scale bits:
00292     temp |= aScl << 3;
00293     // And write the new register value back into CTRL_REG2_XM:
00294     xmWriteByte(CTRL_REG2_XM, temp);
00295     
00296     // We've updated the sensor, but we also need to update our class variables
00297     // First update aScale:
00298     aScale = aScl;
00299     // Then calculate a new aRes, which relies on aScale being set correctly:
00300     calcaRes();
00301 }
00302 
00303 void LSM9DS0::setMagScale(mag_scale mScl)
00304 {
00305     // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
00306     uint8_t temp = xmReadByte(CTRL_REG6_XM);
00307     // Then mask out the mag scale bits:
00308     temp &= 0xFF^(0x3 << 5);
00309     // Then shift in our new scale bits:
00310     temp |= mScl << 5;
00311     // And write the new register value back into CTRL_REG6_XM:
00312     xmWriteByte(CTRL_REG6_XM, temp);
00313     
00314     // We've updated the sensor, but we also need to update our class variables
00315     // First update mScale:
00316     mScale = mScl;
00317     // Then calculate a new mRes, which relies on mScale being set correctly:
00318     calcmRes();
00319 }
00320 
00321 void LSM9DS0::setGyroODR(gyro_odr gRate)
00322 {
00323     // We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
00324     uint8_t temp = gReadByte(CTRL_REG1_G);
00325     // Then mask out the gyro ODR bits:
00326     temp &= 0xFF^(0xF << 4);
00327     // Then shift in our new ODR bits:
00328     temp |= (gRate << 4);
00329     // And write the new register value back into CTRL_REG1_G:
00330     gWriteByte(CTRL_REG1_G, temp);
00331 }
00332 void LSM9DS0::setAccelODR(accel_odr aRate)
00333 {
00334     // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
00335     uint8_t temp = xmReadByte(CTRL_REG1_XM);
00336     // Then mask out the accel ODR bits:
00337     temp &= 0xFF^(0xF << 4);
00338     // Then shift in our new ODR bits:
00339     temp |= (aRate << 4);
00340     // And write the new register value back into CTRL_REG1_XM:
00341     xmWriteByte(CTRL_REG1_XM, temp);
00342 }
00343 void LSM9DS0::setMagODR(mag_odr mRate)
00344 {
00345     // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
00346     uint8_t temp = xmReadByte(CTRL_REG5_XM);
00347     // Then mask out the mag ODR bits:
00348     temp &= 0xFF^(0x7 << 2);
00349     // Then shift in our new ODR bits:
00350     temp |= (mRate << 2);
00351     // And write the new register value back into CTRL_REG5_XM:
00352     xmWriteByte(CTRL_REG5_XM, temp);
00353 }
00354 
00355 void LSM9DS0::configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration)
00356 {
00357     gWriteByte(INT1_CFG_G, int1Cfg);
00358     gWriteByte(INT1_THS_XH_G, (int1ThsX & 0xFF00) >> 8);
00359     gWriteByte(INT1_THS_XL_G, (int1ThsX & 0xFF));
00360     gWriteByte(INT1_THS_YH_G, (int1ThsY & 0xFF00) >> 8);
00361     gWriteByte(INT1_THS_YL_G, (int1ThsY & 0xFF));
00362     gWriteByte(INT1_THS_ZH_G, (int1ThsZ & 0xFF00) >> 8);
00363     gWriteByte(INT1_THS_ZL_G, (int1ThsZ & 0xFF));
00364     if (duration)
00365         gWriteByte(INT1_DURATION_G, 0x80 | duration);
00366     else
00367         gWriteByte(INT1_DURATION_G, 0x00);
00368 }
00369 
00370 void LSM9DS0::calcgRes()
00371 {
00372     // Possible gyro scales (and their register bit settings) are:
00373     // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm
00374     // to calculate DPS/(ADC tick) based on that 2-bit value:
00375     switch (gScale)
00376     {
00377     case G_SCALE_245DPS:
00378         gRes = 245.0 / 32768.0;
00379         break;
00380     case G_SCALE_500DPS:
00381         gRes = 500.0 / 32768.0;
00382         break;
00383     case G_SCALE_2000DPS:
00384         gRes = 2000.0 / 32768.0;
00385         break;
00386     }
00387 }
00388 
00389 void LSM9DS0::calcaRes()
00390 {
00391     // Possible accelerometer scales (and their register bit settings) are:
00392     // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an 
00393     // algorithm to calculate g/(ADC tick) based on that 3-bit value:
00394     aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : 
00395            (((float) aScale + 1.0) * 2.0) / 32768.0;
00396 }
00397 
00398 void LSM9DS0::calcmRes()
00399 {
00400     // Possible magnetometer scales (and their register bit settings) are:
00401     // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm
00402     // to calculate Gs/(ADC tick) based on that 2-bit value:
00403     mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 : 
00404            (float) (mScale << 2) / 32768.0;
00405 }
00406     
00407 void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data)
00408 {
00409     // Whether we're using I2C or SPI, write a byte using the
00410     // gyro-specific I2C address or SPI CS pin.
00411     I2CwriteByte(gAddress, subAddress, data);
00412 }
00413 
00414 void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data)
00415 {
00416     // Whether we're using I2C or SPI, write a byte using the
00417     // accelerometer-specific I2C address or SPI CS pin.
00418         return I2CwriteByte(xmAddress, subAddress, data);
00419 }
00420 
00421 uint8_t LSM9DS0::gReadByte(uint8_t subAddress)
00422 {
00423         return I2CreadByte(gAddress, subAddress);
00424 }
00425 
00426 void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
00427 {
00428     // Whether we're using I2C or SPI, read multiple bytes using the
00429     // gyro-specific I2C address.
00430         I2CreadBytes(gAddress, subAddress, dest, count);
00431 }
00432 
00433 uint8_t LSM9DS0::xmReadByte(uint8_t subAddress)
00434 {
00435     // Whether we're using I2C or SPI, read a byte using the
00436     // accelerometer-specific I2C address.
00437         return I2CreadByte(xmAddress, subAddress);
00438 }
00439 
00440 void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
00441 {
00442     // read multiple bytes using the
00443     // accelerometer-specific I2C address.
00444     I2CreadBytes(xmAddress, subAddress, dest, count);
00445 }
00446 
00447 
00448 void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
00449 {   
00450     i2c_->writeByte(address,subAddress,data);
00451 }
00452 
00453 uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress)
00454 {
00455     char data[1]; // `data` will store the register data
00456     
00457     I2CreadBytes(address, subAddress,(uint8_t*)data, 1);
00458     return (uint8_t)data[0];
00459 
00460 }
00461 
00462 void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest,
00463                             uint8_t count)
00464 {   
00465     i2c_->readBytes(address, subAddress, count, dest);
00466 }