Improved magnetometer calibration function

Uses accelerometer to verify that the sensor has been moved through rotations around all three axes.

Author: Matthew Matz

Learn/Simple Libraries/Sensor/liblsm9ds1/Documentation lsm9ds1 Library.html

@@ -20,15 +20,19 @@
 int __pinM;
 //float __mRes;
 int __mBiasRaw[3] = {0,0,0};
-
+unsigned char __settings_mag_scale;
 
 void imu_calibrateMag()
 {
-  int i, j, mx, my, mz;
+
+  int i = 0, j, mx, my, mz;
+  char ck0 = 0, ck1 = 0, ck2 = 0, ck3 = 0, ck4 = 0, ck5 = 0, ck6 = 0, ck7 = 0, ck8 = 0;
   int magMin[3] = {0, 0, 0};
   int magMax[3] = {0, 0, 0}; // The road warrior
 
-  for (i=0; i<128; i++)
+  float ax, ay, az;
+    
+  while(i < 128 || ck0 == 0 || ck1 == 0 || ck2 == 0 || ck3 == 0 || ck4 == 0 || ck5 == 0 || ck6 == 0 || ck7 == 0 || ck8 == 0)
   {
     while (!imu_magAvailable(ALL_AXIS));
     imu_readMag(&mx, &my, &mz);
@@ -41,6 +45,20 @@ void imu_calibrateMag()
       if (magTemp[j] > magMax[j]) magMax[j] = magTemp[j];
       if (magTemp[j] < magMin[j]) magMin[j] = magTemp[j];
     }
+
+    if(abs(magMax[0] - magMin[0]) > (12000 / ((int) __settings_mag_scale))) ck6 = 1;
+    if(abs(magMax[1] - magMin[1]) > (12000 / ((int) __settings_mag_scale))) ck7 = 1;
+    if(abs(magMax[2] - magMin[2]) > (12000 / ((int) __settings_mag_scale))) ck8 = 1;
+
+    imu_readAccelCalculated(&ax, &ay, &az);
+    if(ax > 0.85 && ay < 0.15 && ay > -0.15 && az < 0.15 && az > -0.15) ck0 = 1;
+    if(ax < -0.85 && ay < 0.15 && ay > -0.15 && az < 0.15 && az > -0.15) ck1 = 1;
+    if(ay > 0.85 && ax < 0.15 && ax > -0.15 && az < 0.15 && az > -0.15) ck2 = 1;
+    if(ay < -0.85 && ax < 0.15 && ax > -0.15 && az < 0.15 && az > -0.15) ck3 = 1;
+    if(az > 0.85 && ay < 0.15 && ay > -0.15 && ax < 0.15 && ax > -0.15) ck4 = 1;
+    if(az < -0.85 && ay < 0.15 && ay > -0.15 && ax < 0.15 && ax > -0.15) ck5 = 1;
+    
+    i++;
   }
 
   for (j = 0; j < 3; j++)
@@ -55,7 +73,7 @@ void imu_calibrateMag()
     lsb = __mBiasRaw[k] & 0x00FF;
     imu_SPIwriteByte(__pinM, OFFSET_X_REG_L_M + (2 * k), lsb);
     imu_SPIwriteByte(__pinM, OFFSET_X_REG_H_M + (2 * k), msb);
-  }    
+  }     
 }
 
 

Learn/Simple Libraries/Sensor/liblsm9ds1/imu_calibrateMag.c

@@ -165,9 +165,12 @@ void imu_calibrateAG();
  * @brief Calibrates the Magnetometer on the LSM9DS1 IMU module.
  *
  * @details This is a function accumulates samples from the magnetometer, averages
- * them, and then passes the biases back to the LSM9DS1
- * for subtraction from all subsequent data.  Results in a more accurate measurement in general and can
- * remove errors due to imprecise or varying initial placement.
+ * them, and then passes the biases back to the LSM9DS1 for subtraction from
+ * all subsequent data.  Results in a more accurate measurement in general and can
+ * remove errors due to imprecise or varying initial placement.  The calibration 
+ * function uses the accelerometer to verify that each of the 3 axes has been pointed 
+ * straight up and straight down slowly enough to get good magnetometer readings in 
+ * each dimension.
  *
  * @note Mag Calibration values ARE stored in the LSM9DS1 and variables that can be retireved and set using other
  * functions in this library.