VGR
Dependents: VITI_motor_angle_1 VITI_motor_angle_2 VITI_motor_angle_3
Diff: LSM9DS1.cpp
- Revision:
- 6:28c4b3c8b43d
- Parent:
- 4:7ffcb378cfd4
- Child:
- 7:e6e3d320eb6c
--- a/LSM9DS1.cpp Wed Jun 14 03:28:09 2017 +0000 +++ b/LSM9DS1.cpp Thu Jun 15 07:17:11 2017 +0000 @@ -38,13 +38,22 @@ uint8_t xgTest = cmd[1]; // Read the accel/gyro WHO_AM_I // Reset to the address of the mag who am i - cmd[1] = WHO_AM_I_M; + cmd[0] = WHO_AM_I_M; // Write the address we are going to read from and don't end the transaction i2c.write(mAddress, cmd, 1, true); // Read in all the 8 bits of data i2c.read(mAddress, cmd+1, 1); uint8_t mTest = cmd[1]; // Read the mag WHO_AM_I + for(int ii = 0; ii < 3; ii++) + { + gBiasRaw[ii] = 0; + aBiasRaw[ii] = 0; + gBias[ii] = 0; + aBias[ii] = 0; + autoCalib = false; + } + // Gyro initialization stuff: initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc. setGyroODR(gODR); // Set the gyro output data rate and bandwidth. @@ -175,6 +184,70 @@ i2c.write(xgAddress, cmd, 2); } +void LSM9DS1::calibration() +{ + + uint16_t samples = 0; + int32_t aBiasRawTemp[3] = {0, 0, 0}; + int32_t gBiasRawTemp[3] = {0, 0, 0}; + /* + // Turn on FIFO and set threshold to 32 samples + enableXgFIFO(true); + setXgFIFO( 1, 0x1F); + while (samples < 0x1F) + { + samples = (i2c.read(FIFO_SRC) & 0x3F); // Read number of stored samples + } + for(int ii = 0; ii < samples ; ii++) + { // Read the gyro data stored in the FIFO + readGyro(); + gBiasRawTemp[0] += gx_raw; + gBiasRawTemp[1] += gy_raw; + gBiasRawTemp[2] += gz_raw; + readAccel(); + aBiasRawTemp[0] += ax_raw; + aBiasRawTemp[1] += ay_raw; + aBiasRawTemp[2] += az_raw - (int16_t)(1./aRes); // Assumes sensor facing up! + } + for (int ii = 0; ii < 3; ii++) + { + gBias_raw[ii] = gBiasRawTemp[ii] / samples; + gBias[ii] = gBias_raw[ii] * gRes; + aBias_raw[ii] = aBiasRawTemp[ii] / samples; + aBias[ii] = aBias_raw[ii] * aRes; + } + + + + enableXgFIFO(false); + setXgFIFO(0, 0x00); + */ + while(samples < 300) + { + readGyro(); + gBiasRawTemp[0] += gx_raw; + gBiasRawTemp[1] += gy_raw; + gBiasRawTemp[2] += gz_raw; + readAccel(); + aBiasRawTemp[0] += ax_raw; + aBiasRawTemp[1] += ay_raw; + aBiasRawTemp[2] += az_raw; + wait_us(1000); + samples++; + } + + for(int ii = 0; ii < 3; ii++) + { + gBiasRaw[ii] = gBiasRawTemp[ii] / samples; + aBiasRaw[ii] = aBiasRawTemp[ii] / samples; + + gBias[ii] = gBiasRaw[ii] * gRes; + aBias[ii] = aBiasRaw[ii] * aRes; + } + + autoCalib = true; +} + void LSM9DS1::readAccel() { // The data we are going to read from the accel @@ -195,6 +268,17 @@ ax = ax_raw * aRes; ay = ay_raw * aRes; az = az_raw * aRes; + + if(autoCalib == true) + { + ax_raw -= aBiasRaw[0]; + ay_raw -= aBiasRaw[1]; + az_raw -= aBiasRaw[2]; + + ax = ax_raw * aRes; + ay = ay_raw * aRes; + az = az_raw * aRes; + } } void LSM9DS1::readMag() @@ -270,6 +354,18 @@ gx = gx_raw * gRes; gy = gy_raw * gRes; gz = gz_raw * gRes; + + if(autoCalib == true) + { + gx_raw -= gBiasRaw[0]; + gy_raw -= gBiasRaw[1]; + gz_raw -= gBiasRaw[2]; + + gx = gx_raw * gRes; + gy = gy_raw * gRes; + gz = gz_raw * gRes; + } + } void LSM9DS1::setGyroScale(gyro_scale gScl) @@ -488,4 +584,30 @@ mRes = 16.0 / 32768.0; break; } -} \ No newline at end of file +} + + +/* +void LSM9DS1::enableXgFIFO(bool enable) +{ + char cmd[2] = {CTRL_REG9, 0}; + + i2c.write(xgAddress, cmd, 1); + cmd[1] = i2c.read(CTRL_REG9); + + if (enable) cmd[1] |= (1<<1); + else cmd[1] &= ~(1<<1); + + i2c.write(xgAddress, cmd, 2); +} + +void LSM9DS1::setXgFIFO(uint8_t fifoMode, uint8_t fifoThs) +{ + // Limit threshold - 0x1F (31) is the maximum. If more than that was asked + // limit it to the maximum. + char cmd[2] = {FIFO_CTRL, 0}; + uint8_t threshold = fifoThs <= 0x1F ? fifoThs : 0x1F; + cmd[1] = ((fifoMode & 0x7) << 5) | (threshold & 0x1F); + i2c.write(xgAddress, cmd, 2); +} +*/ \ No newline at end of file