LSM6DS3.cpp

00001 /******************************************************************************
00002 SparkFunLSM6DS3.cpp
00003 LSM6DS3 Arduino and Teensy Driver
00004 
00005 Marshall Taylor @ SparkFun Electronics
00006 May 20, 2015
00007 https://github.com/sparkfun/LSM6DS3_Breakout
00008 https://github.com/sparkfun/SparkFun_LSM6DS3_Arduino_Library
00009 
00010 Resources:
00011 Uses Wire.h for i2c operation
00012 Uses SPI.h for SPI operation
00013 Either can be omitted if not used
00014 
00015 Development environment specifics:
00016 Arduino IDE 1.6.4
00017 Teensy loader 1.23
00018 
00019 This code is released under the [MIT License](http://opensource.org/licenses/MIT).
00020 
00021 Please review the LICENSE.md file included with this example. If you have any questions
00022 or concerns with licensing, please contact techsupport@sparkfun.com.
00023 
00024 Distributed as-is; no warranty is given.
00025 ******************************************************************************/
00026 
00027 //See SparkFunLSM6DS3.h for additional topology notes.
00028 #include "mbed.h"
00029 #include "LSM6DS3.h"
00030 #include "LSM6DS3_Types.h"
00031 #include "LSM6DS3_Registers.h"
00032 #include "stdint.h"
00033 #include "math.h"
00034 
00035 //I2C i2c(D14,D15);
00036 //SPI spi(D11,D12,D13);
00037 //****************************************************************************//
00038 //
00039 //  LSM6DS3Core functions.
00040 //
00041 //  Construction arguments:
00042 //  ( uint8_t busType, uint8_t inputArg ),
00043 //
00044 //    where inputArg is address for I2C_MODE and chip select pin
00045 //    number for SPI_MODE
00046 //
00047 //  For SPI, construct LSM6DS3Core myIMU(SPI_MODE, 10);
00048 //  For I2C, construct LSM6DS3Core myIMU(I2C_MODE, 0x6B);
00049 //
00050 //  Default construction is I2C mode, address 0x6B.
00051 //
00052 
00053 //=================================
00054 
00055 //=================================
00056 //****************************************************************************//
00057 LSM6DS3Core::LSM6DS3Core( uint8_t busType, uint8_t inputArg) : commInterface(SPI_MODE), I2CAddress(0x6B), spi_(SPI_MOSI,SPI_MISO,SPI_SCK), i2c_(I2C_SDA,I2C_SCL), cs_(SPI_CS)
00058 {
00059     commInterface = busType;
00060     if( commInterface == I2C_MODE ) {
00061         I2CAddress = inputArg;
00062     }
00063     if( commInterface == SPI_MODE ) {
00064         chipSelectPin = inputArg;
00065     }
00066     
00067 
00068 }
00069 
00070 status_t LSM6DS3Core::beginCore(void)
00071 {
00072     status_t returnError = IMU_SUCCESS;
00073 
00074     switch (commInterface) {
00075 
00076         case I2C_MODE:
00077 
00078             break;
00079 
00080         case SPI_MODE:
00081             // start the SPI library:
00082 
00083             //DigitalOut cs((PinName)chipSelectPin);
00084             // Maximum SPI frequency is 10MHz, could divide by 2 here:
00085             spi_.frequency(5000000);
00086             // Data is read and written MSb first.
00087             spi_.format(8,0);
00088             // Data is captured on rising edge of clock (CPHA = 0)
00089             // Base value of the clock is HIGH (CPOL = 1)
00090 
00091             /*      // MODE3 for 328p operation
00092             #ifdef __AVR__
00093                     SPI.setDataMode(SPI_MODE3);
00094             #else
00095             #endif
00096 
00097                     // MODE0 for Teensy 3.1 operation
00098             #ifdef __MK20DX256__
00099                     SPI.setDataMode(SPI_MODE0);
00100             #else
00101             #endif
00102             */
00103             //SPI.format(8,0);//8 bits mode0
00104             // initalize the  data ready and chip select pins:
00105 
00106             cs_ = 1;
00107             break;
00108         default:
00109             break;
00110     }
00111 
00112     //Spin for a few ms
00113     volatile uint8_t temp = 0;
00114     for( uint16_t i = 0; i < 10000; i++ ) {
00115         temp++;
00116     }
00117 
00118     //Check the ID register to determine if the operation was a success.
00119     uint8_t readCheck;
00120     readRegister(&readCheck, LSM6DS3_ACC_GYRO_WHO_AM_I_REG);
00121     if( readCheck != 0x69 ) {
00122         returnError = IMU_HW_ERROR;
00123     }
00124 
00125     return returnError;
00126 
00127 }
00128 
00129 //****************************************************************************//
00130 //
00131 //  ReadRegisterRegion
00132 //
00133 //  Parameters:
00134 //    *outputPointer -- Pass &variable (base address of) to save read data to
00135 //    offset -- register to read
00136 //    length -- number of bytes to read
00137 //
00138 //  Note:  Does not know if the target memory space is an array or not, or
00139 //    if there is the array is big enough.  if the variable passed is only
00140 //    two bytes long and 3 bytes are requested, this will over-write some
00141 //    other memory!
00142 //
00143 //****************************************************************************//
00144 status_t LSM6DS3Core::readRegisterRegion(uint8_t *outputPointer , uint8_t offset, uint8_t length)
00145 {
00146     status_t returnError = IMU_SUCCESS;
00147 
00148     //define pointer that will point to the external space
00149     uint8_t i = 0;
00150     uint8_t c = 0;
00151     uint8_t tempFFCounter = 0;
00152 
00153     switch (commInterface) {
00154 
00155         case I2C_MODE:
00156 /*
00157             i2c.write(offset);
00158             if( i2c.stop() != 0 ) {
00159                 returnError = IMU_HW_ERROR;
00160             } else { //OK, all worked, keep going
00161                 // request 6 bytes from slave device
00162                 Wire.requestFrom(I2CAddress, length);
00163                 while ( (Wire.available()) && (i < length)) { // slave may send less than requested
00164                     c = Wire.read(); // receive a byte as character
00165                     *outputPointer = c;
00166                     outputPointer++;
00167                     i++;
00168                 }
00169             }
00170             */
00171             break;
00172 
00173         case SPI_MODE:
00174             // take the chip select low to select the device:
00175             cs_ = 0;
00176             // send the device the register you want to read:
00177             spi_.write(offset | 0x80);  //Ored with "read request" bit
00178             while ( i < length ) { // slave may send less than requested
00179                 c = spi_.write(0x00); // receive a byte as character
00180                 if( c == 0xFF ) {
00181                     //May have problem
00182                     tempFFCounter++;
00183                 }
00184                 *outputPointer = c;
00185                 outputPointer++;
00186                 i++;
00187             }
00188             if( tempFFCounter == i ) {
00189                 //Ok, we've recieved all ones, report
00190                 returnError = IMU_ALL_ONES_WARNING;
00191             }
00192             // take the chip select high to de-select:
00193             cs_ = 1; //digitalWrite(chipSelectPin, HIGH);
00194             break;
00195 
00196         default:
00197             break;
00198     }
00199 
00200     return returnError;
00201 }
00202 
00203 //****************************************************************************//
00204 //
00205 //  ReadRegister
00206 //
00207 //  Parameters:
00208 //    *outputPointer -- Pass &variable (address of) to save read data to
00209 //    offset -- register to read
00210 //
00211 //****************************************************************************//
00212 status_t LSM6DS3Core::readRegister(uint8_t* outputPointer, uint8_t offset)
00213 {
00214     //Return value
00215     uint8_t result;
00216     //uint8_t numBytes = 1;
00217     status_t returnError = IMU_SUCCESS;
00218 
00219     switch (commInterface) {
00220 
00221         case I2C_MODE:
00222   /*          Wire.beginTransmission(I2CAddress);
00223             Wire.write(offset);
00224             if( Wire.endTransmission() != 0 ) {
00225                 returnError = IMU_HW_ERROR;
00226             }
00227             Wire.requestFrom(I2CAddress, numBytes);
00228             while ( Wire.available() ) { // slave may send less than requested
00229                 result = Wire.read(); // receive a byte as a proper uint8_t
00230             }
00231             */
00232             break;
00233 
00234         case SPI_MODE:
00235             // take the chip select low to select the device:
00236             cs_ = 0; //digitalWrite(chipSelectPin, LOW);
00237             // send the device the register you want to read:
00238             spi_.write(offset | 0x80);  //Ored with "read request" bit
00239             // send a value of 0 to read the first byte returned:
00240             result = spi_.write(0x00);
00241             // take the chip select high to de-select:
00242             cs_ = 1; //digitalWrite(chipSelectPin, HIGH);
00243 
00244             if( result == 0xFF ) {
00245                 //we've recieved all ones, report
00246                 returnError = IMU_ALL_ONES_WARNING;
00247             }
00248             break;
00249 
00250         default:
00251             break;
00252     }
00253 
00254     *outputPointer = result;
00255     return returnError;
00256 }
00257 
00258 //****************************************************************************//
00259 //
00260 //  readRegisterInt16
00261 //
00262 //  Parameters:
00263 //    *outputPointer -- Pass &variable (base address of) to save read data to
00264 //    offset -- register to read
00265 //
00266 //****************************************************************************//
00267 status_t LSM6DS3Core::readRegisterInt16( int16_t* outputPointer, uint8_t offset )
00268 {
00269     uint8_t myBuffer[2];
00270     status_t returnError = readRegisterRegion(myBuffer, offset, 2);  //Does memory transfer
00271     int16_t output = (int16_t)myBuffer[0] | int16_t(myBuffer[1] << 8);
00272 
00273     *outputPointer = output;
00274     return returnError;
00275 }
00276 
00277 //****************************************************************************//
00278 //
00279 //  writeRegister
00280 //
00281 //  Parameters:
00282 //    offset -- register to write
00283 //    dataToWrite -- 8 bit data to write to register
00284 //
00285 //****************************************************************************//
00286 status_t LSM6DS3Core::writeRegister(uint8_t offset, uint8_t dataToWrite)
00287 {
00288     status_t returnError = IMU_SUCCESS;
00289     switch (commInterface) {
00290         case I2C_MODE:
00291 /*            //Write the byte
00292             Wire.beginTransmission(I2CAddress);
00293             Wire.write(offset);
00294             Wire.write(dataToWrite);
00295             if( Wire.endTransmission() != 0 ) {
00296                 returnError = IMU_HW_ERROR;
00297             }
00298   */          break;
00299 
00300         case SPI_MODE:
00301             // take the chip select low to select the device:
00302             cs_ = 0; //digitalWrite(chipSelectPin, LOW);
00303             // send the device the register you want to read:
00304             spi_.write(offset);
00305             // send a value of 0 to read the first byte returned:
00306             spi_.write(dataToWrite);
00307             // decrement the number of bytes left to read:
00308             // take the chip select high to de-select:
00309             cs_ = 1;//digitalWrite(chipSelectPin, HIGH);
00310             break;
00311 
00312             //No way to check error on this write (Except to read back but that's not reliable)
00313 
00314         default:
00315             break;
00316     }
00317 
00318     return returnError;
00319 }
00320 
00321 status_t LSM6DS3Core::embeddedPage( void )
00322 {
00323     status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x80 );
00324 
00325     return returnError;
00326 }
00327 
00328 status_t LSM6DS3Core::basePage( void )
00329 {
00330     status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x00 );
00331 
00332     return returnError;
00333 }
00334 
00335 
00336 //****************************************************************************//
00337 //
00338 //  Main user class -- wrapper for the core class + maths
00339 //
00340 //  Construct with same rules as the core ( uint8_t busType, uint8_t inputArg )
00341 //
00342 //****************************************************************************//
00343 LSM6DS3::LSM6DS3( uint8_t busType, uint8_t inputArg ) : LSM6DS3Core( busType, inputArg )
00344 {
00345     //Construct with these default settings
00346 
00347     settings.gyroEnabled = 1;  //Can be 0 or 1
00348     settings.gyroRange = 2000;   //Max deg/s.  Can be: 125, 245, 500, 1000, 2000
00349     settings.gyroSampleRate = 833;   //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666
00350     settings.gyroBandWidth = 400;  //Hz.  Can be: 50, 100, 200, 400;
00351     settings.gyroFifoEnabled = 0;  //Set to include gyro in FIFO
00352     settings.gyroFifoDecimation = 1;  //set 1 for on /1
00353 
00354     settings.accelEnabled = 1;
00355     settings.accelODROff = 1;
00356     settings.accelRange = 8;      //Max G force readable.  Can be: 2, 4, 8, 16
00357     settings.accelSampleRate = 833;  //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
00358     settings.accelBandWidth = 400;  //Hz.  Can be: 50, 100, 200, 400;
00359     settings.accelFifoEnabled = 0;  //Set to include accelerometer in the FIFO
00360     settings.accelFifoDecimation = 1;  //set 1 for on /1
00361 
00362     settings.tempEnabled = 1;
00363 
00364     //Select interface mode
00365     settings.commMode = 1;  //Can be modes 1, 2 or 3
00366 
00367     //FIFO control data
00368     settings.fifoThreshold = 3000;  //Can be 0 to 4096 (16 bit bytes)
00369     settings.fifoSampleRate = 10;  //default 10Hz
00370     settings.fifoModeWord = 0;  //Default off
00371 
00372     allOnesCounter = 0;
00373     nonSuccessCounter = 0;
00374 
00375 }
00376 
00377 //****************************************************************************//
00378 //
00379 //  Configuration section
00380 //
00381 //  This uses the stored SensorSettings to start the IMU
00382 //  Use statements such as "myIMU.settings.commInterface = SPI_MODE;" or
00383 //  "myIMU.settings.accelEnabled = 1;" to configure before calling .begin();
00384 //
00385 //****************************************************************************//
00386 status_t LSM6DS3::begin()
00387 {
00388     //Check the settings structure values to determine how to setup the device
00389     uint8_t dataToWrite = 0;  //Temporary variable
00390 
00391     //Begin the inherited core.  This gets the physical wires connected
00392     status_t returnError = beginCore();
00393 
00394     //setOffset(-61, -25, -66, 35, -81, -32);
00395 
00396     //Setup the accelerometer******************************
00397     dataToWrite = 0; //Start Fresh!
00398     if ( settings.accelEnabled == 1) {
00399         //Build config reg
00400         //First patch in filter bandwidth
00401         switch (settings.accelBandWidth) {
00402             case 50:
00403                 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_50Hz;
00404                 break;
00405             case 100:
00406                 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_100Hz;
00407                 break;
00408             case 200:
00409                 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_200Hz;
00410                 break;
00411             default:  //set default case to max passthrough
00412             case 400:
00413                 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_400Hz;
00414                 break;
00415         }
00416         //Next, patch in full scale
00417         switch (settings.accelRange) {
00418             case 2:
00419                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_2g;
00420                 break;
00421             case 4:
00422                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_4g;
00423                 break;
00424             case 8:
00425                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_8g;
00426                 break;
00427             default:  //set default case to 16(max)
00428             case 16:
00429                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_16g;
00430                 break;
00431         }
00432         //Lastly, patch in accelerometer ODR
00433         switch (settings.accelSampleRate) {
00434             case 13:
00435                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13Hz;
00436                 break;
00437             case 26:
00438                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_26Hz;
00439                 break;
00440             case 52:
00441                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_52Hz;
00442                 break;
00443             default:  //Set default to 104
00444             case 104:
00445                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_104Hz;
00446                 break;
00447             case 208:
00448                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_208Hz;
00449                 break;
00450             case 416:
00451                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_416Hz;
00452                 break;
00453             case 833:
00454                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_833Hz;
00455                 break;
00456             case 1660:
00457                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_1660Hz;
00458                 break;
00459             case 3330:
00460                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_3330Hz;
00461                 break;
00462             case 6660:
00463                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_6660Hz;
00464                 break;
00465             case 13330:
00466                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13330Hz;
00467                 break;
00468         }
00469     } else {
00470         //dataToWrite already = 0 (powerdown);
00471     }
00472 
00473     //Now, write the patched together data
00474     writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, dataToWrite);
00475 
00476     //Set the ODR bit
00477     readRegister(&dataToWrite, LSM6DS3_ACC_GYRO_CTRL4_C);
00478     dataToWrite &= ~((uint8_t)LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED);
00479     if ( settings.accelODROff == 1) {
00480         dataToWrite |= LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED;
00481     }
00482     writeRegister(LSM6DS3_ACC_GYRO_CTRL4_C, dataToWrite);
00483 
00484     //Setup the gyroscope**********************************************
00485     dataToWrite = 0; //Start Fresh!
00486     if ( settings.gyroEnabled == 1) {
00487         //Build config reg
00488         //First, patch in full scale
00489         switch (settings.gyroRange) {
00490             case 125:
00491                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_125_ENABLED;
00492                 break;
00493             case 245:
00494                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_245dps;
00495                 break;
00496             case 500:
00497                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_500dps;
00498                 break;
00499             case 1000:
00500                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_1000dps;
00501                 break;
00502             default:  //Default to full 2000DPS range
00503             case 2000:
00504                 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_2000dps;
00505                 break;
00506         }
00507         //Lastly, patch in gyro ODR
00508         switch (settings.gyroSampleRate) {
00509             case 13:
00510                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_13Hz;
00511                 break;
00512             case 26:
00513                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_26Hz;
00514                 break;
00515             case 52:
00516                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_52Hz;
00517                 break;
00518             default:  //Set default to 104
00519             case 104:
00520                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_104Hz;
00521                 break;
00522             case 208:
00523                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_208Hz;
00524                 break;
00525             case 416:
00526                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_416Hz;
00527                 break;
00528             case 833:
00529                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_833Hz;
00530                 break;
00531             case 1660:
00532                 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_1660Hz;
00533                 break;
00534         }
00535     } else {
00536         //dataToWrite already = 0 (powerdown);
00537     }
00538     //Write the byte
00539     writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, dataToWrite);
00540 
00541     //Setup the internal temperature sensor
00542     if ( settings.tempEnabled == 1) {
00543     }
00544 
00545     //Return WHO AM I reg  //Not no mo!
00546     uint8_t result;
00547     readRegister(&result, LSM6DS3_ACC_GYRO_WHO_AM_I_REG);
00548 
00549     return returnError;
00550 }
00551 
00552 //****************************************************************************//
00553 //
00554 //  Accelerometer section
00555 //
00556 //****************************************************************************//
00557 
00558 void LSM6DS3::setOffset(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz)
00559 {
00560     accelOffset[0] = ax;
00561     accelOffset[1] = ay;
00562     accelOffset[2] = az;
00563     gyroOffset[0] = gx;
00564     gyroOffset[1] = gy;
00565     gyroOffset[2] = gz;
00566 }
00567 
00568 int16_t LSM6DS3::readRawAccelX( void )
00569 {
00570     int16_t output;
00571     status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_XL );
00572     if( errorLevel != IMU_SUCCESS ) {
00573         if( errorLevel == IMU_ALL_ONES_WARNING ) {
00574             allOnesCounter++;
00575         } else {
00576             nonSuccessCounter++;
00577         }
00578     }
00579     return output;
00580 }
00581 float LSM6DS3::readFloatAccelX( void )
00582 {
00583     float output = calcAccel((int32_t)readRawAccelX()  - (int32_t)accelOffset[0]);
00584     return output;
00585 }
00586 
00587 int16_t LSM6DS3::readRawAccelY( void )
00588 {
00589     int16_t output;
00590     status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_XL );
00591     if( errorLevel != IMU_SUCCESS ) {
00592         if( errorLevel == IMU_ALL_ONES_WARNING ) {
00593             allOnesCounter++;
00594         } else {
00595             nonSuccessCounter++;
00596         }
00597     }
00598     return output;
00599 }
00600 float LSM6DS3::readFloatAccelY( void )
00601 {
00602     float output = calcAccel((int32_t)readRawAccelY() - (int32_t)accelOffset[1]);
00603     return output;
00604 }
00605 
00606 int16_t LSM6DS3::readRawAccelZ( void )
00607 {
00608     int16_t output;
00609     status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_XL );
00610     if( errorLevel != IMU_SUCCESS ) {
00611         if( errorLevel == IMU_ALL_ONES_WARNING ) {
00612             allOnesCounter++;
00613         } else {
00614             nonSuccessCounter++;
00615         }
00616     }
00617     return output;
00618 }
00619 float LSM6DS3::readFloatAccelZ( void )
00620 {
00621     float output = calcAccel((int32_t)readRawAccelZ() - (int32_t)accelOffset[2]);
00622     return output;
00623 }
00624 
00625 float LSM6DS3::calcAccel( int32_t input )
00626 {
00627     float output = (float)input * 0.061 * (settings.accelRange >> 1) / 1000;
00628     return output;
00629 }
00630 
00631 //****************************************************************************//
00632 //
00633 //  Gyroscope section
00634 //
00635 //****************************************************************************//
00636 int16_t LSM6DS3::readRawGyroX( void )
00637 {
00638     int16_t output;
00639     status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_G );
00640     if( errorLevel != IMU_SUCCESS ) {
00641         if( errorLevel == IMU_ALL_ONES_WARNING ) {
00642             allOnesCounter++;
00643         } else {
00644             nonSuccessCounter++;
00645         }
00646     }
00647     return output;
00648 }
00649 float LSM6DS3::readFloatGyroX( void )
00650 {
00651     float output = calcGyro((int32_t)readRawGyroX() - (int32_t)gyroOffset[0]);
00652     return output;
00653 }
00654 
00655 int16_t LSM6DS3::readRawGyroY( void )
00656 {
00657     int16_t output;
00658     status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_G );
00659     if( errorLevel != IMU_SUCCESS ) {
00660         if( errorLevel == IMU_ALL_ONES_WARNING ) {
00661             allOnesCounter++;
00662         } else {
00663             nonSuccessCounter++;
00664         }
00665     }
00666     return output;
00667 }
00668 float LSM6DS3::readFloatGyroY( void )
00669 {
00670     float output = calcGyro((int32_t)readRawGyroY() - (int32_t)gyroOffset[1]);
00671     return output;
00672 }
00673 
00674 int16_t LSM6DS3::readRawGyroZ( void )
00675 {
00676     int16_t output;
00677     status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_G );
00678     if( errorLevel != IMU_SUCCESS ) {
00679         if( errorLevel == IMU_ALL_ONES_WARNING ) {
00680             allOnesCounter++;
00681         } else {
00682             nonSuccessCounter++;
00683         }
00684     }
00685     return output;
00686 }
00687 float LSM6DS3::readFloatGyroZ( void )
00688 {
00689     float output = calcGyro((int32_t)readRawGyroZ() - (int32_t)gyroOffset[2]);
00690     return output;
00691 }
00692 
00693 float LSM6DS3::calcGyro( int32_t input )
00694 {
00695     uint8_t gyroRangeDivisor = settings.gyroRange / 125;
00696     if ( settings.gyroRange == 245 ) {
00697         gyroRangeDivisor = 2;
00698     }
00699 
00700     float output = (float)input * 4.375 * (gyroRangeDivisor) / 1000;
00701     return output;
00702 }
00703 
00704 //****************************************************************************//
00705 //
00706 //  Temperature section
00707 //
00708 //****************************************************************************//
00709 int16_t LSM6DS3::readRawTemp( void )
00710 {
00711     int16_t output;
00712     readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUT_TEMP_L );
00713     return output;
00714 }
00715 
00716 float LSM6DS3::readTempC( void )
00717 {
00718     float output = (float)readRawTemp() / 16; //divide by 16 to scale
00719     output += 25; //Add 25 degrees to remove offset
00720 
00721     return output;
00722 
00723 }
00724 
00725 float LSM6DS3::readTempF( void )
00726 {
00727     float output = (float)readRawTemp() / 16; //divide by 16 to scale
00728     output += 25; //Add 25 degrees to remove offset
00729     output = (output * 9) / 5 + 32;
00730 
00731     return output;
00732 
00733 }
00734 
00735 //****************************************************************************//
00736 //
00737 //  FIFO section
00738 //
00739 //****************************************************************************//
00740 void LSM6DS3::fifoBegin( void )
00741 {
00742     //CONFIGURE THE VARIOUS FIFO SETTINGS
00743     //
00744     //
00745     //This section first builds a bunch of config words, then goes through
00746     //and writes them all.
00747 
00748     //Split and mask the threshold
00749     uint8_t thresholdLByte = settings.fifoThreshold & 0x00FF;
00750     uint8_t thresholdHByte = (settings.fifoThreshold & 0x0F00) >> 8;
00751     //Pedo bits not configured (ctl2)
00752 
00753     //CONFIGURE FIFO_CTRL3
00754     uint8_t tempFIFO_CTRL3 = 0;
00755     if (settings.gyroFifoEnabled == 1) {
00756         //Set up gyro stuff
00757         //Build on FIFO_CTRL3
00758         //Set decimation
00759         tempFIFO_CTRL3 |= (settings.gyroFifoDecimation & 0x07) << 3;
00760 
00761     }
00762     if (settings.accelFifoEnabled == 1) {
00763         //Set up accelerometer stuff
00764         //Build on FIFO_CTRL3
00765         //Set decimation
00766         tempFIFO_CTRL3 |= (settings.accelFifoDecimation & 0x07);
00767     }
00768 
00769     //CONFIGURE FIFO_CTRL4  (nothing for now-- sets data sets 3 and 4
00770     uint8_t tempFIFO_CTRL4 = 0;
00771 
00772 
00773     //CONFIGURE FIFO_CTRL5
00774     uint8_t tempFIFO_CTRL5 = 0;
00775     switch (settings.fifoSampleRate) {
00776         default:  //set default case to 10Hz(slowest)
00777         case 10:
00778             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_10Hz;
00779             break;
00780         case 25:
00781             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_25Hz;
00782             break;
00783         case 50:
00784             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_50Hz;
00785             break;
00786         case 100:
00787             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_100Hz;
00788             break;
00789         case 200:
00790             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_200Hz;
00791             break;
00792         case 400:
00793             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_400Hz;
00794             break;
00795         case 800:
00796             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_800Hz;
00797             break;
00798         case 1600:
00799             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_1600Hz;
00800             break;
00801         case 3300:
00802             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_3300Hz;
00803             break;
00804         case 6600:
00805             tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_6600Hz;
00806             break;
00807     }
00808     //Hard code the fifo mode here:
00809     tempFIFO_CTRL5 |= settings.fifoModeWord = 6;  //set mode:
00810 
00811     //Write the data
00812     writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL1, thresholdLByte);
00813     //Serial.println(thresholdLByte, HEX);
00814     writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL2, thresholdHByte);
00815     //Serial.println(thresholdHByte, HEX);
00816     writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL3, tempFIFO_CTRL3);
00817     writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL4, tempFIFO_CTRL4);
00818     writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, tempFIFO_CTRL5);
00819 
00820 }
00821 void LSM6DS3::fifoClear( void )
00822 {
00823     //Drain the fifo data and dump it
00824     while( (fifoGetStatus() & 0x1000 ) == 0 ) {
00825         fifoRead();
00826     }
00827 
00828 }
00829 int16_t LSM6DS3::fifoRead( void )
00830 {
00831     //Pull the last data from the fifo
00832     uint8_t tempReadByte = 0;
00833     uint16_t tempAccumulator = 0;
00834     readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_L);
00835     tempAccumulator = tempReadByte;
00836     readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_H);
00837     tempAccumulator |= ((uint16_t)tempReadByte << 8);
00838 
00839     return tempAccumulator;
00840 }
00841 
00842 uint16_t LSM6DS3::fifoGetStatus( void )
00843 {
00844     //Return some data on the state of the fifo
00845     uint8_t tempReadByte = 0;
00846     uint16_t tempAccumulator = 0;
00847     readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS1);
00848     tempAccumulator = tempReadByte;
00849     readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS2);
00850     tempAccumulator |= (tempReadByte << 8);
00851 
00852     return tempAccumulator;
00853 
00854 }
00855 void LSM6DS3::fifoEnd( void )
00856 {
00857     // turn off the fifo
00858     writeRegister(LSM6DS3_ACC_GYRO_FIFO_STATUS1, 0x00);  //Disable
00859 }