void setAccelScale(uint8_t aScl)
{
// We need to preserve the other bytes in CTRL_REG6_XL. So, first read it:
uint8_t tempRegValue = xgReadByte(CTRL_REG6_XL);
// Mask out accel scale bits:
tempRegValue &= 0xE7;
switch (aScl)
{
case 4:
tempRegValue |= (0x2 << 3);
settings.accel.scale = 4;
break;
case 8:
tempRegValue |= (0x3 << 3);
settings.accel.scale = 8;
break;
case 16:
tempRegValue |= (0x1 << 3);
settings.accel.scale = 16;
break;
default: // Otherwise it'll be set to 2g (0x0 << 3)
settings.accel.scale = 2;
break;
}
xgWriteByte(CTRL_REG6_XL, tempRegValue);
// Then calculate a new aRes, which relies on aScale being set correctly:
calcaRes();
}
uint16_t LSM9DS1::begin()
{
//! Todo: don't use ACCEL_GYRO_ADDRESS or MAGNETOMETER_ADDRESS, duplicating memory
constrainScales();
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(); // Calculate g / ADC tick, stored in aRes variable
initI2C(); // Initialize I2C
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
uint8_t mTest = mReadByte(WHO_AM_I_M); // Read the gyro WHO_AM_I
uint8_t xgTest = xgReadByte(WHO_AM_I_XG); // Read the accel/mag WHO_AM_I
uint16_t whoAmICombined = (xgTest << 8) | mTest;
if (whoAmICombined != ((WHO_AM_I_AG_RSP << 8) | WHO_AM_I_M_RSP))
return 0;
// Gyro initialization stuff:
initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
// Accelerometer initialization stuff:
initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
// Magnetometer initialization stuff:
initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
// Once everything is initialized, return the WHO_AM_I registers we read:
return whoAmICombined;
}
uint8_t getAccelIntSrc(void)
{
uint8_t intSrc = xgReadByte(INT_GEN_SRC_XL);
// Check if the IA_XL (interrupt active) bit is set
if (intSrc & (1<<6))
{
return (intSrc & 0x3F);
}
return 0;
}
uint8_t getGyroIntSrc()
{
uint8_t intSrc = xgReadByte(INT_GEN_SRC_G);
// Check if the IA_G (interrupt active) bit is set
if (intSrc & (1<<6))
{
return (intSrc & 0x3F);
}
return 0;
}
void enableFIFO(bool enable)
{
uint8_t temp = xgReadByte(CTRL_REG9);
if(enable)
{
temp |= (1<<1);
}
else
{
temp &= ~(1<<1);
}
xgWriteByte(CTRL_REG9, temp);
}
void setAccelODR(uint8_t aRate)
{
// Only do this if aRate is not 0 (which would disable the accel)
if ((aRate & 0x07) != 0)
{
// We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
uint8_t temp = xgReadByte(CTRL_REG6_XL);
// Then mask out the accel ODR bits:
temp &= 0x1F;
// Then shift in our new ODR bits:
temp |= ((aRate & 0x07) << 5);
settings.accel.sampleRate = aRate & 0x07;
// And write the new register value back into CTRL_REG1_XM:
xgWriteByte(CTRL_REG6_XL, temp);
}
}
void setGyroODR(uint8_t gRate)
{
// Only do this if gRate is not 0 (which would disable the gyro)
if ((gRate & 0x07) != 0)
{
// We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
uint8_t temp = xgReadByte(CTRL_REG1_G);
// Then mask out the gyro ODR bits:
temp &= 0xFF^(0x7 << 5);
temp |= (gRate & 0x07) << 5;
// Update our settings struct
settings.gyro.sampleRate = gRate & 0x07;
// And write the new register value back into CTRL_REG1_G:
xgWriteByte(CTRL_REG1_G, temp);
}
}
uint16_t begin(void)
{
//! Todo: don't use _xgAddress or _mAddress, duplicating memory
_xgAddress = settings.device.agAddress;
_mAddress = settings.device.mAddress;
constrainScales();
// Once we have the scale values, we can calculate the resolution
// of each sensor. That's what these functions are for. One for each sensor
calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(); // Calculate g / ADC tick, stored in aRes variable
if (settings.device.commInterface == IMU_MODE_I2C) // If we're using I2C
initI2C(); // Initialize I2C
else if (settings.device.commInterface == IMU_MODE_SPI) // else, if we're using SPI
initSPI(); // Initialize SPI
// To verify communication, we can read from the WHO_AM_I register of
// each device. Store those in a variable so we can return them.
uint8_t mTest = mReadByte(WHO_AM_I_M); // Read the gyro WHO_AM_I
delay(DELAY * 150);
uint8_t xgTest = xgReadByte(WHO_AM_I_XG); // Read the accel/mag WHO_AM_I
uint16_t whoAmICombined = (xgTest << 8) | mTest;
if (whoAmICombined != ((WHO_AM_I_AG_RSP << 8) | WHO_AM_I_M_RSP))
{
return 0;
}
// Gyro initialization stuff:
initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
// Accelerometer initialization stuff:
initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
// Magnetometer initialization stuff:
initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
// Once everything is initialized, return the WHO_AM_I registers we read:
return whoAmICombined;
}
void configInt(interrupt_select interupt, uint8_t generator, h_lactive activeLow, pp_od pushPull)
{
// Write to INT1_CTRL or INT2_CTRL. [interupt] should already be one of
// those two values.
// [generator] should be an OR'd list of values from the interrupt_generators enum
xgWriteByte(interupt, generator);
// Configure CTRL_REG8
uint8_t temp;
temp = xgReadByte(CTRL_REG8);
if (activeLow) temp |= (1<<5);
else temp &= ~(1<<5);
if (pushPull) temp &= ~(1<<4);
else temp |= (1<<4);
xgWriteByte(CTRL_REG8, temp);
}
void calibrate(bool autoCalc)
{
uint8_t data[6] = {0, 0, 0, 0, 0, 0};
uint8_t samples = 0;
int ii;
int32_t aBiasRawTemp[3] = {0, 0, 0};
int32_t gBiasRawTemp[3] = {0, 0, 0};
// Turn on FIFO and set threshold to 32 samples
enableFIFO(TRUE);
setFIFO(FIFO_THS, 0x1F);
/*while (samples < 29)
{*/
samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples
//samples = 10;
//}
for(ii = 0; ii < samples ; ii++)
{ // Read the gyro data stored in the FIFO
readGyro1();
gBiasRawTemp[0] += gx;
gBiasRawTemp[1] += gy;
gBiasRawTemp[2] += gz;
readAccel1();
aBiasRawTemp[0] += ax;
aBiasRawTemp[1] += ay;
aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up!
}
for (ii = 0; ii < samples/*3*/; ii++)
{
gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
gBias[ii] = calcGyro(gBiasRaw[ii]);
aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
aBias[ii] = calcAccel(aBiasRaw[ii]);
}
enableFIFO(FALSE);
setFIFO(FIFO_OFF, 0x00);
if (autoCalc) _autoCalc = TRUE;
}
// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
// them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch
// for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
// the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to
// subtract the biases ourselves. This results in a more accurate measurement in general and can
// remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
// is good practice.
void LSM9DS1::calibrate(bool autoCalc)
{
uint8_t samples = 0;
int ii;
int32_t aBiasRawTemp[3] = {0, 0, 0};
int32_t gBiasRawTemp[3] = {0, 0, 0};
// Turn on FIFO and set threshold to 32 samples
enableFIFO(true);
setFIFO(FIFO_THS, 0x1F);
while (samples < 0x1F)
{
samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples
}
for(ii = 0; ii < samples ; ii++)
{ // Read the gyro data stored in the FIFO
readGyro();
gBiasRawTemp[0] += gx;
gBiasRawTemp[1] += gy;
gBiasRawTemp[2] += gz;
readAccel();
aBiasRawTemp[0] += ax;
aBiasRawTemp[1] += ay;
aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up!
}
for (ii = 0; ii < 3; ii++)
{
gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
gBias[ii] = calcGyro(gBiasRaw[ii]);
aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
aBias[ii] = calcAccel(aBiasRaw[ii]);
}
enableFIFO(false);
setFIFO(FIFO_OFF, 0x00);
if (autoCalc) _autoCalc = true;
}
void setGyroScale(uint16_t gScl)
{
// Read current value of CTRL_REG1_G:
uint8_t ctrl1RegValue = xgReadByte(CTRL_REG1_G);
// Mask out scale bits (3 & 4):
ctrl1RegValue &= 0xE7;
switch (gScl)
{
case 500:
ctrl1RegValue |= (0x1 << 3);
settings.gyro.scale = 500;
break;
case 2000:
ctrl1RegValue |= (0x3 << 3);
settings.gyro.scale = 2000;
break;
default: // Otherwise we'll set it to 245 dps (0x0 << 4)
settings.gyro.scale = 245;
break;
}
xgWriteByte(CTRL_REG1_G, ctrl1RegValue);
calcgRes();
}
uint8_t LSM9DS1::gyroAvailable()
{
uint8_t status = xgReadByte(STATUS_REG_1);
return ((status & (1<<1)) >> 1);
}
uint8_t LSM9DS1::tempAvailable()
{
uint8_t status = xgReadByte(STATUS_REG_1);
return ((status & (1<<2)) >> 2);
}
uint8_t accelAvailable(void)
{
uint8_t status = xgReadByte(STATUS_REG_1);
return (status & (1<<0));
}
uint8_t gyroAvailable(void)
{
uint8_t status = xgReadByte(STATUS_REG_1);
return ((status & (1<<1)) >> 1);
}
uint8_t getInactivity(void)
{
uint8_t temp = xgReadByte(STATUS_REG_0);
temp &= (0x10);
return temp;
}
uint8_t tempAvailable(void)
{
uint8_t status = xgReadByte(STATUS_REG_1);
return ((status & (1<<2)) >> 2);
}
uint8_t LSM9DS1::accelAvailable()
{
uint8_t status = xgReadByte(STATUS_REG_1);
return (status & (1<<0));
}
uint8_t LSM9DS1::getInactivity()
{
uint8_t temp = xgReadByte(STATUS_REG_0);
temp &= (0x10);
return temp;
}
