Pin usage: MOSI(D4), MISO(D5), SCK(D3), CS(D6), format(8, 3) Test OK.
Fork of LSM6DS3 by
LSM6DS3.cpp
- Committer:
- adam_z
- Date:
- 2016-02-23
- Revision:
- 1:cc2caaf5536c
- Parent:
- 0:301b2cb34ead
- Child:
- 2:c19d384b2896
File content as of revision 1:cc2caaf5536c:
/****************************************************************************** SparkFunLSM6DS3.cpp LSM6DS3 Arduino and Teensy Driver Marshall Taylor @ SparkFun Electronics May 20, 2015 https://github.com/sparkfun/LSM6DS3_Breakout https://github.com/sparkfun/SparkFun_LSM6DS3_Arduino_Library Resources: Uses Wire.h for i2c operation Uses SPI.h for SPI operation Either can be omitted if not used Development environment specifics: Arduino IDE 1.6.4 Teensy loader 1.23 This code is released under the [MIT License](http://opensource.org/licenses/MIT). Please review the LICENSE.md file included with this example. If you have any questions or concerns with licensing, please contact techsupport@sparkfun.com. Distributed as-is; no warranty is given. ******************************************************************************/ //See SparkFunLSM6DS3.h for additional topology notes. #include "mbed.h" #include "LSM6DS3.h" #include "LSM6DS3_Types.h" #include "LSM6DS3_Registers.h" #include "stdint.h" #include "math.h" //I2C i2c(D14,D15); //SPI spi(D11,D12,D13); //****************************************************************************// // // LSM6DS3Core functions. // // Construction arguments: // ( uint8_t busType, uint8_t inputArg ), // // where inputArg is address for I2C_MODE and chip select pin // number for SPI_MODE // // For SPI, construct LSM6DS3Core myIMU(SPI_MODE, 10); // For I2C, construct LSM6DS3Core myIMU(I2C_MODE, 0x6B); // // Default construction is I2C mode, address 0x6B. // //================================= //================================= //****************************************************************************// LSM6DS3Core::LSM6DS3Core( uint8_t busType, uint8_t inputArg) : commInterface(SPI_MODE), I2CAddress(0x6B), chipSelectPin(PB_6), spi(D11,D12,D13), i2c(D14,D15), cs((PinName)chipSelectPin) { commInterface = busType; if( commInterface == I2C_MODE ) { I2CAddress = inputArg; } if( commInterface == SPI_MODE ) { chipSelectPin = inputArg; } } status_t LSM6DS3Core::beginCore(void) { status_t returnError = IMU_SUCCESS; switch (commInterface) { case I2C_MODE: break; case SPI_MODE: // start the SPI library: //DigitalOut cs((PinName)chipSelectPin); // Maximum SPI frequency is 10MHz, could divide by 2 here: spi.frequency(5000000); // Data is read and written MSb first. spi.format(8,0); // Data is captured on rising edge of clock (CPHA = 0) // Base value of the clock is HIGH (CPOL = 1) /* // MODE3 for 328p operation #ifdef __AVR__ SPI.setDataMode(SPI_MODE3); #else #endif // MODE0 for Teensy 3.1 operation #ifdef __MK20DX256__ SPI.setDataMode(SPI_MODE0); #else #endif */ //SPI.format(8,0);//8 bits mode0 // initalize the data ready and chip select pins: cs = 1; break; default: break; } //Spin for a few ms volatile uint8_t temp = 0; for( uint16_t i = 0; i < 10000; i++ ) { temp++; } //Check the ID register to determine if the operation was a success. uint8_t readCheck; readRegister(&readCheck, LSM6DS3_ACC_GYRO_WHO_AM_I_REG); if( readCheck != 0x69 ) { returnError = IMU_HW_ERROR; } return returnError; } //****************************************************************************// // // ReadRegisterRegion // // Parameters: // *outputPointer -- Pass &variable (base address of) to save read data to // offset -- register to read // length -- number of bytes to read // // Note: Does not know if the target memory space is an array or not, or // if there is the array is big enough. if the variable passed is only // two bytes long and 3 bytes are requested, this will over-write some // other memory! // //****************************************************************************// status_t LSM6DS3Core::readRegisterRegion(uint8_t *outputPointer , uint8_t offset, uint8_t length) { status_t returnError = IMU_SUCCESS; //define pointer that will point to the external space uint8_t i = 0; uint8_t c = 0; uint8_t tempFFCounter = 0; switch (commInterface) { case I2C_MODE: /* i2c.write(offset); if( i2c.stop() != 0 ) { returnError = IMU_HW_ERROR; } else { //OK, all worked, keep going // request 6 bytes from slave device Wire.requestFrom(I2CAddress, length); while ( (Wire.available()) && (i < length)) { // slave may send less than requested c = Wire.read(); // receive a byte as character *outputPointer = c; outputPointer++; i++; } } */ break; case SPI_MODE: // take the chip select low to select the device: cs = 0; // send the device the register you want to read: spi.write(offset | 0x80); //Ored with "read request" bit while ( i < length ) { // slave may send less than requested c = spi.write(0x00); // receive a byte as character if( c == 0xFF ) { //May have problem tempFFCounter++; } *outputPointer = c; outputPointer++; i++; } if( tempFFCounter == i ) { //Ok, we've recieved all ones, report returnError = IMU_ALL_ONES_WARNING; } // take the chip select high to de-select: cs = 1; //digitalWrite(chipSelectPin, HIGH); break; default: break; } return returnError; } //****************************************************************************// // // ReadRegister // // Parameters: // *outputPointer -- Pass &variable (address of) to save read data to // offset -- register to read // //****************************************************************************// status_t LSM6DS3Core::readRegister(uint8_t* outputPointer, uint8_t offset) { //Return value uint8_t result; uint8_t numBytes = 1; status_t returnError = IMU_SUCCESS; switch (commInterface) { case I2C_MODE: /* Wire.beginTransmission(I2CAddress); Wire.write(offset); if( Wire.endTransmission() != 0 ) { returnError = IMU_HW_ERROR; } Wire.requestFrom(I2CAddress, numBytes); while ( Wire.available() ) { // slave may send less than requested result = Wire.read(); // receive a byte as a proper uint8_t } */ break; case SPI_MODE: // take the chip select low to select the device: cs = 0; //digitalWrite(chipSelectPin, LOW); // send the device the register you want to read: spi.write(offset | 0x80); //Ored with "read request" bit // send a value of 0 to read the first byte returned: result = spi.write(0x00); // take the chip select high to de-select: cs = 1; //digitalWrite(chipSelectPin, HIGH); if( result == 0xFF ) { //we've recieved all ones, report returnError = IMU_ALL_ONES_WARNING; } break; default: break; } *outputPointer = result; return returnError; } //****************************************************************************// // // readRegisterInt16 // // Parameters: // *outputPointer -- Pass &variable (base address of) to save read data to // offset -- register to read // //****************************************************************************// status_t LSM6DS3Core::readRegisterInt16( int16_t* outputPointer, uint8_t offset ) { uint8_t myBuffer[2]; status_t returnError = readRegisterRegion(myBuffer, offset, 2); //Does memory transfer int16_t output = (int16_t)myBuffer[0] | int16_t(myBuffer[1] << 8); *outputPointer = output; return returnError; } //****************************************************************************// // // writeRegister // // Parameters: // offset -- register to write // dataToWrite -- 8 bit data to write to register // //****************************************************************************// status_t LSM6DS3Core::writeRegister(uint8_t offset, uint8_t dataToWrite) { status_t returnError = IMU_SUCCESS; switch (commInterface) { case I2C_MODE: /* //Write the byte Wire.beginTransmission(I2CAddress); Wire.write(offset); Wire.write(dataToWrite); if( Wire.endTransmission() != 0 ) { returnError = IMU_HW_ERROR; } */ break; case SPI_MODE: // take the chip select low to select the device: cs = 0; //digitalWrite(chipSelectPin, LOW); // send the device the register you want to read: spi.write(offset); // send a value of 0 to read the first byte returned: spi.write(dataToWrite); // decrement the number of bytes left to read: // take the chip select high to de-select: cs = 1;//digitalWrite(chipSelectPin, HIGH); break; //No way to check error on this write (Except to read back but that's not reliable) default: break; } return returnError; } status_t LSM6DS3Core::embeddedPage( void ) { status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x80 ); return returnError; } status_t LSM6DS3Core::basePage( void ) { status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x00 ); return returnError; } //****************************************************************************// // // Main user class -- wrapper for the core class + maths // // Construct with same rules as the core ( uint8_t busType, uint8_t inputArg ) // //****************************************************************************// LSM6DS3::LSM6DS3( uint8_t busType, uint8_t inputArg ) : LSM6DS3Core( busType, inputArg ) { //Construct with these default settings settings.gyroEnabled = 1; //Can be 0 or 1 settings.gyroRange = 2000; //Max deg/s. Can be: 125, 245, 500, 1000, 2000 settings.gyroSampleRate = 833; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666 settings.gyroBandWidth = 400; //Hz. Can be: 50, 100, 200, 400; settings.gyroFifoEnabled = 0; //Set to include gyro in FIFO settings.gyroFifoDecimation = 1; //set 1 for on /1 settings.accelEnabled = 1; settings.accelODROff = 1; settings.accelRange = 8; //Max G force readable. Can be: 2, 4, 8, 16 settings.accelSampleRate = 833; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330 settings.accelBandWidth = 400; //Hz. Can be: 50, 100, 200, 400; settings.accelFifoEnabled = 0; //Set to include accelerometer in the FIFO settings.accelFifoDecimation = 1; //set 1 for on /1 settings.tempEnabled = 1; //Select interface mode settings.commMode = 1; //Can be modes 1, 2 or 3 //FIFO control data settings.fifoThreshold = 3000; //Can be 0 to 4096 (16 bit bytes) settings.fifoSampleRate = 10; //default 10Hz settings.fifoModeWord = 0; //Default off allOnesCounter = 0; nonSuccessCounter = 0; } //****************************************************************************// // // Configuration section // // This uses the stored SensorSettings to start the IMU // Use statements such as "myIMU.settings.commInterface = SPI_MODE;" or // "myIMU.settings.accelEnabled = 1;" to configure before calling .begin(); // //****************************************************************************// status_t LSM6DS3::begin() { //Check the settings structure values to determine how to setup the device uint8_t dataToWrite = 0; //Temporary variable //Begin the inherited core. This gets the physical wires connected status_t returnError = beginCore(); //setOffset(-61, -25, -66, 35, -81, -32); //Setup the accelerometer****************************** dataToWrite = 0; //Start Fresh! if ( settings.accelEnabled == 1) { //Build config reg //First patch in filter bandwidth switch (settings.accelBandWidth) { case 50: dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_50Hz; break; case 100: dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_100Hz; break; case 200: dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_200Hz; break; default: //set default case to max passthrough case 400: dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_400Hz; break; } //Next, patch in full scale switch (settings.accelRange) { case 2: dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_2g; break; case 4: dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_4g; break; case 8: dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_8g; break; default: //set default case to 16(max) case 16: dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_16g; break; } //Lastly, patch in accelerometer ODR switch (settings.accelSampleRate) { case 13: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13Hz; break; case 26: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_26Hz; break; case 52: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_52Hz; break; default: //Set default to 104 case 104: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_104Hz; break; case 208: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_208Hz; break; case 416: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_416Hz; break; case 833: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_833Hz; break; case 1660: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_1660Hz; break; case 3330: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_3330Hz; break; case 6660: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_6660Hz; break; case 13330: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13330Hz; break; } } else { //dataToWrite already = 0 (powerdown); } //Now, write the patched together data writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, dataToWrite); //Set the ODR bit readRegister(&dataToWrite, LSM6DS3_ACC_GYRO_CTRL4_C); dataToWrite &= ~((uint8_t)LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED); if ( settings.accelODROff == 1) { dataToWrite |= LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED; } writeRegister(LSM6DS3_ACC_GYRO_CTRL4_C, dataToWrite); //Setup the gyroscope********************************************** dataToWrite = 0; //Start Fresh! if ( settings.gyroEnabled == 1) { //Build config reg //First, patch in full scale switch (settings.gyroRange) { case 125: dataToWrite |= LSM6DS3_ACC_GYRO_FS_125_ENABLED; break; case 245: dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_245dps; break; case 500: dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_500dps; break; case 1000: dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_1000dps; break; default: //Default to full 2000DPS range case 2000: dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_2000dps; break; } //Lastly, patch in gyro ODR switch (settings.gyroSampleRate) { case 13: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_13Hz; break; case 26: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_26Hz; break; case 52: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_52Hz; break; default: //Set default to 104 case 104: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_104Hz; break; case 208: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_208Hz; break; case 416: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_416Hz; break; case 833: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_833Hz; break; case 1660: dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_1660Hz; break; } } else { //dataToWrite already = 0 (powerdown); } //Write the byte writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, dataToWrite); //Setup the internal temperature sensor if ( settings.tempEnabled == 1) { } //Return WHO AM I reg //Not no mo! uint8_t result; readRegister(&result, LSM6DS3_ACC_GYRO_WHO_AM_I_REG); return returnError; } //****************************************************************************// // // Accelerometer section // //****************************************************************************// void LSM6DS3::setOffset(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz) { accelOffset[0] = ax; accelOffset[1] = ay; accelOffset[2] = az; gyroOffset[0] = gx; gyroOffset[1] = gy; gyroOffset[2] = gz; } int16_t LSM6DS3::readRawAccelX( void ) { int16_t output; status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_XL ); if( errorLevel != IMU_SUCCESS ) { if( errorLevel == IMU_ALL_ONES_WARNING ) { allOnesCounter++; } else { nonSuccessCounter++; } } return output; } float LSM6DS3::readFloatAccelX( void ) { float output = calcAccel((int32_t)readRawAccelX() - (int32_t)accelOffset[0]); return output; } int16_t LSM6DS3::readRawAccelY( void ) { int16_t output; status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_XL ); if( errorLevel != IMU_SUCCESS ) { if( errorLevel == IMU_ALL_ONES_WARNING ) { allOnesCounter++; } else { nonSuccessCounter++; } } return output; } float LSM6DS3::readFloatAccelY( void ) { float output = calcAccel((int32_t)readRawAccelY() - (int32_t)accelOffset[1]); return output; } int16_t LSM6DS3::readRawAccelZ( void ) { int16_t output; status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_XL ); if( errorLevel != IMU_SUCCESS ) { if( errorLevel == IMU_ALL_ONES_WARNING ) { allOnesCounter++; } else { nonSuccessCounter++; } } return output; } float LSM6DS3::readFloatAccelZ( void ) { float output = calcAccel((int32_t)readRawAccelZ() - (int32_t)accelOffset[2]); return output; } float LSM6DS3::calcAccel( int32_t input ) { float output = (float)input * 0.061 * (settings.accelRange >> 1) / 1000; return output; } //****************************************************************************// // // Gyroscope section // //****************************************************************************// int16_t LSM6DS3::readRawGyroX( void ) { int16_t output; status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_G ); if( errorLevel != IMU_SUCCESS ) { if( errorLevel == IMU_ALL_ONES_WARNING ) { allOnesCounter++; } else { nonSuccessCounter++; } } return output; } float LSM6DS3::readFloatGyroX( void ) { float output = calcGyro((int32_t)readRawGyroX() - (int32_t)gyroOffset[0]); return output; } int16_t LSM6DS3::readRawGyroY( void ) { int16_t output; status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_G ); if( errorLevel != IMU_SUCCESS ) { if( errorLevel == IMU_ALL_ONES_WARNING ) { allOnesCounter++; } else { nonSuccessCounter++; } } return output; } float LSM6DS3::readFloatGyroY( void ) { float output = calcGyro((int32_t)readRawGyroY() - (int32_t)gyroOffset[1]); return output; } int16_t LSM6DS3::readRawGyroZ( void ) { int16_t output; status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_G ); if( errorLevel != IMU_SUCCESS ) { if( errorLevel == IMU_ALL_ONES_WARNING ) { allOnesCounter++; } else { nonSuccessCounter++; } } return output; } float LSM6DS3::readFloatGyroZ( void ) { float output = calcGyro((int32_t)readRawGyroZ() - (int32_t)gyroOffset[2]); return output; } float LSM6DS3::calcGyro( int32_t input ) { uint8_t gyroRangeDivisor = settings.gyroRange / 125; if ( settings.gyroRange == 245 ) { gyroRangeDivisor = 2; } float output = (float)input * 4.375 * (gyroRangeDivisor) / 1000; return output; } //****************************************************************************// // // Temperature section // //****************************************************************************// int16_t LSM6DS3::readRawTemp( void ) { int16_t output; readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUT_TEMP_L ); return output; } float LSM6DS3::readTempC( void ) { float output = (float)readRawTemp() / 16; //divide by 16 to scale output += 25; //Add 25 degrees to remove offset return output; } float LSM6DS3::readTempF( void ) { float output = (float)readRawTemp() / 16; //divide by 16 to scale output += 25; //Add 25 degrees to remove offset output = (output * 9) / 5 + 32; return output; } //****************************************************************************// // // FIFO section // //****************************************************************************// void LSM6DS3::fifoBegin( void ) { //CONFIGURE THE VARIOUS FIFO SETTINGS // // //This section first builds a bunch of config words, then goes through //and writes them all. //Split and mask the threshold uint8_t thresholdLByte = settings.fifoThreshold & 0x00FF; uint8_t thresholdHByte = (settings.fifoThreshold & 0x0F00) >> 8; //Pedo bits not configured (ctl2) //CONFIGURE FIFO_CTRL3 uint8_t tempFIFO_CTRL3 = 0; if (settings.gyroFifoEnabled == 1) { //Set up gyro stuff //Build on FIFO_CTRL3 //Set decimation tempFIFO_CTRL3 |= (settings.gyroFifoDecimation & 0x07) << 3; } if (settings.accelFifoEnabled == 1) { //Set up accelerometer stuff //Build on FIFO_CTRL3 //Set decimation tempFIFO_CTRL3 |= (settings.accelFifoDecimation & 0x07); } //CONFIGURE FIFO_CTRL4 (nothing for now-- sets data sets 3 and 4 uint8_t tempFIFO_CTRL4 = 0; //CONFIGURE FIFO_CTRL5 uint8_t tempFIFO_CTRL5 = 0; switch (settings.fifoSampleRate) { default: //set default case to 10Hz(slowest) case 10: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_10Hz; break; case 25: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_25Hz; break; case 50: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_50Hz; break; case 100: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_100Hz; break; case 200: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_200Hz; break; case 400: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_400Hz; break; case 800: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_800Hz; break; case 1600: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_1600Hz; break; case 3300: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_3300Hz; break; case 6600: tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_6600Hz; break; } //Hard code the fifo mode here: tempFIFO_CTRL5 |= settings.fifoModeWord = 6; //set mode: //Write the data writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL1, thresholdLByte); //Serial.println(thresholdLByte, HEX); writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL2, thresholdHByte); //Serial.println(thresholdHByte, HEX); writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL3, tempFIFO_CTRL3); writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL4, tempFIFO_CTRL4); writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, tempFIFO_CTRL5); } void LSM6DS3::fifoClear( void ) { //Drain the fifo data and dump it while( (fifoGetStatus() & 0x1000 ) == 0 ) { fifoRead(); } } int16_t LSM6DS3::fifoRead( void ) { //Pull the last data from the fifo uint8_t tempReadByte = 0; uint16_t tempAccumulator = 0; readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_L); tempAccumulator = tempReadByte; readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_H); tempAccumulator |= ((uint16_t)tempReadByte << 8); return tempAccumulator; } uint16_t LSM6DS3::fifoGetStatus( void ) { //Return some data on the state of the fifo uint8_t tempReadByte = 0; uint16_t tempAccumulator = 0; readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS1); tempAccumulator = tempReadByte; readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS2); tempAccumulator |= (tempReadByte << 8); return tempAccumulator; } void LSM6DS3::fifoEnd( void ) { // turn off the fifo writeRegister(LSM6DS3_ACC_GYRO_FIFO_STATUS1, 0x00); //Disable }