2 * Copyright (C) 2014 Intel Corporation.
9 #include <hardware/sensors.h>
11 #include "calibration.h"
13 static void reset (struct gyro_cal* cal_data)
17 cal_data->bias_x = cal_data->bias_y = cal_data->bias_z = 0;
19 cal_data->min_x = cal_data->min_y = cal_data->min_z = 1.0;
20 cal_data->max_x = cal_data->max_y = cal_data->max_z =-1.0;
24 void gyro_cal_init (struct sensor_info_t* info)
29 reset((struct gyro_cal*) info->cal_data);
33 static int gyro_collect (float x, float y, float z, struct gyro_cal* cal_data)
35 /* Analyze gyroscope data */
37 if (fabs(x) >= 1 || fabs(y) >= 1 || fabs(z) >= 1) {
39 /* We're supposed to be standing still ; start over */
42 return 0; /* Uncalibrated */
45 if (cal_data->count < GYRO_DS_SIZE) {
47 if (x < cal_data->min_x)
50 if (y < cal_data->min_y)
53 if (z < cal_data->min_z)
56 if (x > cal_data->max_x)
59 if (y > cal_data->max_y)
62 if (z > cal_data->max_z)
65 if (fabs(cal_data->max_x - cal_data->min_x) <= GYRO_MAX_ERR &&
66 fabs(cal_data->max_y - cal_data->min_y) <= GYRO_MAX_ERR &&
67 fabs(cal_data->max_z - cal_data->min_z) <= GYRO_MAX_ERR)
68 cal_data->count++; /* One more conformant sample */
70 reset(cal_data); /* Out of spec sample ; start over */
72 return 0; /* Still uncalibrated */
75 /* We got enough stable samples to estimate gyroscope bias */
76 cal_data->bias_x = (cal_data->max_x + cal_data->min_x) / 2;
77 cal_data->bias_y = (cal_data->max_y + cal_data->min_y) / 2;
78 cal_data->bias_z = (cal_data->max_z + cal_data->min_z) / 2;
80 return 1; /* Calibrated! */
83 void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
85 struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
90 /* Attempt gyroscope calibration if we have not reached this state */
91 if (info->cal_level == 0)
92 info->cal_level = gyro_collect(event->data[0], event->data[1],
93 event->data[2], cal_data);
96 event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
98 switch (event->type) {
99 case SENSOR_TYPE_GYROSCOPE:
100 /* For the gyroscope apply the bias */
101 event->data[0] = event->data[0] - cal_data->bias_x;
102 event->data[1] = event->data[1] - cal_data->bias_y;
103 event->data[2] = event->data[2] - cal_data->bias_z;
106 case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
108 * For the uncalibrated gyroscope don't apply the bias,
109 * but tell he Android framework what we think it is.
111 event->uncalibrated_gyro.bias[0] = cal_data->bias_x;
112 event->uncalibrated_gyro.bias[1] = cal_data->bias_y;
113 event->uncalibrated_gyro.bias[2] = cal_data->bias_z;