Magneto calibration tweaks

[android-x86/hardware-intel-libsensors.git] / compass-calibration.c

diff --git a/compass-calibration.c b/compass-calibration.c
index 833a3ad..2202dc3 100644 (file)
--- a/compass-calibration.c
+++ b/compass-calibration.c
@@ -21,6 +21,14 @@ static int raw_data_count = 0;
 int file_no = 0;
 #endif
 
+#define CAL_STEPS 5
+
+/* We'll have multiple calibration levels
+*  so that we can provide an estimation as fast as possible
+*/
+static const float min_diffs[CAL_STEPS] =  {0.15,  0.2, 0.4, 0.6, 1.0 };
+static const float max_sqr_errs[CAL_STEPS] = {10.0, 10.0, 8.0, 5.0, 3.5 };
+static const unsigned int lookback_counts[CAL_STEPS] = {2, 3, 4, 5, 6 };
 
 /* reset calibration algorithm */
 static void reset_sample (struct compass_cal* data)
@@ -332,7 +340,7 @@ static int compass_collect (struct sensors_event_t* event, struct sensor_info_t*
         return -1;
 
     /* Discard the point if not valid */
-    if (data[0] == 0 && data[1] == 0 && data[2] == 0)
+    if (data[0] == 0 || data[1] == 0 || data[2] == 0)
         return -1;
 
 #ifdef DBG_RAW_DATA
@@ -381,18 +389,37 @@ static int compass_collect (struct sensors_event_t* event, struct sensor_info_t*
     return 1;
 }
 
-static void compass_compute_cal (struct sensors_event_t* event, struct sensor_info_t* info)
+static void scale_event (struct sensors_event_t* event)
 {
-    struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
     float sqr_norm = 0;
     float sanity_norm = 0;
     float scale = 1;
 
-    if (!info->cal_level || cal_data == NULL)
-        return;
+    sqr_norm = (event->magnetic.x * event->magnetic.x +
+                event->magnetic.y * event->magnetic.y +
+                event->magnetic.z * event->magnetic.z);
+
+    sanity_norm = (sqr_norm < MAGNETIC_LOW) ?  MAGNETIC_LOW : sanity_norm;
+    sanity_norm = (sqr_norm > MAGNETIC_HIGH) ? MAGNETIC_HIGH : sanity_norm;
 
+    if (sanity_norm && sqr_norm) {
+        scale = sanity_norm / sqr_norm;
+        scale = sqrt(scale);
+        event->magnetic.x = event->magnetic.x * scale;
+        event->magnetic.y = event->magnetic.y * scale;
+        event->magnetic.z = event->magnetic.z * scale;
+
+    }
+}
+
+static void compass_compute_cal (struct sensors_event_t* event, struct sensor_info_t* info)
+{
+    struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
     double result[3][1], raw[3][1], diff[3][1];
 
+    if (!info->cal_level || cal_data == NULL)
+        return;
+
     raw[0][0] = event->magnetic.x;
     raw[1][0] = event->magnetic.y;
     raw[2][0] = event->magnetic.z;
@@ -400,24 +427,11 @@ static void compass_compute_cal (struct sensors_event_t* event, struct sensor_in
     substract(3, 1, raw, cal_data->offset, diff);
     multiply (3, 3, 1, cal_data->w_invert, diff, result);
 
-    sqr_norm = (result[0][0] * result[0][0] +
-                result[1][0] * result[1][0] +
-                result[2][0] * result[2][0]);
-
-    sanity_norm = (sqr_norm < MAGNETIC_LOW) ?  MAGNETIC_LOW : sanity_norm;
-    sanity_norm = (sqr_norm > MAGNETIC_HIGH) ? MAGNETIC_HIGH : sanity_norm;
-
-    if (sanity_norm) {
-        scale = sanity_norm / sqr_norm;
-        scale = sqrt(scale);
-        result[0][0] = result[0][0] * scale;
-        result[1][0] = result[1][0] * scale;
-        result[2][0] = result[2][0] * scale;
-    }
-
     event->magnetic.x = event->data[0] = result[0][0];
     event->magnetic.y = event->data[1] = result[1][0];
     event->magnetic.z = event->data[2] = result[2][0];
+
+    scale_event(event);
 }
 
 
@@ -482,6 +496,7 @@ void calibrate_compass (struct sensors_event_t* event, struct sensor_info_t* inf
     switch (cal_level) {
 
         case 0:
+            scale_event(event);
             event->magnetic.status = SENSOR_STATUS_UNRELIABLE;
             break;