//****************************************************************************// // // 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 begin() { uint8_t result; //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); //Setup the internal temperature sensor if ( settings.tempEnabled == 1) { } //Return WHO AM I reg //Not no mo! readRegister(&result, LSM6DS3_ACC_GYRO_WHO_AM_I_REG); #if 1//for clean robot writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, 0x80);// ODR = 1.66kHz, FS = +/- 245 dps writeRegister(LSM6DS3_ACC_GYRO_CTRL4_C, 0x02);// LPF1 Enable writeRegister(LSM6DS3_ACC_GYRO_CTRL6_G, 0x02);// LPF1 Cutoff = 168Hz writeRegister(LSM6DS3_ACC_GYRO_CTRL7_G, 0x00);// Gyro High Performance Mode Enable #endif return returnError; }
//****************************************************************************// // // 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(SensorSettings* pSettingsYouWanted) { //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(); // Copy the values from the user's settings into the output 'pSettingsYouWanted' // compare settings with 'pSettingsYouWanted' after 'begin' to see if anything changed if(pSettingsYouWanted != NULL){ pSettingsYouWanted->gyroEnabled = settings.gyroEnabled; pSettingsYouWanted->gyroRange = settings.gyroRange; pSettingsYouWanted->gyroSampleRate = settings.gyroSampleRate; pSettingsYouWanted->gyroBandWidth = settings.gyroBandWidth; pSettingsYouWanted->gyroFifoEnabled = settings.gyroFifoEnabled; pSettingsYouWanted->gyroFifoDecimation = settings.gyroFifoDecimation; pSettingsYouWanted->accelEnabled = settings.accelEnabled; pSettingsYouWanted->accelODROff = settings.accelODROff; pSettingsYouWanted->accelRange = settings.accelRange; pSettingsYouWanted->accelSampleRate = settings.accelSampleRate; pSettingsYouWanted->accelBandWidth = settings.accelBandWidth; pSettingsYouWanted->accelFifoEnabled = settings.accelFifoEnabled; pSettingsYouWanted->accelFifoDecimation = settings.accelFifoDecimation; pSettingsYouWanted->tempEnabled = settings.tempEnabled; pSettingsYouWanted->commMode = settings.commMode; pSettingsYouWanted->fifoThreshold = settings.fifoThreshold; pSettingsYouWanted->fifoSampleRate = settings.fifoSampleRate; pSettingsYouWanted->fifoModeWord = settings.fifoModeWord; } //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 settings.accelEnabled = 400; 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) settings.accelRange = 16; 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 settings.accelSampleRate = 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 settings.gyroRange = 2000; 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 settings.gyroSampleRate = 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; }