LSM6DS3.cpp

00001 #include "LSM6DS3.h"
00002 
00003 LSM6DS3::LSM6DS3(PinName sda, PinName scl, uint8_t xgAddr) : i2c(sda, scl)
00004 {
00005     // xgAddress will store the 7-bit I2C address, if using I2C.
00006     i2c.frequency(400000);
00007     xgAddress = xgAddr;
00008 }
00009 
00010 uint16_t LSM6DS3::begin(gyro_scale gScl, accel_scale aScl,  
00011                         gyro_odr gODR, accel_odr aODR)
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     
00018     // Once we have the scale values, we can calculate the resolution
00019     // of each sensor. That's what these functions are for. One for each sensor
00020     calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
00021     calcaRes(); // Calculate g / ADC tick, stored in aRes variable
00022     
00023     
00024     // To verify communication, we can read from the WHO_AM_I register of
00025     // each device. Store those in a variable so we can return them.
00026     // The start of the addresses we want to read from
00027     char cmd[2] = {
00028         WHO_AM_I_REG,
00029         0
00030     };
00031 
00032     // Write the address we are going to read from and don't end the transaction
00033     i2c.write(xgAddress, cmd, 1, true);
00034     // Read in all the 8 bits of data
00035     i2c.read(xgAddress, cmd+1, 1);
00036     uint8_t xgTest = cmd[1];                    // Read the accel/gyro WHO_AM_I
00037         
00038     // Gyro initialization stuff:
00039     initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
00040     setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
00041     setGyroScale(gScale); // Set the gyro range
00042     
00043     // Accelerometer initialization stuff:
00044     initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
00045     setAccelODR(aODR); // Set the accel data rate.
00046     setAccelScale(aScale); // Set the accel range.
00047     
00048     // Interrupt initialization stuff;
00049     initIntr();
00050     
00051     // Once everything is initialized, return the WHO_AM_I registers we read:
00052     return xgTest;
00053 }
00054 
00055 void LSM6DS3::initGyro()
00056 {
00057     char cmd[4] = {
00058         CTRL2_G,
00059         gScale | G_ODR_104,
00060         0,          // Default data out and int out
00061         0           // Default power mode and high pass settings
00062     };
00063 
00064     // Write the data to the gyro control registers
00065     i2c.write(xgAddress, cmd, 4);
00066 }
00067 
00068 void LSM6DS3::initAccel()
00069 {
00070     char cmd[4] = {
00071         CTRL1_XL,
00072         0x38,       // Enable all axis and don't decimate data in out Registers
00073         (A_ODR_104 << 5) | (aScale << 3) | (A_BW_AUTO_SCALE),   // 119 Hz ODR, set scale, and auto BW
00074         0           // Default resolution mode and filtering settings
00075     };
00076 
00077     // Write the data to the accel control registers
00078     i2c.write(xgAddress, cmd, 4);
00079 }
00080 
00081 void LSM6DS3::initIntr()
00082 {
00083     char cmd[2];
00084     
00085     cmd[0] = TAP_CFG;
00086     cmd[1] = 0x0E;
00087     i2c.write(xgAddress, cmd, 2);
00088     cmd[0] = TAP_THS_6D;
00089     cmd[1] = 0x03;
00090     i2c.write(xgAddress, cmd, 2);
00091     cmd[0] = INT_DUR2;
00092     cmd[1] = 0x7F;
00093     i2c.write(xgAddress, cmd, 2);
00094     cmd[0] = WAKE_UP_THS;
00095     cmd[1] = 0x80;
00096     i2c.write(xgAddress, cmd, 2);
00097     cmd[0] = MD1_CFG;
00098     cmd[1] = 0x48;
00099     i2c.write(xgAddress, cmd, 2);
00100 }
00101 
00102 void LSM6DS3::readAccel()
00103 {
00104     // The data we are going to read from the accel
00105     char data[6];
00106 
00107     // Set addresses
00108     char subAddressXL = OUTX_L_XL;
00109     char subAddressXH = OUTX_H_XL;
00110     char subAddressYL = OUTY_L_XL;
00111     char subAddressYH = OUTY_H_XL;
00112     char subAddressZL = OUTZ_L_XL;
00113     char subAddressZH = OUTZ_H_XL;
00114 
00115     // Write the address we are going to read from and don't end the transaction
00116     i2c.write(xgAddress, &subAddressXL, 1, true);
00117     // Read in register containing the axes data and alocated to the correct index
00118     i2c.read(xgAddress, data, 1);
00119     
00120     i2c.write(xgAddress, &subAddressXH, 1, true);
00121     i2c.read(xgAddress, (data + 1), 1);
00122     i2c.write(xgAddress, &subAddressYL, 1, true);
00123     i2c.read(xgAddress, (data + 2), 1);
00124     i2c.write(xgAddress, &subAddressYH, 1, true);
00125     i2c.read(xgAddress, (data + 3), 1);
00126     i2c.write(xgAddress, &subAddressZL, 1, true);
00127     i2c.read(xgAddress, (data + 4), 1);
00128     i2c.write(xgAddress, &subAddressZH, 1, true);
00129     i2c.read(xgAddress, (data + 5), 1);
00130 
00131     // Reassemble the data and convert to g
00132     ax_raw = data[0] | (data[1] << 8);
00133     ay_raw = data[2] | (data[3] << 8);
00134     az_raw = data[4] | (data[5] << 8);
00135     ax = ax_raw * aRes;
00136     ay = ay_raw * aRes;
00137     az = az_raw * aRes;
00138 }
00139 
00140 void LSM6DS3::readIntr()
00141 {
00142     char data[1];
00143     char subAddress = TAP_SRC;
00144 
00145     i2c.write(xgAddress, &subAddress, 1, true);
00146     i2c.read(xgAddress, data, 1);
00147 
00148     intr = (float)data[0];
00149 }
00150 
00151 void LSM6DS3::readTemp()
00152 {
00153     // The data we are going to read from the temp
00154     char data[2];
00155 
00156     // Set addresses
00157     char subAddressL = OUT_TEMP_L;
00158     char subAddressH = OUT_TEMP_H;
00159 
00160     // Write the address we are going to read from and don't end the transaction
00161     i2c.write(xgAddress, &subAddressL, 1, true);
00162     // Read in register containing the temperature data and alocated to the correct index
00163     i2c.read(xgAddress, data, 1);
00164     
00165     i2c.write(xgAddress, &subAddressH, 1, true);
00166     i2c.read(xgAddress, (data + 1), 1);
00167 
00168     // Temperature is a 12-bit signed integer   
00169     temperature_raw = data[0] | (data[1] << 8);
00170 
00171     temperature_c = (float)temperature_raw / 16.0 + 25.0;
00172     temperature_f = temperature_c * 1.8 + 32.0;
00173 }
00174 
00175 
00176 void LSM6DS3::readGyro()
00177 {
00178     // The data we are going to read from the gyro
00179     char data[6];
00180 
00181     // Set addresses
00182     char subAddressXL = OUTX_L_G;
00183     char subAddressXH = OUTX_H_G;
00184     char subAddressYL = OUTY_L_G;
00185     char subAddressYH = OUTY_H_G;
00186     char subAddressZL = OUTZ_L_G;
00187     char subAddressZH = OUTZ_H_G;
00188 
00189     // Write the address we are going to read from and don't end the transaction
00190     i2c.write(xgAddress, &subAddressXL, 1, true);
00191     // Read in register containing the axes data and alocated to the correct index
00192     i2c.read(xgAddress, data, 1);
00193     
00194     i2c.write(xgAddress, &subAddressXH, 1, true);
00195     i2c.read(xgAddress, (data + 1), 1);
00196     i2c.write(xgAddress, &subAddressYL, 1, true);
00197     i2c.read(xgAddress, (data + 2), 1);
00198     i2c.write(xgAddress, &subAddressYH, 1, true);
00199     i2c.read(xgAddress, (data + 3), 1);
00200     i2c.write(xgAddress, &subAddressZL, 1, true);
00201     i2c.read(xgAddress, (data + 4), 1);
00202     i2c.write(xgAddress, &subAddressZH, 1, true);
00203     i2c.read(xgAddress, (data + 5), 1);
00204 
00205     // Reassemble the data and convert to degrees/sec
00206     gx_raw = data[0] | (data[1] << 8);
00207     gy_raw = data[2] | (data[3] << 8);
00208     gz_raw = data[4] | (data[5] << 8);
00209     gx = gx_raw * gRes;
00210     gy = gy_raw * gRes;
00211     gz = gz_raw * gRes;
00212 }
00213 
00214 void LSM6DS3::setGyroScale(gyro_scale gScl)
00215 {
00216     // The start of the addresses we want to read from
00217     char cmd[2] = {
00218         CTRL2_G,
00219         0
00220     };
00221 
00222     // Write the address we are going to read from and don't end the transaction
00223     i2c.write(xgAddress, cmd, 1, true);
00224     // Read in all the 8 bits of data
00225     i2c.read(xgAddress, cmd+1, 1);
00226 
00227     // Then mask out the gyro scale bits:
00228     cmd[1] &= 0xFF^(0x3 << 3);
00229     // Then shift in our new scale bits:
00230     cmd[1] |= gScl << 3;
00231 
00232     // Write the gyroscale out to the gyro
00233     i2c.write(xgAddress, cmd, 2);
00234     
00235     // We've updated the sensor, but we also need to update our class variables
00236     // First update gScale:
00237     gScale = gScl;
00238     // Then calculate a new gRes, which relies on gScale being set correctly:
00239     calcgRes();
00240 }
00241 
00242 void LSM6DS3::setAccelScale(accel_scale aScl)
00243 {
00244     // The start of the addresses we want to read from
00245     char cmd[2] = {
00246         CTRL1_XL,
00247         0
00248     };
00249 
00250     // Write the address we are going to read from and don't end the transaction
00251     i2c.write(xgAddress, cmd, 1, true);
00252     // Read in all the 8 bits of data
00253     i2c.read(xgAddress, cmd+1, 1);
00254 
00255     // Then mask out the accel scale bits:
00256     cmd[1] &= 0xFF^(0x3 << 3);
00257     // Then shift in our new scale bits:
00258     cmd[1] |= aScl << 3;
00259 
00260     // Write the accelscale out to the accel
00261     i2c.write(xgAddress, cmd, 2);
00262     
00263     // We've updated the sensor, but we also need to update our class variables
00264     // First update aScale:
00265     aScale = aScl;
00266     // Then calculate a new aRes, which relies on aScale being set correctly:
00267     calcaRes();
00268 }
00269 
00270 void LSM6DS3::setGyroODR(gyro_odr gRate)
00271 {
00272     // The start of the addresses we want to read from
00273     char cmd[2] = {
00274         CTRL2_G,
00275         0
00276     };
00277     
00278     // Set low power based on ODR, else keep sensor on high performance
00279     if(gRate == G_ODR_13_BW_0 | gRate == G_ODR_26_BW_2 | gRate == G_ODR_52_BW_16) {
00280         char cmdLow[2] ={
00281             CTRL7_G,
00282             1
00283         };
00284         
00285         i2c.write(xgAddress, cmdLow, 2);
00286     }
00287     else {
00288         char cmdLow[2] ={
00289             CTRL7_G,
00290             0
00291         };
00292         
00293         i2c.write(xgAddress, cmdLow, 2);
00294     }
00295 
00296     // Write the address we are going to read from and don't end the transaction
00297     i2c.write(xgAddress, cmd, 1, true);
00298     // Read in all the 8 bits of data
00299     i2c.read(xgAddress, cmd+1, 1);
00300 
00301     // Then mask out the gyro odr bits:
00302     cmd[1] &= (0x3 << 3);
00303     // Then shift in our new odr bits:
00304     cmd[1] |= gRate;
00305 
00306     // Write the gyroodr out to the gyro
00307     i2c.write(xgAddress, cmd, 2);
00308 }
00309 
00310 void LSM6DS3::setAccelODR(accel_odr aRate)
00311 {
00312     // The start of the addresses we want to read from
00313     char cmd[2] = {
00314         CTRL1_XL,
00315         0
00316     };
00317     
00318     // Set low power based on ODR, else keep sensor on high performance
00319     if(aRate == A_ODR_13 | aRate == A_ODR_26 | aRate == A_ODR_52) {
00320         char cmdLow[2] ={
00321             CTRL6_C,
00322             1
00323         };
00324         
00325         i2c.write(xgAddress, cmdLow, 2);
00326     }
00327     else {
00328         char cmdLow[2] ={
00329             CTRL6_C,
00330             0
00331         };
00332         
00333         i2c.write(xgAddress, cmdLow, 2);
00334     }
00335 
00336     // Write the address we are going to read from and don't end the transaction
00337     i2c.write(xgAddress, cmd, 1, true);
00338     // Read in all the 8 bits of data
00339     i2c.read(xgAddress, cmd+1, 1);
00340 
00341     // Then mask out the accel odr bits:
00342     cmd[1] &= 0xFF^(0x7 << 5);
00343     // Then shift in our new odr bits:
00344     cmd[1] |= aRate << 5;
00345 
00346     // Write the accelodr out to the accel
00347     i2c.write(xgAddress, cmd, 2);
00348 }
00349 
00350 void LSM6DS3::calcgRes()
00351 {
00352     // Possible gyro scales (and their register bit settings) are:
00353     // 245 DPS (00), 500 DPS (01), 2000 DPS (10).
00354     switch (gScale)
00355     {
00356         case G_SCALE_245DPS:
00357             gRes = 245.0 / 32768.0;
00358             break;
00359         case G_SCALE_500DPS:
00360             gRes = 500.0 / 32768.0;
00361             break;
00362         case G_SCALE_2000DPS:
00363             gRes = 2000.0 / 32768.0;
00364             break;
00365     }
00366 }
00367 
00368 void LSM6DS3::calcaRes()
00369 {
00370     // Possible accelerometer scales (and their register bit settings) are:
00371     // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
00372     switch (aScale)
00373     {
00374         case A_SCALE_2G:
00375             aRes = 2.0 / 32768.0;
00376             break;
00377         case A_SCALE_4G:
00378             aRes = 4.0 / 32768.0;
00379             break;
00380         case A_SCALE_8G:
00381             aRes = 8.0 / 32768.0;
00382             break;
00383         case A_SCALE_16G:
00384             aRes = 16.0 / 32768.0;
00385             break;
00386     }
00387 }