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.
17 #include <sys/types.h>
20 #include <utils/Log.h>
21 #include <hardware/sensors.h>
23 #include "calibration.h"
27 * This implements a crude way of estimating the accelerometer manufacturing mounting bias. We monitor x y and z distribution around a few strategic spots that
28 * should represent most hits when the device is stable (on earth!). We then try to derive an estimation of the accelerometer bias for each of these axes from
29 * that data, continuously. This is a very rough method that should only be used as a last resort for now.
32 static float bucket_center[BUCKET_COUNT] = { -9.8, 0, 9.8 }; /* The spots we are most interested in */
34 #define ACCEL_CALIB_DATA_VERSION 1 /* Update this whenever the stored data structure changes */
36 #define ACCEL_CALIBRATION_PATH "/data/accel.conf" /* Location of saved calibration data */
38 #define REFRESH_INTERVAL 1000*1000*1000 /* Recompute bias estimation every second */
41 static void ascribe_sample (accel_cal_t* cal_data, int channel, float value)
43 /* Check if this falls within one of our ranges of interest */
49 for (i=0; i<BUCKET_COUNT; i++) {
50 range_min = bucket_center[i] - BUCKET_TOLERANCE;
51 range_max = bucket_center[i] + BUCKET_TOLERANCE;
53 if (value >= range_min && value <= range_max) {
54 /* Find suitable bucket */
55 slice = (int) ((value-range_min) / (range_max-range_min) * (SLICES-1));
57 /* Increment counters */
58 cal_data->bucket[channel][i][slice]++;
59 cal_data->bucket_usage[channel][i]++;
66 static float estimate_bias (accel_cal_t* cal_data, int channel)
69 * The long term distribution within the bucket, for each of the buckets, should be centered (samples evenly distributed).
70 * Try to determine the position in the bucket that separates it in two portions holding as many samples, then compute an estimated bias for that axis
71 * (channel) based on that data.
75 uint64_t half_of_the_samples;
78 float estimated_bucket_bias[BUCKET_COUNT] = {0};
79 uint64_t bias_weight[BUCKET_COUNT];
80 uint64_t total_weight;
86 for (i=0; i<BUCKET_COUNT; i++) {
87 half_of_the_samples = cal_data->bucket_usage[channel][i] / 2;
90 for (slice = 0; slice < SLICES; slice++) {
91 count += cal_data->bucket[channel][i][slice];
93 if (count >= half_of_the_samples) {
94 range_min = bucket_center[i] - BUCKET_TOLERANCE;
95 range_max = bucket_center[i] + BUCKET_TOLERANCE;
97 median = range_min + ((float) slice) / (SLICES-1) * (range_max-range_min);
99 estimated_bucket_bias[i] = median - bucket_center[i];
101 bias_weight[i] = count;
107 /* Weight each of the estimated bucket bias values based on the number of samples collected */
111 for (i=0; i<BUCKET_COUNT; i++)
112 total_weight += bias_weight[i];
114 if (total_weight == 0)
119 for (i=0; i<BUCKET_COUNT; i++)
121 estimated_bias += estimated_bucket_bias[i] * (float) bias_weight[i] / (float) total_weight;
123 return estimated_bias;
127 void calibrate_accel (int s, sensors_event_t* event)
129 accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data;
133 if (cal_data == NULL)
141 ascribe_sample(cal_data, 0, x);
142 ascribe_sample(cal_data, 1, y);
143 ascribe_sample(cal_data, 2, z);
145 current_ts = get_timestamp_boot();
147 /* Estimate bias using accumulated data, from time to time*/
148 if (current_ts >= cal_data->last_estimation_ts + REFRESH_INTERVAL) {
149 cal_data->last_estimation_ts = current_ts;
151 cal_data->accel_bias_x = estimate_bias(cal_data, 0);
152 cal_data->accel_bias_y = estimate_bias(cal_data, 1);
153 cal_data->accel_bias_z = estimate_bias(cal_data, 2);
156 ALOGV("Compensating for estimated accelerometer bias: x=%g, y=%g, z=%g\n", cal_data->accel_bias_x, cal_data->accel_bias_y, cal_data->accel_bias_z);
158 /* Apply compensation */
159 event->data[0] = x - cal_data->accel_bias_x;
160 event->data[1] = y - cal_data->accel_bias_y;
161 event->data[2] = z - cal_data->accel_bias_z;
165 void accel_cal_init (int s)
170 accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data;
172 if (cal_data == NULL)
175 if (cal_data->last_estimation_ts)
176 return; /* No need to overwrite perfectly good data at reenable time */
178 fd = open(ACCEL_CALIBRATION_PATH, O_RDONLY);
181 n = read(fd, cal_data, sizeof(accel_cal_t));
185 if (n == sizeof(accel_cal_t) &&
186 cal_data->version == ((ACCEL_CALIB_DATA_VERSION << 16) + sizeof(accel_cal_t)) &&
187 cal_data->bucket_count == BUCKET_COUNT &&
188 cal_data->slices == SLICES &&
189 cal_data->bucket_tolerance == BUCKET_TOLERANCE) {
190 cal_data->last_estimation_ts = 0;
191 return; /* We successfully loaded previously saved accelerometer calibration data */
195 /* Fall back to initial values */
196 memset(cal_data, 0, sizeof(accel_cal_t));
198 /* Store the parameters that are used with that data set, so we can check them against future version of the code to prevent inadvertent reuse */
199 cal_data->version = (ACCEL_CALIB_DATA_VERSION << 16) + sizeof(accel_cal_t);
200 cal_data->bucket_count = BUCKET_COUNT;
201 cal_data->slices = SLICES;
202 cal_data->bucket_tolerance = BUCKET_TOLERANCE;
206 void accel_cal_store (int s)
209 accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data;
211 if (cal_data == NULL)
214 fd = open(ACCEL_CALIBRATION_PATH, O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR);
217 write(fd, cal_data, sizeof(accel_cal_t));