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