Diff: LSM9DS1.cpp

Revision:

1:c2804ae02c80

Parent:

0:50166e25abb5

diff -r 50166e25abb5 -r c2804ae02c80 LSM9DS1.cpp
--- a/LSM9DS1.cpp	Wed Jun 21 07:11:09 2017 +0000
+++ b/LSM9DS1.cpp	Wed Jun 21 09:06:09 2017 +0000
@@ -7,7 +7,7 @@
 
 This file implements all functions of the LSM9DS1 class. Functions here range
 from higher level stuff, like reading/writing LSM9DS1 registers to low-level,
-hardware reads and writes. Both SPI and I2C handler functions can be found
+hardware reads and writes. Both SPI and I2
 towards the bottom of this file.
 
 Development environment specifics:
@@ -74,12 +74,12 @@
     settings.gyro.enableY = true;
     settings.gyro.enableZ = true;
     // gyro scale can be 245, 500, or 2000
-    settings.gyro.scale = 245;
+    settings.gyro.scale = 2000;
     // gyro sample rate: value between 1-6
     // 1 = 14.9    4 = 238
     // 2 = 59.5    5 = 476
     // 3 = 119     6 = 952
-    settings.gyro.sampleRate = 2;
+    settings.gyro.sampleRate = 3;
     // gyro cutoff frequency: value between 0-3
     // Actual value of cutoff frequency depends
     // on sample rate.
@@ -421,14 +421,14 @@
     while(samples < 200)
     {
         readGyro();
-        gBiasRaw[0] += gx;
-        gBiasRaw[1] += gy;
-        gBiasRaw[2] += gz;
+        gBiasRaw[0] += gxRaw;
+        gBiasRaw[1] += gyRaw;
+        gBiasRaw[2] += gzRaw;
         
         readAccel();
-        aBiasRaw[0] += ax;
-        aBiasRaw[1] += ay;
-        aBiasRaw[2] += az;
+        aBiasRaw[0] += axRaw;
+        aBiasRaw[1] += ayRaw;
+        aBiasRaw[2] += azRaw;
         
         wait_ms(1); // 1 ms
         samples++;
@@ -443,14 +443,14 @@
         aBias[i] = aBiasRaw[i] * aRes;
     }
 
-    // Re-calculate the bias
+    //Re-calculate the bias
     int bound_bias = 10;
     int32_t samples_axis[3] = {0, 0, 0};
     for (size_t i = 0; i < 500; ++i){
         readGyro();
-        gDeltaBiasRaw[0] = gx - gBiasRaw[0];
-        gDeltaBiasRaw[1] = gy - gBiasRaw[1];
-        gDeltaBiasRaw[2] = gz - gBiasRaw[2];
+        gDeltaBiasRaw[0] = gxRaw - gBiasRaw[0];
+        gDeltaBiasRaw[1] = gyRaw - gBiasRaw[1];
+        gDeltaBiasRaw[2] = gzRaw - gBiasRaw[2];
         
         for (size_t k = 0; k < 3; ++k){
             if (gDeltaBiasRaw[k] >= -bound_bias && gDeltaBiasRaw[k] <= bound_bias){
@@ -463,50 +463,13 @@
     }
 
     //
-    for(int ii = 0; ii < 3; ii++)
+    for(int i = 0; i < 3; i++)
     {
-        gBiasRaw[ii] = gBiasRawTemp[ii] / samples_axis[ii];
-        gBias[ii] = gBiasRaw[ii] * gRes;
+        gBiasRaw[i] = gBiasRawTemp[i] / samples_axis[i];
+        gBias[i] = gBiasRaw[i] * gRes;
     }
 
     if (autoCalc)   _autoCalc = true;
-
-    //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 < 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;
 }
 
 void LSM9DS1::calibrateMag(bool loadIn)
@@ -579,93 +542,29 @@
     return ((status & (1<<axis)) >> axis);
 }
 
-void LSM9DS1::readAccel()
-{
-    uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp   
-    xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL
-    ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
-    ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
-    az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
-    if (_autoCalc)
-    {
-        ax -= aBiasRaw[X_AXIS];
-        ay -= aBiasRaw[Y_AXIS];
-        az -= aBiasRaw[Z_AXIS];
-    }
-}
-
-int16_t LSM9DS1::readAccel(lsm9ds1_axis axis)
-{
-    uint8_t temp[2];
-    int16_t value;
-    xgReadBytes(OUT_X_L_XL + (2 * axis), temp, 2);
-    value = (temp[1] << 8) | temp[0];
-    
-    if (_autoCalc)
-        value -= aBiasRaw[axis];
-    
-    return value;
-}
-
-// =========================================================================== //
-
-void LSM9DS1::readAccelFloatVector(vector<float> &v_out)
-{
-    readAccel();
-    
-    v_out[X_AXIS] = calcAccel(ax)*G;
-    v_out[Y_AXIS] = calcAccel(ay)*G;
-    v_out[Z_AXIS] = calcAccel(az)*G;
-}
-
-float LSM9DS1::readAccelFloat(lsm9ds1_axis axis)
-{
-    return calcAccel(readAccel(axis))*G;
-}
-// =========================================================================== //
-
-
-
-
-void LSM9DS1::readMag()
-{
-    uint8_t temp[6]; // We'll read six bytes from the mag into temp 
-    mReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M
-    mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
-    my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
-    mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
-}
-
-int16_t LSM9DS1::readMag(lsm9ds1_axis axis)
-{
-    uint8_t temp[2];
-    mReadBytes(OUT_X_L_M + (2 * axis), temp, 2);
-    return (temp[1] << 8) | temp[0];
-}
-
-void LSM9DS1::readTemp()
-{
-    uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
-    xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L
-    temperature = ((int16_t)temp[1] << 8) | temp[0];
-}
-
 void LSM9DS1::readGyro()
 {
     uint8_t temp[6]; // We'll read six bytes from the gyro into temp
+
     xgReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
-    gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
-    gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
-    gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
+
+    gxRaw = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
+    gyRaw = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
+    gzRaw = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
+    
     if (_autoCalc)
     {
-        gx -= gBiasRaw[X_AXIS];
-        gy -= gBiasRaw[Y_AXIS];
-        gz -= gBiasRaw[Z_AXIS];
+        gxRaw -= gBiasRaw[X_AXIS];
+        gyRaw -= gBiasRaw[Y_AXIS];
+        gzRaw -= gBiasRaw[Z_AXIS];
     }
+
+    gx = calcGyro(gxRaw);
+    gy = calcGyro(gyRaw);
+    gz = calcGyro(gzRaw);
 }
 
-int16_t LSM9DS1::readGyro(lsm9ds1_axis axis)
+float LSM9DS1::readGyro(lsm9ds1_axis axis)
 {
     uint8_t temp[2];
     int16_t value;
@@ -676,41 +575,86 @@
     
     if (_autoCalc)
         value -= gBiasRaw[axis];
+
+    float calcValue = calcGyro(value);
     
-    return value;
+    return calcValue;
 }
 
-// =========================================================================== //
-
-void LSM9DS1::readGyroFloatVectorRad(vector<float> &v_out)
+void LSM9DS1::readAccel()
 {
-    readGyro();
-    
-    v_out[X_AXIS] = calcGyro(gx)*deg2rad;
-    v_out[Y_AXIS] = calcGyro(gy)*deg2rad;
-    v_out[Z_AXIS] = calcGyro(gz)*deg2rad;
+    uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp
+
+    xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL
+
+    axRaw = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
+    ayRaw = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
+    azRaw = (temp[5] << 8) | temp[4]; // Store z-axis values into az
+
+    if (_autoCalc)
+    {
+        axRaw -= aBiasRaw[X_AXIS];
+        ayRaw -= aBiasRaw[Y_AXIS];
+        azRaw -= aBiasRaw[Z_AXIS];
+    }
+
+    ax = calcAccel(axRaw);
+    ay = calcAccel(ayRaw);
+    az = calcAccel(azRaw);
 }
 
-void LSM9DS1::readGyroFloatVectorDeg(vector<float> &v_out)
+float LSM9DS1::readAccel(lsm9ds1_axis axis)
 {
-    readGyro();
+    uint8_t temp[2];
+    int16_t value;
+
+    xgReadBytes(OUT_X_L_XL + (2 * axis), temp, 2);
+
+    value = (temp[1] << 8) | temp[0];
     
-    v_out[X_AXIS] = calcGyro(gx);
-    v_out[Y_AXIS] = calcGyro(gy);
-    v_out[Z_AXIS] = calcGyro(gz);
+    if (_autoCalc)
+        value -= aBiasRaw[axis];
+
+    float calcValue = calcAccel(value);
+
+    return calcValue;
 }
 
-float LSM9DS1::readGyroFloatRad(lsm9ds1_axis axis)
+void LSM9DS1::readMag()
 {
-    return calcGyro(readGyro(axis))*deg2rad;    
+    uint8_t temp[6]; // We'll read six bytes from the mag into temp 
+
+    mReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M
+
+    mxRaw = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
+    myRaw = (temp[3] << 8) | temp[2]; // Store y-axis values into my
+    mzRaw = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
+
+    mx = calcMag(mxRaw);
+    my = calcMag(myRaw);
+    mz = calcMag(mzRaw);
 }
 
-float LSM9DS1::readGyroFloatDeg(lsm9ds1_axis axis)
+float LSM9DS1::readMag(lsm9ds1_axis axis)
 {
-    return calcGyro(readGyro(axis));    
+    uint8_t temp[2];
+    int16_t value;
+
+    mReadBytes(OUT_X_L_M + (2 * axis), temp, 2);
+
+    value = (temp[1] << 8) | temp[0];
+
+    float calcValue = calcMag(value);
+
+    return calcValue;
 }
 
-// =========================================================================== //
+void LSM9DS1::readTemp()
+{
+    uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
+    xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L
+    temperature = ((int16_t)temp[1] << 8) | temp[0];
+}
 
 float LSM9DS1::calcGyro(int16_t gyro)
 {