2 * Copyright (C) 2014 Intel Corporation.
8 #include <hardware/sensors.h>
11 #include <utils/Log.h>
12 #include "calibration.h"
13 #include "matrix-ops.h"
14 #include "description.h"
17 #define MAX_RAW_DATA_COUNT 2000
18 static FILE *raw_data = NULL;
19 static FILE *raw_data_selected = NULL;
20 static int raw_data_count = 0;
26 /* We'll have multiple calibration levels
27 * so that we can provide an estimation as fast as possible
29 static const float min_diffs[CAL_STEPS] = {0.15, 0.2, 0.4, 0.6, 1.0 };
30 static const float max_sqr_errs[CAL_STEPS] = {10.0, 10.0, 8.0, 5.0, 3.5 };
31 static const unsigned int lookback_counts[CAL_STEPS] = {2, 3, 4, 5, 6 };
33 /* reset calibration algorithm */
34 static void reset_sample (struct compass_cal* data)
37 data->sample_count = 0;
38 for (i = 0; i < DS_SIZE; i++)
40 data->sample[i][j] = 0;
43 static double calc_square_err (struct compass_cal* data)
46 double raw[3][1], result[3][1], mat_diff[3][1];
49 for (i = 0; i < DS_SIZE; i++) {
50 raw[0][0] = data->sample[i][0];
51 raw[1][0] = data->sample[i][1];
52 raw[2][0] = data->sample[i][2];
54 substract (3, 1, raw, data->offset, mat_diff);
55 multiply(3, 3, 1, data->w_invert, mat_diff, result);
57 double diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0]
58 + result[2][0] * result[2][0]) - data->bfield;
66 // Given an real symmetric 3x3 matrix A, compute the eigenvalues
67 static void compute_eigenvalues(double mat[3][3], double* eig1, double* eig2, double* eig3)
69 double p = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2];
78 double q = (mat[0][0] + mat[1][1] + mat[2][2]) / 3;
79 double temp1 = mat[0][0] - q;
80 double temp2 = mat[1][1] - q;
81 double temp3 = mat[2][2] - q;
83 p = temp1 * temp1 + temp2 * temp2 + temp3 * temp3 + 2 * p;
87 assign(3, 3, mat, mat2);
91 multiply_scalar_inplace(3, 3, mat2, 1/p);
93 double r = (mat2[0][0] * mat2[1][1] * mat2[2][2] + mat2[0][1] * mat2[1][2] * mat2[2][0]
94 + mat2[0][2] * mat2[1][0] * mat2[2][1] - mat2[0][2] * mat2[1][1] * mat2[2][0]
95 - mat2[0][0] * mat2[1][2] * mat2[2][1] - mat2[0][1] * mat2[1][0] * mat2[2][2]) / 2;
105 *eig3 = q + 2 * p * cos(phi);
106 *eig1 = q + 2 * p * cos(phi + 2 * M_PI / 3);
107 *eig2 = 3 * q - *eig1 - *eig3;
110 static void calc_evector(double mat[3][3], double eig, double vec[3][1])
114 assign(3, 3, mat, h);
125 assign(2, 2, x_tmp, x);
127 double temp1 = x[0][0] * (-h[1][0]) + x[0][1] * (-h[2][0]);
128 double temp2 = x[1][0] * (-h[1][0]) + x[1][1] * (-h[2][0]);
129 double norm = sqrt(1 + temp1 * temp1 + temp2 * temp2);
131 vec[0][0] = 1.0 / norm;
132 vec[1][0] = temp1 / norm;
133 vec[2][0] = temp2 / norm;
136 static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3][3], double* bfield)
139 double h[DS_SIZE][9];
140 double w[DS_SIZE][1];
141 double h_trans[9][DS_SIZE];
142 double p_temp1[9][9];
143 double p_temp2[9][DS_SIZE];
144 double temp1[3][3], temp[3][3];
145 double temp1_inv[3][3];
149 double a[3][3], sqrt_evals[3][3], evecs[3][3], evecs_trans[3][3];
150 double evec1[3][1], evec2[3][1], evec3[3][1];
152 for (i = 0; i < DS_SIZE; i++) {
153 w[i][0] = m[i][0] * m[i][0];
157 h[i][3] = -1 * m[i][0] * m[i][1];
158 h[i][4] = -1 * m[i][0] * m[i][2];
159 h[i][5] = -1 * m[i][1] * m[i][2];
160 h[i][6] = -1 * m[i][1] * m[i][1];
161 h[i][7] = -1 * m[i][2] * m[i][2];
164 transpose (DS_SIZE, 9, h, h_trans);
165 multiply (9, DS_SIZE, 9, h_trans, h, result);
166 invert (9, result, p_temp1);
167 multiply (9, 9, DS_SIZE, p_temp1, h_trans, p_temp2);
168 multiply (9, DS_SIZE, 1, p_temp2, w, p);
171 temp1[0][1] = p[3][0];
172 temp1[0][2] = p[4][0];
173 temp1[1][0] = p[3][0];
174 temp1[1][1] = 2 * p[6][0];
175 temp1[1][2] = p[5][0];
176 temp1[2][0] = p[4][0];
177 temp1[2][1] = p[5][0];
178 temp1[2][2] = 2 * p[7][0];
180 temp2[0][0] = p[0][0];
181 temp2[1][0] = p[1][0];
182 temp2[2][0] = p[2][0];
184 invert(3, temp1, temp1_inv);
185 multiply(3, 3, 1, temp1_inv, temp2, offset);
186 double off_x = offset[0][0];
187 double off_y = offset[1][0];
188 double off_z = offset[2][0];
191 a[0][0] = 1.0 / (p[8][0] + off_x * off_x + p[6][0] * off_y * off_y
192 + p[7][0] * off_z * off_z + p[3][0] * off_x * off_y
193 + p[4][0] * off_x * off_z + p[5][0] * off_y * off_z);
195 a[0][1] = p[3][0] * a[0][0] / 2;
196 a[0][2] = p[4][0] * a[0][0] / 2;
197 a[1][2] = p[5][0] * a[0][0] / 2;
198 a[1][1] = p[6][0] * a[0][0];
199 a[2][2] = p[7][0] * a[0][0];
204 double eig1 = 0, eig2 = 0, eig3 = 0;
205 compute_eigenvalues(a, &eig1, &eig2, &eig3);
207 sqrt_evals[0][0] = sqrt(eig1);
208 sqrt_evals[1][0] = 0;
209 sqrt_evals[2][0] = 0;
210 sqrt_evals[0][1] = 0;
211 sqrt_evals[1][1] = sqrt(eig2);
212 sqrt_evals[2][1] = 0;
213 sqrt_evals[0][2] = 0;
214 sqrt_evals[1][2] = 0;
215 sqrt_evals[2][2] = sqrt(eig3);
217 calc_evector(a, eig1, evec1);
218 calc_evector(a, eig2, evec2);
219 calc_evector(a, eig3, evec3);
221 evecs[0][0] = evec1[0][0];
222 evecs[1][0] = evec1[1][0];
223 evecs[2][0] = evec1[2][0];
224 evecs[0][1] = evec2[0][0];
225 evecs[1][1] = evec2[1][0];
226 evecs[2][1] = evec2[2][0];
227 evecs[0][2] = evec3[0][0];
228 evecs[1][2] = evec3[1][0];
229 evecs[2][2] = evec3[2][0];
231 multiply (3, 3, 3, evecs, sqrt_evals, temp1);
232 transpose(3, 3, evecs, evecs_trans);
233 multiply (3, 3, 3, temp1, evecs_trans, temp);
234 transpose (3, 3, temp, w_invert);
235 *bfield = pow(sqrt(1/eig1) * sqrt(1/eig2) * sqrt(1/eig3), 1.0/3.0);
236 multiply_scalar_inplace(3, 3, w_invert, *bfield);
241 static void compass_cal_init (FILE* data_file, struct sensor_info_t* info)
250 if (raw_data_selected) {
251 fclose(raw_data_selected);
252 raw_data_selected = NULL;
256 snprintf(path, 64, RAW_DATA_FULL_PATH, file_no);
257 raw_data = fopen(path,"w+");
258 snprintf(path, 64, RAW_DATA_SELECTED_PATH, file_no);
259 raw_data_selected = fopen(path,"w+");
264 struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
266 if (cal_data == NULL)
270 reset_sample(cal_data);
272 if (!info->cal_level && data_file != NULL) {
273 int ret = fscanf(data_file, "%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
274 &info->cal_level, &cal_data->offset[0][0], &cal_data->offset[1][0], &cal_data->offset[2][0],
275 &cal_data->w_invert[0][0], &cal_data->w_invert[0][1], &cal_data->w_invert[0][2],
276 &cal_data->w_invert[1][0], &cal_data->w_invert[1][1], &cal_data->w_invert[1][2],
277 &cal_data->w_invert[2][0], &cal_data->w_invert[2][1], &cal_data->w_invert[2][2],
280 if (ret != data_count) {
285 if (info->cal_level) {
286 ALOGV("CompassCalibration: load old data, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f",
287 cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
288 cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0],
289 cal_data->w_invert[1][1], cal_data->w_invert[1][2], cal_data->w_invert[2][0], cal_data->w_invert[2][1],
290 cal_data->w_invert[2][2], cal_data->bfield);
293 cal_data->offset[0][0] = 0;
294 cal_data->offset[1][0] = 0;
295 cal_data->offset[2][0] = 0;
297 cal_data->w_invert[0][0] = 1;
298 cal_data->w_invert[1][0] = 0;
299 cal_data->w_invert[2][0] = 0;
300 cal_data->w_invert[0][1] = 0;
301 cal_data->w_invert[1][1] = 1;
302 cal_data->w_invert[2][1] = 0;
303 cal_data->w_invert[0][2] = 0;
304 cal_data->w_invert[1][2] = 0;
305 cal_data->w_invert[2][2] = 1;
307 cal_data->bfield = 0;
312 static void compass_store_result(FILE* data_file, struct sensor_info_t* info)
314 struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
316 if (data_file == NULL || cal_data == NULL)
319 int ret = fprintf(data_file, "%d %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
320 info->cal_level, cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0],
321 cal_data->w_invert[0][0], cal_data->w_invert[0][1], cal_data->w_invert[0][2],
322 cal_data->w_invert[1][0], cal_data->w_invert[1][1], cal_data->w_invert[1][2],
323 cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2],
327 ALOGE ("compass calibration - store data failed!");
330 static int compass_collect (struct sensors_event_t* event, struct sensor_info_t* info, int64_t current_time)
332 float data[3] = {event->magnetic.x, event->magnetic.y, event->magnetic.z};
333 unsigned int index,j;
334 unsigned int lookback_count;
337 struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
339 if (cal_data == NULL)
342 /* Discard the point if not valid */
343 if (data[0] == 0 || data[1] == 0 || data[2] == 0)
347 if (raw_data && raw_data_count < MAX_RAW_DATA_COUNT) {
348 fprintf(raw_data, "%f %f %f\n", (double)data[0], (double)data[1],
353 if (raw_data && raw_data_count >= MAX_RAW_DATA_COUNT) {
359 lookback_count = lookback_counts[info->cal_level];
360 min_diff = min_diffs[info->cal_level];
362 // For the current point to be accepted, each x/y/z value must be different enough
363 // to the last several collected points
364 if (cal_data->sample_count > 0 && cal_data->sample_count < DS_SIZE) {
365 unsigned int lookback = lookback_count < cal_data->sample_count ? lookback_count :
366 cal_data->sample_count;
367 for (index = 0; index < lookback; index++){
368 for (j = 0; j < 3; j++) {
369 if (fabsf(data[j] - cal_data->sample[cal_data->sample_count-1-index][j]) < min_diff) {
370 ALOGV("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d",
371 data[0], data[1], data[2], cal_data->sample_count);
378 if (cal_data->sample_count < DS_SIZE) {
379 memcpy(cal_data->sample[cal_data->sample_count], data, sizeof(float) * 3);
380 cal_data->sample_count++;
381 ALOGV("CompassCalibration:point collected [%f,%f,%f], selected_count=%d",
382 (double)data[0], (double)data[1], (double)data[2], cal_data->sample_count);
384 if (raw_data_selected) {
385 fprintf(raw_data_selected, "%f %f %f\n", (double)data[0], (double)data[1], (double)data[2]);
392 static void scale_event (struct sensors_event_t* event)
395 float sanity_norm = 0;
398 sqr_norm = (event->magnetic.x * event->magnetic.x +
399 event->magnetic.y * event->magnetic.y +
400 event->magnetic.z * event->magnetic.z);
402 sanity_norm = (sqr_norm < MAGNETIC_LOW) ? MAGNETIC_LOW : sanity_norm;
403 sanity_norm = (sqr_norm > MAGNETIC_HIGH) ? MAGNETIC_HIGH : sanity_norm;
405 if (sanity_norm && sqr_norm) {
406 scale = sanity_norm / sqr_norm;
408 event->magnetic.x = event->magnetic.x * scale;
409 event->magnetic.y = event->magnetic.y * scale;
410 event->magnetic.z = event->magnetic.z * scale;
415 static void compass_compute_cal (struct sensors_event_t* event, struct sensor_info_t* info)
417 struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
418 double result[3][1], raw[3][1], diff[3][1];
420 if (!info->cal_level || cal_data == NULL)
423 raw[0][0] = event->magnetic.x;
424 raw[1][0] = event->magnetic.y;
425 raw[2][0] = event->magnetic.z;
427 substract(3, 1, raw, cal_data->offset, diff);
428 multiply (3, 3, 1, cal_data->w_invert, diff, result);
430 event->magnetic.x = event->data[0] = result[0][0];
431 event->magnetic.y = event->data[1] = result[1][0];
432 event->magnetic.z = event->data[2] = result[2][0];
438 static int compass_ready (struct sensor_info_t* info)
444 struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
446 if (cal_data->sample_count < DS_SIZE)
447 return info->cal_level;
449 max_sqr_err = max_sqr_errs[info->cal_level];
451 /* enough points have been collected, do the ellipsoid calibration */
452 for (i = 0; i < DS_SIZE; i++) {
453 mat[i][0] = cal_data->sample[i][0];
454 mat[i][1] = cal_data->sample[i][1];
455 mat[i][2] = cal_data->sample[i][2];
458 /* check if result is good */
459 struct compass_cal new_cal_data;
460 /* the sample data must remain the same */
461 new_cal_data = *cal_data;
462 if (ellipsoid_fit(mat, new_cal_data.offset, new_cal_data.w_invert, &new_cal_data.bfield)) {
463 double new_err = calc_square_err (&new_cal_data);
464 ALOGI("new err is %f, max sqr err id %f", new_err,max_sqr_err);
465 if (new_err < max_sqr_err) {
466 double err = calc_square_err(cal_data);
468 /* new cal data is better, so we switch to the new */
469 memcpy(cal_data->offset, new_cal_data.offset, sizeof(cal_data->offset));
470 memcpy(cal_data->w_invert, new_cal_data.w_invert, sizeof(cal_data->w_invert));
471 cal_data->bfield = new_cal_data.bfield;
472 if (info->cal_level < (CAL_STEPS - 1))
474 ALOGV("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f, err %f",
475 cal_data->offset[0][0], cal_data->offset[1][0], cal_data->offset[2][0], cal_data->w_invert[0][0],
476 cal_data->w_invert[0][1], cal_data->w_invert[0][2], cal_data->w_invert[1][0], cal_data->w_invert[1][1],
477 cal_data->w_invert[1][2], cal_data->w_invert[2][0], cal_data->w_invert[2][1], cal_data->w_invert[2][2],
478 cal_data->bfield, new_err);
482 reset_sample(cal_data);
483 return info->cal_level;
486 void calibrate_compass (struct sensors_event_t* event, struct sensor_info_t* info, int64_t current_time)
488 long current_time_ms = current_time / 1000000;
491 /* Calibration is continuous */
492 compass_collect (event, info, current_time_ms);
494 cal_level = compass_ready(info);
500 event->magnetic.status = SENSOR_STATUS_UNRELIABLE;
504 compass_compute_cal (event, info);
505 event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
509 compass_compute_cal (event, info);
510 event->magnetic.status = SENSOR_STATUS_ACCURACY_MEDIUM;
514 compass_compute_cal (event, info);
515 event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
520 void compass_read_data (struct sensor_info_t* info)
522 FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "r");
524 compass_cal_init(data_file, info);
529 void compass_store_data (struct sensor_info_t* info)
531 FILE* data_file = fopen (COMPASS_CALIBRATION_PATH, "w");
533 compass_store_result(data_file, info);