X-Git-Url: http://git.osdn.net/view?a=blobdiff_plain;f=gyro-calibration.c;h=d9f8183a86426811e54162727b785e3416dbbc95;hb=6efc469de2492112acc89359da86a66cb0263ec0;hp=09fa659c234e0558126efcc8c24e2adef71e5dd1;hpb=0bec00e4515a427344a10bdd51e9cf69e8ae7dae;p=android-x86%2Fhardware-intel-libsensors.git

diff --git a/gyro-calibration.c b/gyro-calibration.c
index 09fa659..d9f8183 100644
--- 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>
@@ -10,12 +10,15 @@
 #include "common.h"
 #include "calibration.h"
 
- /* gyro defines */
+
+/* Gyro defines */
 #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* cal_data)
+static void reset (gyro_cal_t* cal_data)
 {
 	cal_data->count = 0;
 
@@ -26,11 +29,12 @@ static void reset (struct gyro_cal* cal_data)
 }
 
 
-void gyro_cal_init (struct sensor_info_t* info)
+void gyro_cal_init (sensor_info_t* info)
 {
 	int ret;
-	struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
+	gyro_cal_t* cal_data = (gyro_cal_t*) info->cal_data;
 	FILE* data_file;
+	float version;
 
 	info->cal_level = 0;
 
@@ -43,15 +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 (sensor_info_t* info)
 {
 	int ret;
-	struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
+	gyro_cal_t* cal_data = (gyro_cal_t*) info->cal_data;
 	FILE* data_file;
 
 	if (cal_data == NULL)
@@ -62,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)
@@ -72,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* cal_data)
+static int gyro_collect (float x, float y, float z, gyro_cal_t* cal_data)
 {
 	/* Analyze gyroscope data */
 
@@ -122,9 +133,10 @@ static int gyro_collect (float x, float y, float z, struct gyro_cal* cal_data)
 	return 1; /* Calibrated! */
 }
 
-void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
+
+void calibrate_gyro (sensors_event_t* event, sensor_info_t* info)
 {
-	struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
+	gyro_cal_t* cal_data = (gyro_cal_t*) info->cal_data;
 
 	if (cal_data == NULL)
 		return;
@@ -134,25 +146,11 @@ void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
 		info->cal_level = gyro_collect(event->data[0], event->data[1],
 					       event->data[2], cal_data);
 
-	switch (event->type) {
-		case SENSOR_TYPE_GYROSCOPE:
-			/* For the gyroscope apply the bias */
-			event->data[0] = event->data[0] - cal_data->bias_x;
-			event->data[1] = event->data[1] - cal_data->bias_y;
-			event->data[2] = event->data[2] - cal_data->bias_z;
-
-			if (info->cal_level)
-			       event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
-			break;
-
-		case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
-			/*
-			 * For the uncalibrated gyroscope don't apply the bias,
-			 * but tell he Android framework what we think it is.
-			 */
-			event->uncalibrated_gyro.bias[0] = cal_data->bias_x;
-			event->uncalibrated_gyro.bias[1] = cal_data->bias_y;
-			event->uncalibrated_gyro.bias[2] = cal_data->bias_z;
-			break;
-	}
+
+	event->data[0] = event->data[0] - cal_data->bias_x;
+	event->data[1] = event->data[1] - cal_data->bias_y;
+	event->data[2] = event->data[2] - cal_data->bias_z;
+
+	if (info->cal_level)
+	       event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 }