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