LSM6DS33_GR1.cpp

00001 #include "LSM6DS33_GR1.h"
00002 
00003 LSM6DS33::LSM6DS33(PinName sda, PinName scl, uint8_t xgAddr) : i2c(sda, scl)
00004 {
00005     // xgAddress will store the 7-bit I2C address, if using I2C.
00006     xgAddress = xgAddr;
00007 
00008 // init gyro offset
00009     gx_off=0;
00010     gy_off=0;
00011     gz_off=0;
00012     gxol=0;
00013     gxoh=0;
00014     gyol=0;
00015     gyoh=0;
00016     gzol=0;
00017     gzoh=0;
00018 }
00019 
00020 uint16_t LSM6DS33::begin(gyro_scale gScl, accel_scale aScl,
00021                          gyro_odr gODR, accel_odr aODR)
00022 {
00023 
00024     // Store the given scales in class variables. These scale variables
00025     // are used throughout to calculate the actual g's, DPS,and Gs's.
00026     gScale = gScl;
00027     aScale = aScl;
00028 
00029     // Once we have the scale values, we can calculate the resolution
00030     // of each sensor. That's what these functions are for. One for each sensor
00031     calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
00032     calcaRes(); // Calculate g / ADC tick, stored in aRes variable
00033 
00034 
00035     // To verify communication, we can read from the WHO_AM_I register of
00036     // each device. Store those in a variable so we can return them.
00037     // The start of the addresses we want to read from
00038     char cmd[2] = {
00039         WHO_AM_I_REG,
00040         0
00041     };
00042 
00043     // Write the address we are going to read from and don't end the transaction
00044     i2c.write(xgAddress, cmd, 1, true);
00045     // Read in all the 8 bits of data
00046     i2c.read(xgAddress, cmd+1, 1);
00047     uint8_t xgTest = cmd[1];                    // Read the accel/gyro WHO_AM_I
00048 
00049     // Gyro initialization stuff:
00050     initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
00051     setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
00052     setGyroScale(gScale); // Set the gyro range
00053 
00054     // Accelerometer initialization stuff:
00055     initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
00056     setAccelODR(aODR); // Set the accel data rate.
00057     setAccelScale(aScale); // Set the accel range.
00058 
00059     //set high res timestamp where LSB is 25us
00060     cmd[0] = WAKE_UP_DUR;
00061     cmd[1] = 0x10;
00062     i2c.write(xgAddress, cmd, 2);
00063 
00064 
00065     // Once everything is initialized, return the WHO_AM_I registers we read:
00066     return xgTest;
00067 }
00068 
00069 void LSM6DS33::initGyro()
00070 {
00071     char cmd[2] = {
00072         CTRL2_G,
00073         gScale | G_ODR_104
00074     };
00075 
00076     // Write the data to the gyro control registers
00077     i2c.write(xgAddress, cmd, 2);
00078 }
00079 
00080 void LSM6DS33::initAccel()
00081 {
00082     char cmd[4] = {
00083         CTRL1_XL,
00084         0x30
00085     };
00086 
00087     // Write the data to the accel control registers
00088     i2c.write(xgAddress, cmd, 2);
00089 }
00090 
00091 void LSM6DS33::initIntr()
00092 {
00093 
00094 }
00095 //modif gr1
00096 void LSM6DS33::readAllraw()
00097 {
00098     // The data we are going to read from the temp/gyr/acc/timestamp
00099     //char data[14];//from 0x20 to 0x42
00100 
00101     char data[14];
00102     char tsdata[3];
00103 
00104     i2c.start();
00105     i2c.write(xgAddress);
00106     i2c.write(OUT_TEMP_L);
00107     i2c.start();
00108     i2c.write(xgAddress | 0x1);
00109     for(int i =0; i<13; i++) {
00110         data[i]=i2c.read(1);
00111     }
00112     data[13]=i2c.read(0);
00113     i2c.stop();
00114 
00115     // Temperature is a 12-bit signed integer
00116     temperature_raw = data[0] | (data[1] << 8);
00117     gxl = data[2] ;
00118     gxh =data[3] ;
00119     gyl = data[4] ;
00120     gyh= data[5] ;
00121     gzl = data[6] ;
00122     gzh=data[7] ;
00123     axl= data[8] ;
00124     axh=data[9] ;
00125     ayl = data[10];
00126     ayh=data[11] ;
00127     azl = data[12] ;
00128     azh=data[13] ;
00129 
00130     //i2c.start();
00131     // i2c.write(xgAddress);
00132     // i2c.write(TIMESTAMP0_REG);
00133     // i2c.start();
00134     // i2c.write(xgAddress | 0x1);
00135     // for(int i =0; i<3; i++) {
00136     //     tsdata[i]=i2c.read(1);
00137     // }
00138     // tsdata[3]=i2c.read(0);
00139     //i2c.stop();
00140 
00141     // time_raw = tsdata[0] | (tsdata[1] << 8) | (tsdata[2] << 16);
00142 
00143 
00144     temperature_c = (float)temperature_raw / 16.0 + 25.0;
00145     // gx = gx_raw * gRes;
00146     //  gy = gy_raw * gRes;
00147     // gz = gz_raw * gRes;
00148     //  ax = ax_raw * aRes;
00149     // ay = ay_raw * aRes;
00150     // az = az_raw * aRes;
00151     //  time = time_raw*(0.000025);
00152 
00153 
00154 }
00155 //fin modif gr1
00156 void LSM6DS33::readAll()
00157 {
00158     // The data we are going to read from the temp/gyr/acc/timestamp
00159     //char data[14];//from 0x20 to 0x42
00160 
00161     char data[14];
00162     char tsdata[3];
00163 
00164     i2c.start();
00165     i2c.write(xgAddress);
00166     i2c.write(OUT_TEMP_L);
00167     i2c.start();
00168     i2c.write(xgAddress | 0x1);
00169     for(int i =0; i<13; i++) {
00170         data[i]=i2c.read(1);
00171     }
00172     data[13]=i2c.read(0);
00173     i2c.stop();
00174 
00175     // Temperature is a 12-bit signed integer
00176     temperature_raw = data[0] | (data[1] << 8);
00177     gx_raw = data[2] | (data[3] << 8);
00178     gy_raw = data[4] | (data[5] << 8);
00179     gz_raw = data[6] | (data[7] << 8);
00180     ax_raw = data[8] | (data[9] << 8);
00181     ay_raw = data[10] | (data[11] << 8);
00182     az_raw = data[12] | (data[13] << 8);
00183 
00184     i2c.start();
00185     i2c.write(xgAddress);
00186     i2c.write(TIMESTAMP0_REG);
00187     i2c.start();
00188     i2c.write(xgAddress | 0x1);
00189     for(int i =0; i<3; i++) {
00190         tsdata[i]=i2c.read(1);
00191     }
00192     tsdata[3]=i2c.read(0);
00193     i2c.stop();
00194 
00195     time_raw = tsdata[0] | (tsdata[1] << 8) | (tsdata[2] << 16);
00196 
00197 
00198     temperature_c = (float)temperature_raw / 16.0 + 25.0;
00199     gx = gx_raw * gRes;
00200     gy = gy_raw * gRes;
00201     gz = gz_raw * gRes;
00202     ax = ax_raw * aRes;
00203     ay = ay_raw * aRes;
00204     az = az_raw * aRes;
00205     time = time_raw*(0.000025);
00206 
00207 
00208 }
00209 
00210 
00211 void LSM6DS33::readAccel()
00212 {
00213     // The data we are going to read from the accel
00214     char data[6];
00215 
00216     // Set addresses
00217     char subAddressXL = OUTX_L_XL;
00218     char subAddressXH = OUTX_H_XL;
00219     char subAddressYL = OUTY_L_XL;
00220     char subAddressYH = OUTY_H_XL;
00221     char subAddressZL = OUTZ_L_XL;
00222     char subAddressZH = OUTZ_H_XL;
00223 
00224     // Write the address we are going to read from and don't end the transaction
00225     i2c.write(xgAddress, &subAddressXL, 1, true);
00226     // Read in register containing the axes data and alocated to the correct index
00227     i2c.read(xgAddress, data, 1);
00228 
00229     i2c.write(xgAddress, &subAddressXH, 1, true);
00230     i2c.read(xgAddress, (data + 1), 1);
00231     i2c.write(xgAddress, &subAddressYL, 1, true);
00232     i2c.read(xgAddress, (data + 2), 1);
00233     i2c.write(xgAddress, &subAddressYH, 1, true);
00234     i2c.read(xgAddress, (data + 3), 1);
00235     i2c.write(xgAddress, &subAddressZL, 1, true);
00236     i2c.read(xgAddress, (data + 4), 1);
00237     i2c.write(xgAddress, &subAddressZH, 1, true);
00238     i2c.read(xgAddress, (data + 5), 1);
00239 
00240     // Reassemble the data and convert to g
00241     ax_raw = data[0] | (data[1] << 8);
00242     ay_raw = data[2] | (data[3] << 8);
00243     az_raw = data[4] | (data[5] << 8);
00244     ax = ax_raw * aRes;
00245     ay = ay_raw * aRes;
00246     az = az_raw * aRes;
00247     //gr
00248     axl= data[0] ;
00249     axh=data[1] ;
00250     ayl = data[2];
00251     ayh=data[3] ;
00252     azl = data[4] ;
00253     azh=data[5] ;
00254 }
00255 
00256 void LSM6DS33::readIntr()
00257 {
00258     char data[1];
00259     char subAddress = TAP_SRC;
00260 
00261     i2c.write(xgAddress, &subAddress, 1, true);
00262     i2c.read(xgAddress, data, 1);
00263 
00264     intr = (float)data[0];
00265 }
00266 
00267 void LSM6DS33::readTemp()
00268 {
00269     // The data we are going to read from the temp
00270     char data[2];
00271 
00272     // Set addresses
00273     char subAddressL = OUT_TEMP_L;
00274     char subAddressH = OUT_TEMP_H;
00275 
00276     // Write the address we are going to read from and don't end the transaction
00277     i2c.write(xgAddress, &subAddressL, 1, true);
00278     // Read in register containing the temperature data and alocated to the correct index
00279     i2c.read(xgAddress, data, 1);
00280 
00281     i2c.write(xgAddress, &subAddressH, 1, true);
00282     i2c.read(xgAddress, (data + 1), 1);
00283 
00284     // Temperature is a 12-bit signed integer
00285     temperature_raw = data[0] | (data[1] << 8);
00286 
00287     temperature_c = (float)temperature_raw / 16.0 + 25.0;
00288     temperature_f = temperature_c * 1.8 + 32.0;
00289 }
00290 
00291 
00292 void LSM6DS33::readGyro()
00293 {
00294     // The data we are going to read from the gyro
00295     char data[6];
00296 
00297     // Set addresses
00298     char subAddressXL = OUTX_L_G;
00299     char subAddressXH = OUTX_H_G;
00300     char subAddressYL = OUTY_L_G;
00301     char subAddressYH = OUTY_H_G;
00302     char subAddressZL = OUTZ_L_G;
00303     char subAddressZH = OUTZ_H_G;
00304 
00305     // Write the address we are going to read from and don't end the transaction
00306     i2c.write(xgAddress, &subAddressXL, 1, true);
00307     // Read in register containing the axes data and alocated to the correct index
00308     i2c.read(xgAddress, data, 1);
00309 
00310     i2c.write(xgAddress, &subAddressXH, 1, true);
00311     i2c.read(xgAddress, (data + 1), 1);
00312     i2c.write(xgAddress, &subAddressYL, 1, true);
00313     i2c.read(xgAddress, (data + 2), 1);
00314     i2c.write(xgAddress, &subAddressYH, 1, true);
00315     i2c.read(xgAddress, (data + 3), 1);
00316     i2c.write(xgAddress, &subAddressZL, 1, true);
00317     i2c.read(xgAddress, (data + 4), 1);
00318     i2c.write(xgAddress, &subAddressZH, 1, true);
00319     i2c.read(xgAddress, (data + 5), 1);
00320 
00321     // Reassemble the data and convert to degrees/sec
00322     gx_raw = data[0] | (data[1] << 8);
00323     gy_raw = data[2] | (data[3] << 8);
00324     gz_raw = data[4] | (data[5] << 8);
00325     gx = gx_raw * gRes;
00326     gy = gy_raw * gRes;
00327     gz = gz_raw * gRes;
00328     // gr
00329     gxl = data[0] ;
00330     gxh =data[1] ;
00331     gyl = data[2] ;
00332     gyh= data[3] ;
00333     gzl = data[4] ;
00334     gzh=data[5] ;
00335 }
00336 
00337 void LSM6DS33::setGyroScale(gyro_scale gScl)
00338 {
00339     // The start of the addresses we want to read from
00340     char cmd[2] = {
00341         CTRL2_G,
00342         0
00343     };
00344 
00345     // Write the address we are going to read from and don't end the transaction
00346     i2c.write(xgAddress, cmd, 1, true);
00347     // Read in all the 8 bits of data
00348     i2c.read(xgAddress, cmd+1, 1);
00349 
00350     // Then mask out the gyro scale bits:
00351     cmd[1] &= 0xFF^(0x7 << 1);  //// << 2 au lieu de 3
00352     // Then shift in our new scale bits:
00353     cmd[1] |= gScl << 1;    //// << 0 au lieu de 3
00354 
00355     // Write the gyroscale out to the gyro
00356     i2c.write(xgAddress, cmd, 2);
00357 
00358     // We've updated the sensor, but we also need to update our class variables
00359     // First update gScale:
00360     gScale = gScl;
00361     // Then calculate a new gRes, which relies on gScale being set correctly:
00362     calcgRes();
00363 }
00364 
00365 void LSM6DS33::setAccelScale(accel_scale aScl)
00366 {
00367     // The start of the addresses we want to read from
00368     char cmd[2] = {
00369         CTRL1_XL,
00370         0
00371     };
00372 
00373     // Write the address we are going to read from and don't end the transaction
00374     i2c.write(xgAddress, cmd, 1, true);
00375     // Read in all the 8 bits of data
00376     i2c.read(xgAddress, cmd+1, 1);
00377 
00378     // Then mask out the accel scale bits:
00379     cmd[1] &= 0xFF^(0x3 << 2);  //// gr 2 au lieu de 3  mise a zero des bits 3 et 4
00380     // Then shift in our new scale bits:
00381     cmd[1] |= aScl << 2;   //// gr 2 au lieu de 3
00382 
00383     // Write the accelscale out to the accel
00384     i2c.write(xgAddress, cmd, 2);
00385 
00386     // We've updated the sensor, but we also need to update our class variables
00387     // First update aScale:
00388     aScale = aScl;
00389     // Then calculate a new aRes, which relies on aScale being set correctly:
00390     calcaRes();
00391 }
00392 
00393 void LSM6DS33::setGyroODR(gyro_odr gRate)
00394 {
00395     // The start of the addresses we want to read from
00396     char cmd[2] = {
00397         CTRL2_G,
00398         0
00399     };
00400 
00401     // Set low power based on ODR, else keep sensor on high performance
00402     if(gRate == G_ODR_13_BW_0 | gRate == G_ODR_26_BW_2 | gRate == G_ODR_52_BW_16) {
00403         char cmdLow[2] = {
00404             CTRL7_G,
00405             1
00406         };
00407 
00408         i2c.write(xgAddress, cmdLow, 2);
00409     } else {
00410         char cmdLow[2] = {
00411             CTRL7_G,
00412             0
00413         };
00414 
00415         i2c.write(xgAddress, cmdLow, 2);
00416     }
00417 
00418     // Write the address we are going to read from and don't end the transaction
00419     i2c.write(xgAddress, cmd, 1, true);
00420     // Read in all the 8 bits of data
00421     i2c.read(xgAddress, cmd+1, 1);
00422 
00423     // Then mask out the gyro odr bits:
00424     cmd[1]  &= 0xFF^(0xF << 4);
00425     // Then shift in our new odr bits:
00426     cmd[1] |= gRate;
00427 
00428     // Write the gyroodr out to the gyro
00429     i2c.write(xgAddress, cmd, 2);
00430 }
00431 
00432 void LSM6DS33::setAccelODR(accel_odr aRate)
00433 {
00434     // The start of the addresses we want to read from
00435     char cmd[2] = {
00436         CTRL1_XL,
00437         0
00438     };
00439 
00440     // Set low power based on ODR, else keep sensor on high performance
00441     if(aRate == A_ODR_13 | aRate == A_ODR_26 | aRate == A_ODR_52) {
00442         char cmdLow[2] = {
00443             CTRL6_C,
00444             1
00445         };
00446 
00447         i2c.write(xgAddress, cmdLow, 2);
00448     } else {
00449         char cmdLow[2] = {
00450             CTRL6_C,
00451             0
00452         };
00453 
00454         i2c.write(xgAddress, cmdLow, 2);
00455     }
00456 
00457     // Write the address we are going to read from and don't end the transaction
00458     i2c.write(xgAddress, cmd, 1, true);
00459     // Read in all the 8 bits of data
00460     i2c.read(xgAddress, cmd+1, 1);
00461 
00462     // Then mask out the accel odr bits:
00463     cmd[1] &= 0xFF^(0xF << 4);  // gr erreur ??
00464     // Then shift in our new odr bits:
00465     cmd[1] |= aRate << 4;     //  gr erreur
00466 
00467     // Write the accelodr out to the accel
00468     i2c.write(xgAddress, cmd, 2);
00469 }
00470 
00471 void LSM6DS33::calcgRes()
00472 {
00473     // Possible gyro scales (and their register bit settings) are:
00474     // 125 DPS , 245 DPS (00), 500 DPS (01), 2000 DPS (10).
00475     switch (gScale) {
00476         case G_SCALE_125DPS:
00477             gRes = 125.0 / 32768.0;
00478             break;
00479         case G_SCALE_250DPS:
00480             gRes = 250.0 / 32768.0;
00481             break;
00482         case G_SCALE_500DPS:
00483             gRes = 500.0 / 32768.0;
00484             break;
00485         case G_SCALE_1000DPS:
00486             gRes = 1000.0 / 32768.0;
00487             break;
00488         case G_SCALE_2000DPS:
00489             gRes = 2000.0 / 32768.0;
00490             break;
00491     }
00492 }
00493 
00494 void LSM6DS33::calcaRes()
00495 {
00496     // Possible accelerometer scales (and their register bit settings) are:
00497     // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100).
00498     switch (aScale) {
00499         case A_SCALE_2G:
00500             aRes = 2.0 / 32768.0;
00501             break;
00502         case A_SCALE_4G:
00503             aRes = 4.0 / 32768.0;
00504             break;
00505         case A_SCALE_8G:
00506             aRes = 8.0 / 32768.0;
00507             break;
00508         case A_SCALE_16G:
00509             aRes = 16.0 / 32768.0;
00510             break;
00511     }
00512 }
00513 void LSM6DS33::calibration( int16_t iter)
00514 {
00515     int32_t gxoll=0,gyoll=0,gzoll=0;
00516     for(int ii=0; ii<iter; ii++) {
00517         this->readGyro();
00518         gx_off=gx_off+gx;
00519         gy_off=gy_off+gy;
00520         gz_off=gz_off+gz;
00521         //
00522         
00523         gxoll=gxoll+(int32_t)gx_raw;
00524         gyoll=gyoll+(int32_t)gy_raw;
00525         gzoll=gzoll+(int32_t)gz_raw;
00526         
00527         
00528         //wait(0.01);
00529     }
00530     gx_off=gx_off/iter;
00531     gy_off=gy_off/iter;
00532     gz_off=gz_off/iter;
00533     //
00534     gxoll=gxoll/iter;
00535     gyoll=gyoll/iter;
00536     gzoll=gzoll/iter;
00537     
00538     //
00539     gxol=(gxoll&0x00FF);
00540     gxoh=(gxoll>>8);
00541     gyol=(gyoll&0x00FF);
00542     gyoh=(gyoll>>8);
00543     gzol=(gzoll&0x00FF);
00544     gzoh=(gzoll>>8);
00545     
00546 }
00547