Diff: LSM9DS1.cpp

Revision:

3:f5be1e31a464

Parent:

2:36abf8e18ade

diff -r 36abf8e18ade -r f5be1e31a464 LSM9DS1.cpp
--- 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)