//****************************************************************************//
//
// 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;
}
