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