/****************************************************************************** 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 "LSM6DS3.h" #include "stdint.h" #include "Wire.h" #include "SPI.h" #if defined(TARGET_SEEED_XIAO_NRF52840_SENSE) || defined(TARGET_SEEED_XIAO_NRF52840_SENSE_PLUS) #define Wire Wire1 #endif #if defined(ARDUINO_XIAO_MG24) #define Wire Wire1 #define PIN_LSM6DS3TR_C_POWER PD5 #endif //****************************************************************************// // // 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(I2C_MODE), I2CAddress(0x6B), chipSelectPin(10) { commInterface = busType; if (commInterface == I2C_MODE) { I2CAddress = inputArg; } if (commInterface == SPI_MODE) { chipSelectPin = inputArg; } } status_t LSM6DS3Core::beginCore(void) { status_t returnError = IMU_SUCCESS; #ifdef PIN_LSM6DS3TR_C_POWER pinMode(PIN_LSM6DS3TR_C_POWER, OUTPUT); #if defined(NRF52840_XXAA) NRF_P1->PIN_CNF[8] = ((uint32_t)NRF_GPIO_PIN_DIR_OUTPUT << GPIO_PIN_CNF_DIR_Pos) | ((uint32_t)NRF_GPIO_PIN_INPUT_DISCONNECT << GPIO_PIN_CNF_INPUT_Pos) | ((uint32_t)NRF_GPIO_PIN_NOPULL << GPIO_PIN_CNF_PULL_Pos) | ((uint32_t)NRF_GPIO_PIN_H0H1 << GPIO_PIN_CNF_DRIVE_Pos) | ((uint32_t)NRF_GPIO_PIN_NOSENSE << GPIO_PIN_CNF_SENSE_Pos); #endif digitalWrite(PIN_LSM6DS3TR_C_POWER, HIGH); delay(10); #endif switch (commInterface) { case I2C_MODE: Wire.begin(); break; case SPI_MODE: #if defined(NRF52840_XXAA) // start the SPI library: SPI.begin(); // Maximum SPI frequency is 10MHz, could divide by 2 here: #if defined(ARDUINO_XIAO_RA4M1) || defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_ARCH_RP2350) SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE0)); #elif defined(ARDUINO_XIAO_MG24) // SPI.setClockDivider(SPI_CLOCK_DIV4); SPI.setClockDivider(4); #endif // Data is read and written MSb first. #ifdef ESP32 SPI.setBitOrder(SPI_MSBFIRST); #elif defined(ARDUINO_XIAO_RA4M1) || defined(ARDUINO_ARCH_MBED) // MbedSPI and XIAO_RA4M1 don't support setBitOrder - use SPISettings instead #else SPI.setBitOrder(MSBFIRST); #endif // Data is captured on rising edge of clock (CPHA = 0) // Base value of the clock is HIGH (CPOL = 1) // MODE3 for 328p operation #if defined(__AVR__) && !defined(ARDUINO_ARCH_MBED) SPI.setDataMode(SPI_MODE3); #endif // MODE0 for Teensy 3.1 operation #if defined(__MK20DX256__) && !defined(ARDUINO_ARCH_MBED) SPI.setDataMode(SPI_MODE0); #endif // initalize the data ready and chip select pins: pinMode(chipSelectPin, OUTPUT); digitalWrite(chipSelectPin, HIGH); #endif 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 == LSM6DS3_ACC_GYRO_WHO_AM_I || readCheck == LSM6DS3_C_ACC_GYRO_WHO_AM_I)) { 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; #if defined(NRF52840_XXAA) uint8_t tempFFCounter = 0; #endif switch (commInterface) { case I2C_MODE: Wire.beginTransmission(I2CAddress); Wire.write(offset); if (Wire.endTransmission() != 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: #if defined(NRF52840_XXAA) // take the chip select low to select the device: digitalWrite(chipSelectPin, LOW); // send the device the register you want to read: SPI.transfer(offset | 0x80); //Ored with "read request" bit while (i < length) { // slave may send less than requested c = SPI.transfer(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: digitalWrite(chipSelectPin, HIGH); #endif 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 = 0; 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: #if defined(NRF52840_XXAA) // take the chip select low to select the device: digitalWrite(chipSelectPin, LOW); // send the device the register you want to read: SPI.transfer(offset | 0x80); //Ored with "read request" bit // send a value of 0 to read the first byte returned: result = SPI.transfer(0x00); // take the chip select high to de-select: digitalWrite(chipSelectPin, HIGH); if (result == 0xFF) { //we've recieved all ones, report returnError = IMU_ALL_ONES_WARNING; } #endif 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] = {0, 0}; 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: #if defined(NRF52840_XXAA) // take the chip select low to select the device: digitalWrite(chipSelectPin, LOW); // send the device the register you want to read: SPI.transfer(offset); // send a value of 0 to read the first byte returned: SPI.transfer(dataToWrite); // decrement the number of bytes left to read: // take the chip select high to de-select: digitalWrite(chipSelectPin, HIGH); #endif 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 = 416; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666 settings.gyroBandWidth = 400; //Hz. Can be: 50, 100, 200, 400; settings.gyroFifoEnabled = 1; //Set to include gyro in FIFO settings.gyroFifoDecimation = 1; //set 1 for on /1 settings.accelEnabled = 1; settings.accelODROff = 1; settings.accelRange = 16; //Max G force readable. Can be: 2, 4, 8, 16 settings.accelSampleRate = 416; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330 settings.accelBandWidth = 100; //Hz. Can be: 50, 100, 200, 400; settings.accelFifoEnabled = 1; //Set to include accelerometer in the FIFO settings.accelFifoDecimation = 1; //set 1 for on /1 settings.tempEnabled = 1; settings.timestampEnabled = 0; settings.timestampFifoEnabled = 0; settings.timestampResolution = 0; //Select interface mode settings.commMode = 1; //Can be modes 1, 2 or 3 //FIFO control data settings.fifoThreshold = 2047; //Can be 0 to 2047 (11 bits) 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(); //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); //Return WHO AM I reg //Not no mo! uint8_t result; readRegister(&result, LSM6DS3_ACC_GYRO_WHO_AM_I_REG); //Setup the internal temperature sensor if (settings.tempEnabled == 1) { if (result == LSM6DS3_ACC_GYRO_WHO_AM_I) { //0x69 LSM6DS3 settings.tempSensitivity = 16; // Sensitivity to scale 16 } else if (result == LSM6DS3_C_ACC_GYRO_WHO_AM_I) { //0x6A LSM6dS3-C settings.tempSensitivity = 256; // Sensitivity to scale 256 } } if (settings.timestampEnabled) { // Configure the timestamp resolution before starting the counter. This // applies to both direct register reads and FIFO timestamps. uint8_t wake_up_dur; readRegister(&wake_up_dur, LSM6DS3_ACC_GYRO_WAKE_UP_DUR); const uint8_t current_timer_hr = wake_up_dur & LSM6DS3_ACC_GYRO_TIMER_HR_25us; const uint8_t configured_timer_hr = (settings.timestampResolution & 0x01) ? LSM6DS3_ACC_GYRO_TIMER_HR_25us : LSM6DS3_ACC_GYRO_TIMER_HR_6_4ms; if (current_timer_hr != configured_timer_hr) { wake_up_dur &= ~((uint8_t)LSM6DS3_ACC_GYRO_TIMER_HR_25us); wake_up_dur |= configured_timer_hr; writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_DUR, wake_up_dur); // ST requires a counter reset when switching from 6.4 ms to 25 us. if (current_timer_hr == LSM6DS3_ACC_GYRO_TIMER_HR_6_4ms && configured_timer_hr == LSM6DS3_ACC_GYRO_TIMER_HR_25us) { writeRegister(LSM6DS3_ACC_GYRO_TIMESTAMP2_REG, 0xAA); } } // Enable the timestamp counter. TIMER_EN lives at a different // register/bit depending on the die revision (see WHO_AM_I check // above): on the original LSM6DS3 it is TAP_CFG1 bit7, while on // LSM6DS3-C/TR-C that bit position is repurposed and TIMER_EN moved // into CTRL10_C bit5 (which otherwise reads as ZEN_G). if (result == LSM6DS3_ACC_GYRO_WHO_AM_I) { //0x69 LSM6DS3 uint8_t tap_cfg1; readRegister(&tap_cfg1, LSM6DS3_ACC_GYRO_TAP_CFG1); tap_cfg1 |= LSM6DS3_ACC_GYRO_TIMER_EN_ENABLED; writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, tap_cfg1); } else if (result == LSM6DS3_C_ACC_GYRO_WHO_AM_I) { //0x6A LSM6DS3-C/TR-C uint8_t ctrl10_c; readRegister(&ctrl10_c, LSM6DS3_ACC_GYRO_CTRL10_C); ctrl10_c |= LSM6DS3_ACC_GYRO_ZEN_G_ENABLED; // set TIMER_EN writeRegister(LSM6DS3_ACC_GYRO_CTRL10_C, ctrl10_c); } } return returnError; } //****************************************************************************// // // Accelerometer section // //****************************************************************************// 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(readRawAccelX()); 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(readRawAccelY()); 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(readRawAccelZ()); return output; } float LSM6DS3::calcAccel(int16_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(readRawGyroX()); 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(readRawGyroY()); 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(readRawGyroZ()); return output; } float LSM6DS3::calcGyro(int16_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() / settings.tempSensitivity; //divide by tempSensitivity to scale output += 25; //Add 25 degrees to remove offset return output; } float LSM6DS3::readTempF(void) { float output = (float)readRawTemp() / settings.tempSensitivity; //divide by tempSensitivity to scale output += 25; //Add 25 degrees to remove offset output = (output * 9) / 5 + 32; return output; } //****************************************************************************// // // Timestamp section // //****************************************************************************// uint32_t LSM6DS3::fifoTimestamp() { uint8_t data[6]; status_t error = readRegisterRegion(data, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_L, 3*sizeof(uint16_t)); if (error == IMU_SUCCESS) { return ((uint32_t)data[1] << 16) | ((uint32_t)data[0] << 8) | (uint32_t)data[3]; } return 0; } //****************************************************************************// // // 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_CTRL2 uint8_t tempFIFO_CTRL2 = 0; readRegister(&tempFIFO_CTRL2, LSM6DS3_ACC_GYRO_FIFO_CTRL2); if (settings.timestampFifoEnabled == 1) { // Write timestamp data to the FIFO. tempFIFO_CTRL2 |= LSM6DS3_ACC_GYRO_TIM_PEDO_FIFO_EN_ENABLED; // FIFO_CTRL2(07H)->TIMER_PEDO_FIFO_EN } // Adding thresholdHByte to FIFO_CTRL2 register tempFIFO_CTRL2 &= 0xF0; tempFIFO_CTRL2 |= (thresholdHByte & 0x0F); //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 // Select the decimation factor for the 4th FIFO dataset by configuring the DEC_DS4_FIFO[2:0] field in the FIFO_CTRL4 register. //If you don't need the third and fourth datasets, this value can be set to 0 uint8_t tempFIFO_CTRL4 = 0x09; //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); writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL2, tempFIFO_CTRL2); 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) { // The previous implementation drained the FIFO by calling fifoRead() until // the FIFO_EMPTY flag asserted. When the sensor runs in a continuous FIFO // mode (e.g. DYNAMIC STREAM 2, the mode set by fifoBegin()) at high ODR // (e.g. accel at 6660 Hz), new samples are pushed into the FIFO faster than // they can be read back out over the bus, so FIFO_EMPTY never asserts and // the drain loop never exits -- the sketch hangs (see issue #36). // // The datasheet-sanctioned way to clear the FIFO is to switch FIFO_CTRL5.MODE // to BYPASS, which flushes the FIFO contents, and then restore the previous // mode. ("The FIFO buffer is cleared when the FIFO_MODE field is set to // BYPASS mode.") uint8_t ctrl5; readRegister(&ctrl5, LSM6DS3_ACC_GYRO_FIFO_CTRL5); // Clear MODE[2:0] -> BYPASS (000) to flush the FIFO. writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, (ctrl5 & 0xF8) | LSM6DS3_ACC_GYRO_FIFO_MODE_BYPASS); delay(2); // Restore the previous FIFO mode / ODR. writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, ctrl5); } 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_CTRL5, 0x00); //Disable }