void LSM330D::initGyro()
{
/* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables
Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen
DR[1:0] - Output data rate selection
00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz
BW[1:0] - Bandwidth selection (sets cutoff frequency)
Value depends on ODR. See datasheet table 21.
PD - Power down enable (0=power down mode, 1=normal or sleep mode)
Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled) */
gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
/* CTRL_REG2_G sets up the HPF
Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0
HPM[1:0] - High pass filter mode selection
00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering,
10=normal, 11=autoreset on interrupt
HPCF[3:0] - High pass filter cutoff frequency
Value depends on data rate. See datasheet table 26.
*/
gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency
/* CTRL_REG3_G sets up interrupt and DRDY_G pins
Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY
I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable)
I1_BOOT - Boot status available on INT_G (0=disable, 1=enable)
H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low)
PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain)
I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable)
I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable)
I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable)
I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */
// Int1 enabled (pp, active low), data read on DRDY_G:
gWriteByte(CTRL_REG3_G, 0x00); //0x88
/* CTRL_REG4_G sets the scale, update mode
Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM
BDU - Block data update (0=continuous, 1=output not updated until read
BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add)
FS[1:0] - Full-scale selection
00=245dps, 01=500dps, 10=2000dps, 11=2000dps
ST[1:0] - Self-test enable
00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+)
SIM - SPI serial interface mode select
0=4 wire, 1=3 wire */
gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps
/* CTRL_REG5_G sets up the FIFO, HPF, and INT1
Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
BOOT - Reboot memory content (0=normal, 1=reboot)
FIFO_EN - FIFO enable (0=disable, 1=enable)
HPen - HPF enable (0=disable, 1=enable)
INT1_Sel[1:0] - Int 1 selection configuration
Out_Sel[1:0] - Out selection configuration */
gWriteByte(CTRL_REG5_G, 0x00);
// Temporary !!! For testing !!! Remove !!! Or make useful !!!
//configGyroInt(0x2A, 0, 0, 0, 0); // Trigger interrupt when above 0 DPS...
}
/**
@brief Init gyro.
*/
void initGyro()
{
gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency
gWriteByte(CTRL_REG3_G, 0x00); // Disable interrupts
gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps
gWriteByte(CTRL_REG5_G, 0x00); // Disable fifo mode
}
void LSM330D::setGyroODR(gyro_odr gRate)
{
// We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
uint8_t temp = gReadByte(CTRL_REG1_G);
// Then mask out the gyro ODR bits:
temp &= 0x0F;
// Then shift in our new ODR bits:
temp |= (gRate << 4);
// And write the new register value back into CTRL_REG1_G:
gWriteByte(CTRL_REG1_G, temp);
}
void setGyroODR(mraa_i2c_context gyro, gyro_odr gRate)
{
// We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
uint8_t temp = gReadByte(gyro, CTRL_REG1_G);
// Then mask out the gyro ODR bits:
temp &= 0xFF^(0xF << 4);
// Then shift in our new ODR bits:
temp |= (gRate << 4);
// And write the new register value back into CTRL_REG1_G:
gWriteByte(gyro, CTRL_REG1_G, temp);
}
void setGyroODR(LSM9DS0_t* lsm_t, gyro_odr gRate)
{
// We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
//uint8_t temp = gReadByte(lsm_t,CTRL_REG1_G);
// Then mask out the gyro ODR bits:
lsm_t->gCtrl[0] &= 0xFF^(0xF << 4);
// Then shift in our new ODR bits:
lsm_t->gCtrl[0] |= (gRate << 4);
// And write the new register value back into CTRL_REG1_G:
if (lsm_t->update==UPDATE_ON_SET)
gWriteByte(lsm_t, CTRL_REG1_G, lsm_t->gCtrl);
}
void LSM330D::setGyroScale(gyro_scale gScl)
{
// We need to preserve the other bytes in CTRL_REG4_G. So, first read it:
uint8_t temp = gReadByte(CTRL_REG4_G);
// Then mask out the gyro scale bits:
temp &= 0xCF;
// Then shift in our new scale bits:
temp |= gScl << 4;
// And write the new register value back into CTRL_REG4_G:
gWriteByte(CTRL_REG4_G, temp);
// We've updated the sensor, but we also need to update our class variables
// First update gScale:
gScale = gScl;
// Then calculate a new gRes, which relies on gScale being set correctly:
calcgRes();
}
void setGyroScale(LSM9DS0_t* lsm_t, gyro_scale gScl)
{
// We need to preserve the other bytes in CTRL_REG4_G. So, first read it:
//uint8_t temp = gReadByte(lsm_t,CTRL_REG4_G);
// Then mask out the gyro scale bits:
lsm_t->gCtrl[3] &= 0xFF^(0x3 << 4);
// Then shift in our new scale bits:
lsm_t->gCtrl[3] |= gScl << 4;
// And write the new register value back into CTRL_REG4_G:
if (lsm_t->update==UPDATE_ON_SET)
gWriteByte(lsm_t, CTRL_REG4_G, lsm_t->gCtrl+3);
// We've updated the sensor, but we also need to update our class variables
// First update gScale:
lsm_t->gScale = gScl;
// Then calculate a new gRes, which relies on gScale being set correctly:
calcgRes(lsm_t);
}
void LSM330D::configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration)
{
gWriteByte(INT1_CFG_G, int1Cfg);
gWriteByte(INT1_TSH_XH_G, (int1ThsX & 0xFF00) >> 8);
gWriteByte(INT1_TSH_XL_G, (int1ThsX & 0xFF));
gWriteByte(INT1_TSH_YH_G, (int1ThsY & 0xFF00) >> 8);
gWriteByte(INT1_TSH_YL_G, (int1ThsY & 0xFF));
gWriteByte(INT1_TSH_ZH_G, (int1ThsZ & 0xFF00) >> 8);
gWriteByte(INT1_TSH_ZL_G, (int1ThsZ & 0xFF));
if (duration)
gWriteByte(INT1_DURATION_G, 0x80 | duration);
else
gWriteByte(INT1_DURATION_G, 0x00);
}
void configGyroInt(LSM9DS0_t* lsm_t, uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration)
{
gWriteByte(lsm_t, INT1_CFG_G, int1Cfg);
gWriteByte(lsm_t, INT1_THS_XH_G, (int1ThsX & 0xFF00) >> 8);
gWriteByte(lsm_t, INT1_THS_XL_G, (int1ThsX & 0xFF));
gWriteByte(lsm_t, INT1_THS_YH_G, (int1ThsY & 0xFF00) >> 8);
gWriteByte(lsm_t, INT1_THS_YL_G, (int1ThsY & 0xFF));
gWriteByte(lsm_t, INT1_THS_ZH_G, (int1ThsZ & 0xFF00) >> 8);
gWriteByte(lsm_t, INT1_THS_ZL_G, (int1ThsZ & 0xFF));
if (duration)
gWriteByte(lsm_t, INT1_DURATION_G, 0x80 | duration);
else
gWriteByte(lsm_t, INT1_DURATION_G, 0x00);
}
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
}
void initGyro( LSM9DS0_t* lsm_t )
{
#if (DEBUG0==1)
uint8_t test[5],test1[5],i;
#endif
/* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables
Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen
DR[1:0] - Output data rate selection
00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz
BW[1:0] - Bandwidth selection (sets cutoff frequency)
Value depends on ODR. See datasheet table 21.
PD - Power down enable (0=power down mode, 1=normal or sleep mode)
Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled) */
//gWriteByte(lsm_t, CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
lsm_t->gCtrl[0]=0b01111111;
#if (DEBUG0==1)
gWriteByte(lsm_t, CTRL_REG1_G, lsm_t->gCtrl[0]);
test[0] = gReadByte(lsm_t,CTRL_REG1_G);
assert(test[0]==lsm_t->gCtrl[0]);
#endif
/* CTRL_REG2_G sets up the HPF
Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0
HPM[1:0] - High pass filter mode selection
00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering,
10=normal, 11=autoreset on interrupt
HPCF[3:0] - High pass filter cutoff frequency
Value depends on data rate. See datasheet table 26.
*/
//gWriteByte(lsm_t, CTRL_REG2_G, 0x25); // Normal mode, high cutoff frequency //0x00
lsm_t->gCtrl[1]=0b00100101;
#if (DEBUG0==1)
gWriteByte(lsm_t, CTRL_REG2_G, lsm_t->gCtrl[1]);
test[1] = gReadByte(lsm_t,CTRL_REG2_G);
assert(test[1]==lsm_t->gCtrl[1]);
#endif
/* CTRL_REG3_G sets up interrupt and DRDY_G pins
Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY
I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable)
I1_BOOT - Boot status available on INT_G (0=disable, 1=enable)
H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low)
PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain)
I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable)
I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable)
I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable)
I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */
// Int1 enabled (pp, active low), data read on DRDY_G:
//gWriteByte(lsm_t, CTRL_REG3_G, 0x00); //88
lsm_t->gCtrl[2]=0b00000000;
#if (DEBUG0==1)
gWriteByte(lsm_t, CTRL_REG3_G, lsm_t->gCtrl[2]);
test[2] = gReadByte(lsm_t,CTRL_REG3_G);
assert(test[2]==lsm_t->gCtrl[2]);
#endif
/* CTRL_REG4_G sets the scale, update mode
Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM
BDU - Block data update (0=continuous, 1=output not updated until read
BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add)
FS[1:0] - Full-scale selection
00=245dps, 01=500dps, 10=2000dps, 11=2000dps
ST[1:0] - Self-test enable
00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+)
SIM - SPI serial interface mode select
0=4 wire, 1=3 wire */
//gWriteByte(lsm_t, CTRL_REG4_G, 0x90); // Set scale to 245 dps//00
lsm_t->gCtrl[3]=0b10010000;
#if (DEBUG0==1)
gWriteByte(lsm_t, CTRL_REG4_G, lsm_t->gCtrl[3]);
test[3] = gReadByte(lsm_t,CTRL_REG4_G);
assert(test[3]==lsm_t->gCtrl[3]);
#endif
/* CTRL_REG5_G sets up the FIFO, HPF, and INT1
Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
BOOT - Reboot memory content (0=normal, 1=reboot)
FIFO_EN - FIFO enable (0=disable, 1=enable)
HPen - HPF enable (0=disable, 1=enable)
INT1_Sel[1:0] - Int 1 selection configuration
Out_Sel[1:0] - Out selection configuration */
//gWriteByte(lsm_t, CTRL_REG5_G, 0x00);
lsm_t->gCtrl[4]=0b00000000;
#if (DEBUG0==1)
gWriteByte(lsm_t, CTRL_REG5_G, lsm_t->gCtrl[4]);
test[4] = gReadByte(lsm_t,CTRL_REG5_G);
assert(test[4]==lsm_t->gCtrl[4]);
#endif
if (lsm_t->update==UPDATE_ON_CALL)
{
gWriteBytes(lsm_t,CTRL_REG1_G, lsm_t->gCtrl,5);
}
#if (DEBUG0==1)
gReadBytes(lsm_t, CTRL_REG1_G,test1,5);
for (i=0;i<5;i++)
{
assert(test[i]==test1[i]);
}
#endif
}
