LSM9DS0_mbed.cpp

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