2 // Copyright (c) 2015 Intel Corporation
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
8 // http://www.apache.org/licenses/LICENSE-2.0
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
20 #include <utils/Log.h>
21 #include <hardware/sensors.h>
23 #include "calibration.h"
27 #define GYRO_MAX_ERR 0.05
28 #define GYRO_DS_SIZE 100
29 #define GYRO_CALIBRATION_PATH "/data/gyro.conf"
30 #define GYRO_CAL_VERSION 1.0
33 static void reset (gyro_cal_t* cal_data)
37 cal_data->bias_x = cal_data->bias_y = cal_data->bias_z = 0;
39 cal_data->min_x = cal_data->min_y = cal_data->min_z = 1.0;
40 cal_data->max_x = cal_data->max_y = cal_data->max_z =-1.0;
44 void gyro_cal_init (int s)
47 gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
51 sensor[s].cal_level = 0;
58 data_file = fopen (GYRO_CALIBRATION_PATH, "r");
59 if (data_file == NULL)
62 ret = fscanf(data_file, "%f %f %f %f", &version,
63 &cal_data->bias_x, &cal_data->bias_y, &cal_data->bias_z);
65 if (ret != 4 || version != GYRO_CAL_VERSION) {
67 ALOGE("Gyro calibration - init failed!\n");
74 void gyro_store_data (int s)
77 gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
83 data_file = fopen (GYRO_CALIBRATION_PATH, "w");
85 if (data_file == NULL)
88 ret = fprintf(data_file, "%f %f %f %f", GYRO_CAL_VERSION,
89 cal_data->bias_x, cal_data->bias_y, cal_data->bias_z);
92 ALOGE ("Gyro calibration - store data failed!");
98 static int gyro_collect (float x, float y, float z, gyro_cal_t* cal_data)
100 /* Analyze gyroscope data */
102 if (fabs(x) >= 1 || fabs(y) >= 1 || fabs(z) >= 1) {
104 /* We're supposed to be standing still ; start over */
107 return 0; /* Uncalibrated */
110 if (cal_data->count < GYRO_DS_SIZE) {
112 if (x < cal_data->min_x)
115 if (y < cal_data->min_y)
118 if (z < cal_data->min_z)
121 if (x > cal_data->max_x)
124 if (y > cal_data->max_y)
127 if (z > cal_data->max_z)
130 if (fabs(cal_data->max_x - cal_data->min_x) <= GYRO_MAX_ERR &&
131 fabs(cal_data->max_y - cal_data->min_y) <= GYRO_MAX_ERR &&
132 fabs(cal_data->max_z - cal_data->min_z) <= GYRO_MAX_ERR)
133 cal_data->count++; /* One more conformant sample */
135 reset(cal_data); /* Out of spec sample ; start over */
137 return 0; /* Still uncalibrated */
140 /* We got enough stable samples to estimate gyroscope bias */
141 cal_data->bias_x = (cal_data->max_x + cal_data->min_x) / 2;
142 cal_data->bias_y = (cal_data->max_y + cal_data->min_y) / 2;
143 cal_data->bias_z = (cal_data->max_z + cal_data->min_z) / 2;
145 return 1; /* Calibrated! */
149 void calibrate_gyro (int s, sensors_event_t* event)
151 gyro_cal_t* cal_data = (gyro_cal_t*) sensor[s].cal_data;
153 if (cal_data == NULL)
156 /* Attempt gyroscope calibration if we have not reached this state */
157 if (sensor[s].cal_level == 0)
158 sensor[s].cal_level = gyro_collect(event->data[0], event->data[1],
159 event->data[2], cal_data);
162 event->data[0] = event->data[0] - cal_data->bias_x;
163 event->data[1] = event->data[1] - cal_data->bias_y;
164 event->data[2] = event->data[2] - cal_data->bias_z;
166 if (sensor[s].cal_level)
167 event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;