uint16_t LSM9DS0_begin( LSM9DS0_t* lsm_t, LMS9DS0_Init_t* init_t )
{
#if (DEBUG0==1)
uint8_t test[8];
#endif
uint8_t gTest, xmTest;
if(init_t==NULL)
init_t = &LMS_Device_Defaults;
// Store the given scales in class variables. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
lsm_t->gScale = init_t->gScl;
lsm_t->aScale = init_t->aScl;
lsm_t->mScale = init_t->mScl;
// 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(lsm_t); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(lsm_t); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(lsm_t); // Calculate g / ADC tick, stored in aRes variable
// Now, initialize our hardware interface.
#if(LSM_I2C_SUPPORT==1)
if (lsm_t->interfaceMode == MODE_I2C) // If we're using I2C
LSM_initI2C(); // Initialize I2C
else if (lsm_t->interfaceMode == MODE_SPI) // else, if we're using SPI
#endif
//LSM_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.
gTest = gReadByte(lsm_t,WHO_AM_I_G); // Read the gyro WHO_AM_I
xmTest = xmReadByte(lsm_t,WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(lsm_t); // This will "turn on" the gyro. Setting up interrupts, etc.
setGyroODR(lsm_t,init_t->gODR); // Set the gyro output data rate and bandwidth.
setGyroScale(lsm_t,lsm_t->gScale); // Set the gyro range
// Accelerometer initialization stuff:
initAccel(lsm_t); // "Turn on" all axes of the accel. Set up interrupts, etc.
setAccelODR(lsm_t,init_t->aODR); // Set the accel data rate.
setAccelScale(lsm_t,lsm_t->aScale); // Set the accel range.
// Magnetometer initialization stuff:
initMag(lsm_t); // "Turn on" all axes of the mag. Set up interrupts, etc.
setMagODR(lsm_t,init_t->mODR); // Set the magnetometer output data rate.
setMagScale(lsm_t,lsm_t->mScale); // Set the magnetometer's range.
LSM9DS0_Update_Ctrl(lsm_t);
lsm_t->update=UPDATE_ON_SET;
#if (DEBUG0==1)
I2CreadBytes(lsm_t->gAddress,CTRL_REG1_G,test,5);
I2CreadBytes(lsm_t->xmAddress,CTRL_REG0_XM,test,7);
#endif
// Once everything is initialized, return the WHO_AM_I registers we read:
return (xmTest << 8) | gTest;
}
uint16_t begin(LSM9DS0_t* imu, gyro_scale gScl, accel_scale aScl,
mag_scale mScl, gyro_odr gODR, accel_odr aODR, mag_odr mODR)
{
// Store the given scales in imu struct. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
imu->gScale = gScl;
imu->aScale = aScl;
imu->mScale = mScl;
// 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(imu); // Calculate DPS / ADC tick, stored in gRes variable
calcaRes(imu); // Calculate g / ADC tick, stored in aRes variable
calcmRes(imu); // Calculate Gs / ADC tick, stored in mRes variable
// 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 gTest = gReadByte(imu->gyro, WHO_AM_I_G); // Read the gyro WHO_AM_I
uint8_t xmTest = xmReadByte(imu->xm, WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(imu->gyro); // This will "turn on" the gyro. Setting up interrupts, etc.
setGyroODR(imu->gyro, gODR); // Set the gyro output data rate and bandwidth.
setGyroScale(imu, imu->gScale); // Set the gyro range
}
/* ************************************************************************** */
uint16_t LSM9DS0_begin_adv( stLSM9DS0_t * stThis,
gyro_scale gScl,
accel_scale aScl,
mag_scale mScl,
gyro_odr gODR,
accel_odr aODR,
mag_odr mODR )
{
// Default to 0xFF to indicate status is not OK
uint8_t gTest = 0xFF;
uint8_t xmTest = 0xFF;
byte byDatas;
byte byAddress = 0x1D;
byte bySubAddress = 0x0F;
// Wiat for a few millis at the beginning for the chip to boot
WAIT1_Waitms( 200 );
// Store the given scales in class variables. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
stThis->gScale = gScl;
stThis->aScale = aScl;
stThis->mScale = mScl;
// 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(stThis); // Calculate DPS / ADC tick, stored in gRes variable
calcmRes(stThis); // Calculate Gs / ADC tick, stored in mRes variable
calcaRes(stThis); // Calculate g / ADC tick, stored in aRes variable
// Now, initialize our hardware interface.
if (stThis->interfaceMode == MODE_I2C) // If we're using I2C
initI2C(stThis); // Initialize I2C
else if (stThis->interfaceMode == MODE_SPI) // else, if we're using SPI
initSPI(stThis); // 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.
gTest = gReadByte( stThis, WHO_AM_I_G ); // Read the gyro WHO_AM_I
xmTest = xmReadByte( stThis, WHO_AM_I_XM ); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(stThis); // This will "turn on" the gyro. Setting up interrupts, etc.
LSM9DS0_setGyroODR(stThis, gODR); // Set the gyro output data rate and bandwidth.
LSM9DS0_setGyroScale(stThis, stThis->gScale); // Set the gyro range
// Accelerometer initialization stuff:
initAccel(stThis); // "Turn on" all axes of the accel. Set up interrupts, etc.
LSM9DS0_setAccelODR(stThis, aODR); // Set the accel data rate.
LSM9DS0_setAccelScale(stThis, stThis->aScale); // Set the accel range.
// Magnetometer initialization stuff:
initMag(stThis); // "Turn on" all axes of the mag. Set up interrupts, etc.
LSM9DS0_setMagODR(stThis, mODR); // Set the magnetometer output data rate.
LSM9DS0_setMagScale(stThis, stThis->mScale); // Set the magnetometer's range.
// Once everything is initialized, return the WHO_AM_I registers we read:
return (xmTest << 8) | gTest;
}
bool LSM9DS0::mDataOverflow()
{
const uint8_t dOverflowMask = 0b10000000;
uint8_t statusRegVal = xmReadByte(STATUS_REG_M);
if ((dOverflowMask & statusRegVal) != 0)
{
return true;
}
return false;
}
bool LSM9DS0::newMData()
{
const uint8_t dReadyMask = 0b00001000;
uint8_t statusRegVal = xmReadByte(STATUS_REG_M);
if ((dReadyMask & statusRegVal) != 0)
{
return true;
}
return false;
}
void LSM9DS0::setAccelABW(accel_abw abwRate)
{
// We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
uint8_t temp = xmReadByte(CTRL_REG2_XM);
// Then mask out the accel ABW bits:
temp &= 0xFF^(0x3 << 6);
// Then shift in our new ODR bits:
temp |= (abwRate << 6);
// And write the new register value back into CTRL_REG2_XM:
xmWriteByte(CTRL_REG2_XM, temp);
}
void setAccelODR(LSM9DS0_t* lsm_t, accel_odr aRate)
{
// We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
uint8_t temp = xmReadByte(lsm_t,CTRL_REG1_XM);
// Then mask out the accel ODR bits:
temp &= 0xFF^(0xF << 4);
// Then shift in our new ODR bits:
temp |= (aRate << 4);
// And write the new register value back into CTRL_REG1_XM:
xmWriteByte(lsm_t,CTRL_REG1_XM, temp);
}
void LSM330D::setAccelODR(accel_odr aRate)
{
// We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
uint8_t temp = xmReadByte(CTRL_REG1_A);
// Then mask out the accel ODR bits:
temp &= 0x0F;
// Then shift in our new ODR bits:
temp |= (aRate << 4);
// And write the new register value back into CTRL_REG1_XM:
xmWriteByte(CTRL_REG1_A, temp);
}
void LSM9DS0::setMagODR(mag_odr mRate)
{
// We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
uint8_t temp = xmReadByte(CTRL_REG5_XM);
// Then mask out the mag ODR bits:
temp &= 0xFF^(0x7 << 2);
// Then shift in our new ODR bits:
temp |= (mRate << 2);
// And write the new register value back into CTRL_REG5_XM:
xmWriteByte(CTRL_REG5_XM, temp);
}
/**
@brief Read accelerometer.
The return values are in g units multiplied by factor of 100.
*/
void readAccel(int32_t* ax, int32_t* ay, int32_t* az)
{
uint8_t temp[6]; // We'll read six bytes from the gyro into temp
int16_t x, y, z;
float aScale = 2.0 / 32768.0; // Scale factor for 2g. Convert from ADC raw value to g
float fx, fy, fz;
const float factor = 100.0;
temp[0] = xmReadByte(OUT_X_L_A);
temp[1] = xmReadByte(OUT_X_H_A);
temp[2] = xmReadByte(OUT_Y_L_A);
temp[3] = xmReadByte(OUT_Y_H_A);
temp[4] = xmReadByte(OUT_Z_L_A);
temp[5] = xmReadByte(OUT_Z_H_A);
x = (temp[1] << 8) | temp[0];
y = (temp[3] << 8) | temp[2];
z = (temp[5] << 8) | temp[4];
// Convert the values
fx = x * aScale * factor;
fy = y * aScale * factor;
fz = z * aScale * factor;
*ax = (int)fx;
*ay = (int)fy;
*az = (int)fz;
}
/**
@brief Read mag values
The return values are in Gs units multiplied by factor of 100.
*/
void readMag(int32_t* mx, int32_t* my, int32_t* mz)
{
uint8_t temp[6]; // We'll read six bytes from the gyro into temp
float mScale = 2.0 / 32768.0; // 2Gs/(ADC tick) based on 2-bit value
float fx, fy, fz;
const float factor = 100.0;
int16_t x, y, z;
temp[0] = xmReadByte(OUT_X_L_M);
temp[1] = xmReadByte(OUT_X_H_M);
temp[2] = xmReadByte(OUT_Y_L_M);
temp[3] = xmReadByte(OUT_Y_H_M);
temp[4] = xmReadByte(OUT_Z_L_M);
temp[5] = xmReadByte(OUT_Z_H_M);
x = (temp[1] << 8) | temp[0];
y = (temp[3] << 8) | temp[2];
z = (temp[5] << 8) | temp[4];
// Convert the values
fx = x * mScale * factor;
fy = y * mScale * factor;
fz = z * mScale * factor;
*mx = (int)fx;
*my = (int)fy;
*mz = (int)fz;
}
void LSM9DS0::setMagScale(mag_scale mScl)
{
// We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
uint8_t temp = xmReadByte(CTRL_REG6_XM);
// Then mask out the mag scale bits:
temp &= 0xFF^(0x3 << 5);
// Then shift in our new scale bits:
temp |= mScl << 5;
// And write the new register value back into CTRL_REG6_XM:
xmWriteByte(CTRL_REG6_XM, temp);
// We've updated the sensor, but we also need to update our class variables
// First update mScale:
mScale = mScl;
// Then calculate a new mRes, which relies on mScale being set correctly:
calcmRes();
}
void LSM330D::setAccelScale(accel_scale aScl)
{
// We need to preserve the other bytes in CTRL_REG4_A. So, first read it:
uint8_t temp = xmReadByte(CTRL_REG4_A);
// Then mask out the accel scale bits:
temp &= 0xCF;
// Then shift in our new scale bits:
temp |= aScl << 4;
// And write the new register value back into CTRL_REG2_XM:
xmWriteByte(CTRL_REG4_A, temp);
// We've updated the sensor, but we also need to update our class variables
// First update aScale:
aScale = aScl;
// Then calculate a new aRes, which relies on aScale being set correctly:
calcaRes();
}
/* ************************************************************************** */
void LSM9DS0_setAccelScale(stLSM9DS0_t * stThis, accel_scale aScl)
{
// We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
uint8_t temp = xmReadByte(stThis, CTRL_REG2_XM);
// Then mask out the accel scale bits:
temp &= 0xFF^(0x3 << 3);
// Then shift in our new scale bits:
temp |= aScl << 3;
// And write the new register value back into CTRL_REG2_XM:
xmWriteByte(stThis, CTRL_REG2_XM, temp);
// We've updated the sensor, but we also need to update our class variables
// First update aScale:
stThis->aScale = aScl;
// Then calculate a new aRes, which relies on aScale being set correctly:
calcaRes(stThis);
}
uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
gyro_odr gODR, accel_odr aODR, mag_odr mODR)
{
// Store the given scales in class variables. These scale variables
// are used throughout to calculate the actual g's, DPS,and Gs's.
gScale = gScl;
aScale = aScl;
mScale = mScl;
// 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
// Now, initialize our hardware interface.
if (interfaceMode == MODE_I2C) // If we're using I2C
{} //initI2C(); // Initialize I2C
else if (interfaceMode == 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 gTest = gReadByte(WHO_AM_I_G); // Read the gyro WHO_AM_I
uint8_t xmTest = xmReadByte(WHO_AM_I_XM); // Read the accel/mag WHO_AM_I
// Gyro initialization stuff:
initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
setGyroScale(gScale); // Set the gyro range
// Accelerometer initialization stuff:
initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
setAccelODR(aODR); // Set the accel data rate.
setAccelScale(aScale); // Set the accel range.
// Magnetometer initialization stuff:
initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
setMagODR(mODR); // Set the magnetometer output data rate.
setMagScale(mScale); // Set the magnetometer's range.
// Once everything is initialized, return the WHO_AM_I registers we read:
return (xmTest << 8) | gTest;
}
void LSM330D::readTemp()
{
int8_t temp; // We'll read two bytes from the temperature sensor into temp
temp = xmReadByte(OUT_TEMP_G); // Read 2 bytes, beginning at OUT_TEMP_L_M
temperature = temp; // Temperature is a 12-bit signed integer
}
void LSM330D::calLSM330D(float * gbias, float * abias)
{
uint8_t data[6] = {0, 0, 0, 0, 0, 0};
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
uint16_t samples, ii;
// First get gyro bias
byte c = gReadByte(CTRL_REG5_G);
gWriteByte(CTRL_REG5_G, c | 0x40); // Enable gyro FIFO
delay(20); // Wait for change to take effect
gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples
delay(1000); // delay 1000 milliseconds to collect FIFO samples
samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO
int16_t gyro_temp[3] = {0, 0, 0};
gReadBytes(OUT_X_L_G, &data[0], 6);
gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);
gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
gyro_bias[0] /= samples; // average the data
gyro_bias[1] /= samples;
gyro_bias[2] /= samples;
gbias[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s
gbias[1] = (float)gyro_bias[1]*gRes;
gbias[2] = (float)gyro_bias[2]*gRes;
c = gReadByte(CTRL_REG5_G);
gWriteByte(CTRL_REG5_G, c & ~0x40); // Disable gyro FIFO
delay(20);
gWriteByte(FIFO_CTRL_REG_G, 0x00); // Enable gyro bypass mode
// Now get the accelerometer biases
c = xmReadByte(CTRL_REG5_A);
xmWriteByte(CTRL_REG5_A, c | 0x40); // Enable accelerometer FIFO
delay(20); // Wait for change to take effect
xmWriteByte(FIFO_CTRL_REG, 0x40 | 0x1F); // Enable accelerometer FIFO stream mode and set watermark at 32 samples
delay(1000); // delay 1000 milliseconds to collect FIFO samples
samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
for(ii = 0; ii < samples ; ii++) { // Read the accelerometer data stored in the FIFO
int16_t accel_temp[3] = {0, 0, 0};
xmReadBytes(OUT_X_L_A, &data[0], 6);
accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]);// Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);
accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
}
accel_bias[0] /= samples; // average the data
accel_bias[1] /= samples;
accel_bias[2] /= samples;
if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) (1.0/aRes);} // Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias[2] += (int32_t) (1.0/aRes);}
abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
abias[1] = (float)accel_bias[1]*aRes;
abias[2] = (float)accel_bias[2]*aRes;
c = xmReadByte(CTRL_REG5_A);
xmWriteByte(CTRL_REG5_A, c & ~0x40); // Disable accelerometer FIFO
delay(20);
xmWriteByte(FIFO_CTRL_REG, 0x00); // Enable accelerometer bypass mode
}
// 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 calLSM9DS0(LSM9DS0_t* lsm_t, float * gbias, float * abias)
{
uint8_t data[6] = {0, 0, 0, 0, 0, 0};
int16_t
gyro_bias[3] = {0, 0, 0},
accel_bias[3] = {0, 0, 0};
int samples, ii;
// First get gyro bias
uint8_t c = gReadByte(lsm_t,CTRL_REG5_G); //read modify write
gWriteByte(lsm_t, CTRL_REG5_G, c | 0x40); // Enable gyro FIFO
delay(20); // Wait for change to take effect
gWriteByte(lsm_t, FIFO_CTRL_REG_G, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples
delay(1000); // delay 1000 milliseconds to collect FIFO samples
samples = (gReadByte(lsm_t,FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO
gReadBytes(lsm_t,OUT_X_L_G, &data[0], 6);
gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
}
gyro_bias[0] /= samples; // average the data
gyro_bias[1] /= samples;
gyro_bias[2] /= samples;
gbias[0] = (float)gyro_bias[0]*lsm_t->gRes; // Properly scale the data to get deg/s
gbias[1] = (float)gyro_bias[1]*lsm_t->gRes;
gbias[2] = (float)gyro_bias[2]*lsm_t->gRes;
c = gReadByte(lsm_t,CTRL_REG5_G);
gWriteByte(lsm_t, CTRL_REG5_G, c & ~0x40); // Disable gyro FIFO
delay(20);
gWriteByte(lsm_t, FIFO_CTRL_REG_G, 0x00); // Enable gyro bypass mode
// Now get the accelerometer biases
c = xmReadByte(lsm_t,CTRL_REG0_XM);
xmWriteByte(lsm_t,CTRL_REG0_XM, c | 0x40); // Enable accelerometer FIFO
delay(20); // Wait for change to take effect
xmWriteByte(lsm_t,FIFO_CTRL_REG, 0x20 | 0x1F); // Enable accelerometer FIFO stream mode and set watermark at 32 samples
delay(1000); // delay 1000 milliseconds to collect FIFO samples
samples = (xmReadByte(lsm_t,FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
for(ii = 0; ii < samples ; ii++) { // Read the accelerometer data stored in the FIFO
xmReadBytes(lsm_t,OUT_X_L_A, &data[0], 6);
accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1.0f/lsm_t->aRes); // Assumes sensor facing up!
}
accel_bias[0] /= samples; // average the data
accel_bias[1] /= samples;
accel_bias[2] /= samples;
abias[0] = (float)accel_bias[0]*lsm_t->aRes; // Properly scale data to get gs
abias[1] = (float)accel_bias[1]*lsm_t->aRes;
abias[2] = (float)accel_bias[2]*lsm_t->aRes;
c = xmReadByte(lsm_t,CTRL_REG0_XM);
xmWriteByte(lsm_t,CTRL_REG0_XM, c & ~0x40); // Disable accelerometer FIFO
delay(20);
xmWriteByte(lsm_t,FIFO_CTRL_REG, 0x00); // Enable accelerometer bypass mode
}