Revision 3:f5be1e31a464, committed 2017-04-30

Comitter:

jgensel3

Date:

Sun Apr 30 19:47:58 2017 +0000

Parent:

2:36abf8e18ade

Commit message:

Added soft iron calibration

Changed in this revision

--- a/LSM9DS1.cpp	Wed Feb 03 18:45:40 2016 +0000
+++ b/LSM9DS1.cpp	Sun Apr 30 19:47:58 2017 +0000
@@ -143,6 +143,7 @@
         mBiasRaw[i] = 0;
     }
     _autoCalc = false;
+    magCalibrated = false;
 }
 
 
@@ -384,11 +385,19 @@
     }
     for (j = 0; j < 3; j++) {
         mBiasRaw[j] = (magMax[j] + magMin[j]) / 2;
+        mScaleRaw[j] = (magMax[j] - magMin[j])/2;
         mBias[j] = calcMag(mBiasRaw[j]);
+        mScale[j] = calcMag(mScaleRaw[j]);
         pc.printf("%f  ",mBias[j]);
         if (loadIn)
             magOffset(j, mBiasRaw[j]);
     }
+    float avg = (mScale[0] + mScale[1] + mScale[2])/3;
+    mScale[0] = avg / mScale[0];
+    mScale[1] = avg / mScale[1];
+    mScale[2] =avg / mScale[2];
+    pc.printf("one: %f two: %f three: %f\n\r", mScale[0], mScale[1], mScale[2]);
+    magCalibrated = true;
     pc.printf("\n\rMAG calibration done\n\r");
 }
 void LSM9DS1::magOffset(uint8_t axis, int16_t offset)
@@ -536,6 +545,11 @@
     mx = mx - mBiasRaw[0];
     my = my - mBiasRaw[1];
     mz = mz - mBiasRaw[2];
+    if(magCalibrated) {
+        mx *= mScale[0];
+        my *= mScale[1];
+        mz *= mScale[2];
+    }
 }
 
 int16_t LSM9DS1::readMag(lsm9ds1_axis axis)

--- a/LSM9DS1.h	Wed Feb 03 18:45:40 2016 +0000
+++ b/LSM9DS1.h	Sun Apr 30 19:47:58 2017 +0000
@@ -58,9 +58,9 @@
     int16_t ax, ay, az; // x, y, and z axis readings of the accelerometer
     int16_t mx, my, mz; // x, y, and z axis readings of the magnetometer
     int16_t temperature; // Chip temperature
-    float gBias[3], aBias[3], mBias[3];
-    int16_t gBiasRaw[3], aBiasRaw[3], mBiasRaw[3];
-    
+    float gBias[3], aBias[3], mBias[3], mScale[3];
+    int16_t gBiasRaw[3], aBiasRaw[3], mBiasRaw[3], mScaleRaw[3];
+    bool magCalibrated;
     // LSM9DS1 -- LSM9DS1 class constructor
     // The constructor will set up a handful of private variables, and set the
     // communication mode as well.