Min/Max delay for Light and Temperature

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

diff --git a/compass-calibration.c b/compass-calibration.c
index 2909ccd..b4d2e49 100644 (file)
--- a/compass-calibration.c
+++ b/compass-calibration.c
@@ -21,6 +21,12 @@ static int raw_data_count = 0;
 int file_no = 0;
 #endif
 
+/* 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.2, 0.4, 0.6, 1.0 };
+static const float max_sqr_errs[CAL_STEPS] = { 10.0, 8.0, 5.0, 3.5 };
+static const unsigned int lookback_counts[CAL_STEPS] = { 3, 4, 5, 6 };
 
 /* reset calibration algorithm */
 static void reset_sample (struct compass_cal* data)
@@ -384,6 +390,9 @@ static int compass_collect (struct sensors_event_t* event, struct sensor_info_t*
 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;
+    float sqr_norm = 0;
+    float sanity_norm = 0;
+    float scale = 1;
 
     if (!info->cal_level || cal_data == NULL)
         return;
@@ -397,6 +406,21 @@ 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];
@@ -454,12 +478,33 @@ static int compass_ready (struct sensor_info_t* info)
 void calibrate_compass (struct sensors_event_t* event, struct sensor_info_t* info, int64_t current_time)
 {
     long current_time_ms = current_time / 1000000;
+    int cal_level;
+
+    /* Calibration is continuous */
     compass_collect (event, info, current_time_ms);
-    if (compass_ready(info)) {
-        compass_compute_cal (event, info);
-        event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
-    } else {
-        event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
+
+    cal_level = compass_ready(info);
+
+    switch (cal_level) {
+
+        case 0:
+            event->magnetic.status = SENSOR_STATUS_UNRELIABLE;
+            break;
+
+        case 1:
+            compass_compute_cal (event, info);
+            event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
+            break;
+
+        case 2:
+            compass_compute_cal (event, info);
+            event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM;
+            break;
+
+        default:
+            compass_compute_cal (event, info);
+            event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
+            break;
     }
 }