LSM9DS1.cpp

00001 #include "LSM9DS1.h"
00002 
00003 LSM9DS1::LSM9DS1(I2C& p_i2c, uint8_t xgAddr, uint8_t mAddr) : i2c(p_i2c)
00004 {
00005     // xgAddress and mAddress will store the 7-bit I2C address, if using I2C.
00006     xgAddress = xgAddr;
00007     mAddress = mAddr;
00008 }
00009 
00010 uint16_t 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[1] = 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     // Gyro initialization stuff:
00049     initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
00050     setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
00051     setGyroScale(gScale); // Set the gyro range
00052     
00053     // Accelerometer initialization stuff:
00054     initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
00055     setAccelODR(aODR); // Set the accel data rate.
00056     setAccelScale(aScale); // Set the accel range.
00057     
00058     // Magnetometer initialization stuff:
00059     initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
00060     setMagODR(mODR); // Set the magnetometer output data rate.
00061     setMagScale(mScale); // Set the magnetometer's range.
00062     
00063     // Once everything is initialized, return the WHO_AM_I registers we read:
00064     return (xgTest << 8) | mTest;
00065 }
00066 
00067 void LSM9DS1::initGyro()
00068 {
00069     char cmd[4] = {
00070         CTRL_REG1_G,
00071         (char)(gScale | G_ODR_119_BW_14),
00072         0,          // Default data out and int out
00073         0           // Default power mode and high pass settings
00074     };
00075 
00076     // Write the data to the gyro control registers
00077     i2c.write(xgAddress, cmd, 4);
00078 }
00079 
00080 void LSM9DS1::initAccel()
00081 {
00082     char cmd[4] = {
00083         CTRL_REG5_XL,
00084         0x38,       // Enable all axis and don't decimate data in out Registers
00085         (char)((A_ODR_119 << 5) | (aScale << 3) | (A_BW_AUTO_SCALE)),   // 119 Hz ODR, set scale, and auto BW
00086         0           // Default resolution mode and filtering settings
00087     };
00088 
00089     // Write the data to the accel control registers
00090     i2c.write(xgAddress, cmd, 4);
00091 }
00092 
00093 void LSM9DS1::initMag()
00094 {   
00095     char cmd[4] = {
00096         CTRL_REG1_M,
00097         0x10,       // Default data rate, xy axes mode, and temp comp
00098         (char)(mScale << 5), // Set mag scale
00099         0           // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
00100     };
00101 
00102     // Write the data to the mag control registers
00103     i2c.write(mAddress, cmd, 4);
00104 }
00105 
00106 void LSM9DS1::readAccel()
00107 {
00108     // The data we are going to read from the accel
00109     char data[6];
00110 
00111     // The start of the addresses we want to read from
00112     char subAddress = OUT_X_L_XL;
00113 
00114     // Write the address we are going to read from and don't end the transaction
00115     i2c.write(xgAddress, &subAddress, 1, true);
00116     // Read in all 8 bit registers containing the axes data
00117     i2c.read(xgAddress, data, 6);
00118 
00119     // Reassemble the data and convert to g
00120     ax_raw = data[0] | (data[1] << 8);
00121     ay_raw = data[2] | (data[3] << 8);
00122     az_raw = data[4] | (data[5] << 8);
00123     ax = ax_raw * aRes;
00124     ay = ay_raw * aRes;
00125     az = az_raw * aRes;
00126 }
00127 
00128 void LSM9DS1::readMag()
00129 {
00130     // The data we are going to read from the mag
00131     char data[6];
00132 
00133     // The start of the addresses we want to read from
00134     char subAddress = OUT_X_L_M;
00135 
00136     // Write the address we are going to read from and don't end the transaction
00137     i2c.write(mAddress, &subAddress, 1, true);
00138     // Read in all 8 bit registers containing the axes data
00139     i2c.read(mAddress, data, 6);
00140 
00141     // Reassemble the data and convert to degrees
00142     mx_raw = data[0] | (data[1] << 8);
00143     my_raw = data[2] | (data[3] << 8);
00144     mz_raw = data[4] | (data[5] << 8);
00145     mx = mx_raw * mRes;
00146     my = my_raw * mRes;
00147     mz = mz_raw * mRes;
00148 }
00149 
00150 void LSM9DS1::readTemp()
00151 {
00152     // The data we are going to read from the temp
00153     char data[2];
00154 
00155     // The start of the addresses we want to read from
00156     char subAddress = OUT_TEMP_L;
00157 
00158     // Write the address we are going to read from and don't end the transaction
00159     i2c.write(xgAddress, &subAddress, 1, true);
00160     // Read in all 8 bit registers containing the axes data
00161     i2c.read(xgAddress, data, 2);
00162 
00163     // Temperature is a 12-bit signed integer   
00164     temperature_raw = data[0] | (data[1] << 8);
00165 
00166     temperature_c = (float)temperature_raw / 8.0 + 25;
00167     temperature_f = temperature_c * 1.8 + 32;
00168 }
00169 
00170 
00171 void LSM9DS1::readGyro()
00172 {
00173     // The data we are going to read from the gyro
00174     char data[6];
00175 
00176     // The start of the addresses we want to read from
00177     char subAddress = OUT_X_L_G;
00178 
00179     // Write the address we are going to read from and don't end the transaction
00180     i2c.write(xgAddress, &subAddress, 1, true);
00181     // Read in all 8 bit registers containing the axes data
00182     i2c.read(xgAddress, data, 6);
00183 
00184     // Reassemble the data and convert to degrees/sec
00185     gx_raw = data[0] | (data[1] << 8);
00186     gy_raw = data[2] | (data[3] << 8);
00187     gz_raw = data[4] | (data[5] << 8);
00188     gx = gx_raw * gRes;
00189     gy = gy_raw * gRes;
00190     gz = gz_raw * gRes;
00191 }
00192 
00193 void LSM9DS1::setGyroScale(gyro_scale gScl)
00194 {
00195     // The start of the addresses we want to read from
00196     char cmd[2] = {
00197         CTRL_REG1_G,
00198         0
00199     };
00200 
00201     // Write the address we are going to read from and don't end the transaction
00202     i2c.write(xgAddress, cmd, 1, true);
00203     // Read in all the 8 bits of data
00204     i2c.read(xgAddress, cmd+1, 1);
00205 
00206     // Then mask out the gyro scale bits:
00207     cmd[1] &= 0xFF^(0x3 << 3);
00208     // Then shift in our new scale bits:
00209     cmd[1] |= gScl << 3;
00210 
00211     // Write the gyroscale out to the gyro
00212     i2c.write(xgAddress, cmd, 2);
00213     
00214     // We've updated the sensor, but we also need to update our class variables
00215     // First update gScale:
00216     gScale = gScl;
00217     // Then calculate a new gRes, which relies on gScale being set correctly:
00218     calcgRes();
00219 }
00220 
00221 void LSM9DS1::setAccelScale(accel_scale aScl)
00222 {
00223     // The start of the addresses we want to read from
00224     char cmd[2] = {
00225         CTRL_REG6_XL,
00226         0
00227     };
00228 
00229     // Write the address we are going to read from and don't end the transaction
00230     i2c.write(xgAddress, cmd, 1, true);
00231     // Read in all the 8 bits of data
00232     i2c.read(xgAddress, cmd+1, 1);
00233 
00234     // Then mask out the accel scale bits:
00235     cmd[1] &= 0xFF^(0x3 << 3);
00236     // Then shift in our new scale bits:
00237     cmd[1] |= aScl << 3;
00238 
00239     // Write the accelscale out to the accel
00240     i2c.write(xgAddress, cmd, 2);
00241     
00242     // We've updated the sensor, but we also need to update our class variables
00243     // First update aScale:
00244     aScale = aScl;
00245     // Then calculate a new aRes, which relies on aScale being set correctly:
00246     calcaRes();
00247 }
00248 
00249 void LSM9DS1::setMagScale(mag_scale mScl)
00250 {
00251     // The start of the addresses we want to read from
00252     char cmd[2] = {
00253         CTRL_REG2_M,
00254         0
00255     };
00256 
00257     // Write the address we are going to read from and don't end the transaction
00258     i2c.write(mAddress, cmd, 1, true);
00259     // Read in all the 8 bits of data
00260     i2c.read(mAddress, cmd+1, 1);
00261 
00262     // Then mask out the mag scale bits:
00263     cmd[1] &= 0xFF^(0x3 << 5);
00264     // Then shift in our new scale bits:
00265     cmd[1] |= mScl << 5;
00266 
00267     // Write the magscale out to the mag
00268     i2c.write(mAddress, cmd, 2);
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 LSM9DS1::setGyroODR(gyro_odr gRate)
00278 {
00279     // The start of the addresses we want to read from
00280     char cmd[2] = {
00281         CTRL_REG1_G,
00282         0
00283     };
00284 
00285     // Write the address we are going to read from and don't end the transaction
00286     i2c.write(xgAddress, cmd, 1, true);
00287     // Read in all the 8 bits of data
00288     i2c.read(xgAddress, cmd+1, 1);
00289 
00290     // Then mask out the gyro odr bits:
00291     cmd[1] &= (0x3 << 3);
00292     // Then shift in our new odr bits:
00293     cmd[1] |= gRate;
00294 
00295     // Write the gyroodr out to the gyro
00296     i2c.write(xgAddress, cmd, 2);
00297 }
00298 
00299 void LSM9DS1::setAccelODR(accel_odr aRate)
00300 {
00301     // The start of the addresses we want to read from
00302     char cmd[2] = {
00303         CTRL_REG6_XL,
00304         0
00305     };
00306 
00307     // Write the address we are going to read from and don't end the transaction
00308     i2c.write(xgAddress, cmd, 1, true);
00309     // Read in all the 8 bits of data
00310     i2c.read(xgAddress, cmd+1, 1);
00311 
00312     // Then mask out the accel odr bits:
00313     cmd[1] &= 0xFF^(0x7 << 5);
00314     // Then shift in our new odr bits:
00315     cmd[1] |= aRate << 5;
00316 
00317     // Write the accelodr out to the accel
00318     i2c.write(xgAddress, cmd, 2);
00319 }
00320 
00321 void LSM9DS1::setMagODR(mag_odr mRate)
00322 {
00323     // The start of the addresses we want to read from
00324     char cmd[2] = {
00325         CTRL_REG1_M,
00326         0
00327     };
00328 
00329     // Write the address we are going to read from and don't end the transaction
00330     i2c.write(mAddress, cmd, 1, true);
00331     // Read in all the 8 bits of data
00332     i2c.read(mAddress, cmd+1, 1);
00333 
00334     // Then mask out the mag odr bits:
00335     cmd[1] &= 0xFF^(0x7 << 2);
00336     // Then shift in our new odr bits:
00337     cmd[1] |= mRate << 2;
00338 
00339     // Write the magodr out to the mag
00340     i2c.write(mAddress, cmd, 2);
00341 }
00342 
00343 void LSM9DS1::calcgRes()
00344 {
00345     // Possible gyro scales (and their register bit settings) are:
00346     // 245 DPS (00), 500 DPS (01), 2000 DPS (10).
00347     switch (gScale)
00348     {
00349         case G_SCALE_245DPS:
00350             gRes = 245.0 / 32768.0;
00351             break;
00352         case G_SCALE_500DPS:
00353             gRes = 500.0 / 32768.0;
00354             break;
00355         case G_SCALE_2000DPS:
00356             gRes = 2000.0 / 32768.0;
00357             break;
00358     }
00359 }
00360 
00361 void LSM9DS1::calcaRes()
00362 {
00363     // Possible accelerometer scales (and their register bit settings) are:
00364     // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
00365     switch (aScale)
00366     {
00367         case A_SCALE_2G:
00368             aRes = 2.0 / 32768.0;
00369             break;
00370         case A_SCALE_4G:
00371             aRes = 4.0 / 32768.0;
00372             break;
00373         case A_SCALE_8G:
00374             aRes = 8.0 / 32768.0;
00375             break;
00376         case A_SCALE_16G:
00377             aRes = 16.0 / 32768.0;
00378             break;
00379     }
00380 }
00381 
00382 void LSM9DS1::calcmRes()
00383 {
00384     // Possible magnetometer scales (and their register bit settings) are:
00385     // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). 
00386     switch (mScale)
00387     {
00388         case M_SCALE_4GS:
00389             mRes = 4.0 / 32768.0;
00390             break;
00391         case M_SCALE_8GS:
00392             mRes = 8.0 / 32768.0;
00393             break;
00394         case M_SCALE_12GS:
00395             mRes = 12.0 / 32768.0;
00396             break;
00397         case M_SCALE_16GS:
00398             mRes = 16.0 / 32768.0;
00399             break;
00400     }
00401 }