LSM9DS0.cpp

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