2 * Copyright (C) 2014-2015 Intel Corporation.
9 #include <hardware/sensors.h>
11 #include "calibration.h"
15 #define GYRO_MAX_ERR 0.05
16 #define GYRO_DS_SIZE 100
17 #define GYRO_CALIBRATION_PATH "/data/gyro.conf"
18 #define GYRO_CAL_VERSION 1.0
21 static void reset (gyro_cal_t* cal_data)
25 cal_data->bias_x = cal_data->bias_y = cal_data->bias_z = 0;
27 cal_data->min_x = cal_data->min_y = cal_data->min_z = 1.0;
28 cal_data->max_x = cal_data->max_y = cal_data->max_z =-1.0;
32 void gyro_cal_init (int s)
35 gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
39 sensor[s].cal_level = 0;
46 data_file = fopen (GYRO_CALIBRATION_PATH, "r");
47 if (data_file == NULL)
50 ret = fscanf(data_file, "%f %f %f %f", &version,
51 &cal_data->bias_x, &cal_data->bias_y, &cal_data->bias_z);
53 if (ret != 4 || version != GYRO_CAL_VERSION) {
55 ALOGE("Gyro calibration - init failed!\n");
62 void gyro_store_data (int s)
65 gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
71 data_file = fopen (GYRO_CALIBRATION_PATH, "w");
73 if (data_file == NULL)
76 ret = fprintf(data_file, "%f %f %f %f", GYRO_CAL_VERSION,
77 cal_data->bias_x, cal_data->bias_y, cal_data->bias_z);
80 ALOGE ("Gyro calibration - store data failed!");
86 static int gyro_collect (float x, float y, float z, gyro_cal_t* cal_data)
88 /* Analyze gyroscope data */
90 if (fabs(x) >= 1 || fabs(y) >= 1 || fabs(z) >= 1) {
92 /* We're supposed to be standing still ; start over */
95 return 0; /* Uncalibrated */
98 if (cal_data->count < GYRO_DS_SIZE) {
100 if (x < cal_data->min_x)
103 if (y < cal_data->min_y)
106 if (z < cal_data->min_z)
109 if (x > cal_data->max_x)
112 if (y > cal_data->max_y)
115 if (z > cal_data->max_z)
118 if (fabs(cal_data->max_x - cal_data->min_x) <= GYRO_MAX_ERR &&
119 fabs(cal_data->max_y - cal_data->min_y) <= GYRO_MAX_ERR &&
120 fabs(cal_data->max_z - cal_data->min_z) <= GYRO_MAX_ERR)
121 cal_data->count++; /* One more conformant sample */
123 reset(cal_data); /* Out of spec sample ; start over */
125 return 0; /* Still uncalibrated */
128 /* We got enough stable samples to estimate gyroscope bias */
129 cal_data->bias_x = (cal_data->max_x + cal_data->min_x) / 2;
130 cal_data->bias_y = (cal_data->max_y + cal_data->min_y) / 2;
131 cal_data->bias_z = (cal_data->max_z + cal_data->min_z) / 2;
133 return 1; /* Calibrated! */
137 void calibrate_gyro (int s, sensors_event_t* event)
139 gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
141 if (cal_data == NULL)
144 /* Attempt gyroscope calibration if we have not reached this state */
145 if (sensor[s].cal_level == 0)
146 sensor[s].cal_level = gyro_collect(event->data[0], event->data[1],
147 event->data[2], cal_data);
150 event->data[0] = event->data[0] - cal_data->bias_x;
151 event->data[1] = event->data[1] - cal_data->bias_y;
152 event->data[2] = event->data[2] - cal_data->bias_z;
154 if (sensor[s].cal_level)
155 event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;