Merge remote-tracking branch 'origin/abt/topic/gmin/l-dev/mr1/sensors/master' into...

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

diff --git a/gyro-calibration.c b/gyro-calibration.c
index 48518c3..dd9570c 100644 (file)
--- a/gyro-calibration.c
+++ b/gyro-calibration.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2014 Intel Corporation.
+ * Copyright (C) 2014-2015 Intel Corporation.
  */
 
 #include <stdio.h>
@@ -15,9 +15,10 @@
 #define GYRO_MAX_ERR 0.05
 #define GYRO_DS_SIZE 100
 #define GYRO_CALIBRATION_PATH "/data/gyro.conf"
+#define GYRO_CAL_VERSION 1.0
 
 
-static void reset (struct gyro_cal_t* cal_data)
+static void reset (gyro_cal_t* cal_data)
 {
        cal_data->count = 0;
 
@@ -28,13 +29,14 @@ static void reset (struct gyro_cal_t* cal_data)
 }
 
 
-void gyro_cal_init (struct sensor_info_t* info)
+void gyro_cal_init (int s)
 {
        int ret;
-       struct gyro_cal_t* cal_data = (struct gyro_cal_t*) info->cal_data;
+       gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
        FILE* data_file;
+       float version;
 
-       info->cal_level = 0;
+       sensor[s].cal_level = 0;
 
        if (cal_data == NULL)
                return;
@@ -45,16 +47,22 @@ void gyro_cal_init (struct sensor_info_t* info)
        if (data_file == NULL)
                return;
 
-       ret = fscanf(data_file, "%f %f %f",
+       ret = fscanf(data_file, "%f %f %f %f", &version,
                        &cal_data->bias_x, &cal_data->bias_y, &cal_data->bias_z);
+
+       if (ret != 4 || version != GYRO_CAL_VERSION) {
+               reset(cal_data);
+               ALOGE("Gyro calibration - init failed!\n");
+       }
+
        fclose(data_file);
 }
 
 
-void gyro_store_data (struct sensor_info_t* info)
+void gyro_store_data (int s)
 {
        int ret;
-       struct gyro_cal_t* cal_data = (struct gyro_cal_t*) info->cal_data;
+       gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
        FILE* data_file;
 
        if (cal_data == NULL)
@@ -65,7 +73,7 @@ void gyro_store_data (struct sensor_info_t* info)
        if (data_file == NULL)
                return;
 
-       ret = fprintf(data_file, "%f %f %f",
+       ret = fprintf(data_file, "%f %f %f %f", GYRO_CAL_VERSION,
                cal_data->bias_x, cal_data->bias_y, cal_data->bias_z);
 
        if (ret < 0)
@@ -75,7 +83,7 @@ void gyro_store_data (struct sensor_info_t* info)
 }
 
 
-static int gyro_collect (float x, float y, float z, struct gyro_cal_t* cal_data)
+static int gyro_collect (float x, float y, float z, gyro_cal_t* cal_data)
 {
        /* Analyze gyroscope data */
 
@@ -126,16 +134,16 @@ static int gyro_collect (float x, float y, float z, struct gyro_cal_t* cal_data)
 }
 
 
-void calibrate_gyro (struct sensors_event_t* event, struct sensor_info_t* info)
+void calibrate_gyro (int s, sensors_event_t* event)
 {
-       struct gyro_cal_t* cal_data = (struct gyro_cal_t*) info->cal_data;
+       gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
 
        if (cal_data == NULL)
                return;
 
        /* Attempt gyroscope calibration if we have not reached this state */
-       if (info->cal_level == 0)
-               info->cal_level = gyro_collect(event->data[0], event->data[1],
+       if (sensor[s].cal_level == 0)
+               sensor[s].cal_level = gyro_collect(event->data[0], event->data[1],
                                               event->data[2], cal_data);
 
 
@@ -143,6 +151,6 @@ void calibrate_gyro (struct sensors_event_t* event, struct sensor_info_t* info)
        event->data[1] = event->data[1] - cal_data->bias_y;
        event->data[2] = event->data[2] - cal_data->bias_z;
 
-       if (info->cal_level)
+       if (sensor[s].cal_level)
               event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 }