compass-calibration.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation.
   3  */
   4
   5 #include <errno.h>
   6 #include <fcntl.h>
   7 #include <math.h>
   8 #include <hardware/sensors.h>
   9 #include <sys/stat.h>
  10 #include <string.h>
  11 #include <utils/Log.h>
  12 #include "calibration.h"
  13 #include "matrix-ops.h"
  14
  15
  16 #ifdef DBG_RAW_DATA
  17 #define MAX_RAW_DATA_COUNT 2000
  18 static FILE *raw_data = NULL;
  19 static FILE *raw_data_selected = NULL;
  20 static FILE *compensation_data = NULL;
  21 static int raw_data_count = 0;
  22 int file_no = 0;
  23 #endif
  24
  25 static float select_points[DS_SIZE][3];
  26 static int select_point_count = 0;
  27
  28 static int calibrated = 0;
  29 static calibration_data cal_data;
  30
  31 /* reset calibration algorithm */
  32 static void reset_calibration ()
  33 {
  34     int i,j;
  35     select_point_count = 0;
  36     for (i = 0; i < DS_SIZE; i++)
  37         for (j=0; j < 3; j++)
  38             select_points[i][j] = 0;
  39 }
  40
  41 static double calc_square_err (calibration_data data)
  42 {
  43     double err = 0;
  44     double raw[3][1], result[3][1], mat_diff[3][1];
  45     int i;
  46
  47     for (i = 0; i < DS_SIZE; i++) {
  48         raw[0][0] = select_points[i][0];
  49         raw[1][0] = select_points[i][1];
  50         raw[2][0] = select_points[i][2];
  51
  52         substract (3, 1, raw, data.offset, mat_diff);
  53         multiply(3, 3, 1, data.w_invert, mat_diff, result);
  54
  55         double diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0]
  56              + result[2][0] * result[2][0]) - data.bfield;
  57
  58         err += diff * diff;
  59     }
  60     err /= DS_SIZE;
  61     return err;
  62 }
  63
  64 // Given an real symmetric 3x3 matrix A, compute the eigenvalues
  65 static void compute_eigenvalues(double mat[3][3], double* eig1, double* eig2, double* eig3)
  66 {
  67     double p = mat[0][1] * mat[0][1] + mat[0][2] * mat[0][2] + mat[1][2] * mat[1][2];
  68
  69     if (p < EPSILON) {
  70         *eig1 = mat[0][0];
  71         *eig2 = mat[1][1];
  72         *eig3 = mat[2][2];
  73         return;
  74     }
  75
  76     double q = (mat[0][0] + mat[1][1] + mat[2][2]) / 3;
  77     double temp1 = mat[0][0] - q;
  78     double temp2 = mat[1][1] - q;
  79     double temp3 = mat[2][2] - q;
  80
  81     p = temp1 * temp1 + temp2 * temp2 + temp3 * temp3 + 2 * p;
  82     p = sqrt(p / 6);
  83
  84     double mat2[3][3];
  85     assign(3, 3, mat, mat2);
  86     mat2[0][0] -= q;
  87     mat2[1][1] -= q;
  88     mat2[2][2] -= q;
  89     multiply_scalar_inplace(3, 3, mat2, 1/p);
  90
  91     double r = (mat2[0][0] * mat2[1][1] * mat2[2][2] + mat2[0][1] * mat2[1][2] * mat2[2][0]
  92         + mat2[0][2] * mat2[1][0] * mat2[2][1] - mat2[0][2] * mat2[1][1] * mat2[2][0]
  93         - mat2[0][0] * mat2[1][2] * mat2[2][1] - mat2[0][1] * mat2[1][0] * mat2[2][2]) / 2;
  94
  95     double phi;
  96     if (r <= -1.0)
  97         phi = M_PI/3;
  98     else if (r >= 1.0)
  99         phi = 0;
 100     else
 101         phi = acos(r) / 3;
 102
 103     *eig3 = q + 2 * p * cos(phi);
 104     *eig1 = q + 2 * p * cos(phi + 2 * M_PI / 3);
 105     *eig2 = 3 * q - *eig1 - *eig3;
 106 }
 107
 108 static void calc_evector(double mat[3][3], double eig, double vec[3][1])
 109 {
 110     double h[3][3];
 111     double x_tmp[2][2];
 112     assign(3, 3, mat, h);
 113     h[0][0] -= eig;
 114     h[1][1] -= eig;
 115     h[2][2] -= eig;
 116
 117     double x[2][2];
 118     x[0][0] = h[1][1];
 119     x[0][1] = h[1][2];
 120     x[1][0] = h[2][1];
 121     x[1][1] = h[2][2];
 122     invert(2, x, x_tmp);
 123     assign(2, 2, x_tmp, x);
 124
 125     double temp1 = x[0][0] * (-h[1][0]) + x[0][1] * (-h[2][0]);
 126     double temp2 = x[1][0] * (-h[1][0]) + x[1][1] * (-h[2][0]);
 127     double norm = sqrt(1 + temp1 * temp1 + temp2 * temp2);
 128
 129     vec[0][0] = 1.0 / norm;
 130     vec[1][0] = temp1 / norm;
 131     vec[2][0] = temp2 / norm;
 132 }
 133
 134 static int ellipsoid_fit (mat_input_t m, double offset[3][1], double w_invert[3][3], double* bfield)
 135 {
 136     int i,j;
 137     double h[DS_SIZE][9];
 138     double w[DS_SIZE][1];
 139     double h_trans[9][DS_SIZE];
 140     double p_temp1[9][9];
 141     double p_temp2[9][DS_SIZE];
 142     double temp1[3][3], temp[3][3];
 143     double temp1_inv[3][3];
 144     double temp2[3][1];
 145     double result[9][9];
 146     double p[9][1];
 147     double a[3][3], sqrt_evals[3][3], evecs[3][3], evecs_trans[3][3];
 148     double evec1[3][1], evec2[3][1], evec3[3][1];
 149
 150     for (i = 0; i < DS_SIZE; i++) {
 151         w[i][0] = m[i][0] * m[i][0];
 152         h[i][0] = m[i][0];
 153         h[i][1] = m[i][1];
 154         h[i][2] = m[i][2];
 155         h[i][3] = -1 * m[i][0] * m[i][1];
 156         h[i][4] = -1 * m[i][0] * m[i][2];
 157         h[i][5] = -1 * m[i][1] * m[i][2];
 158         h[i][6] = -1 * m[i][1] * m[i][1];
 159         h[i][7] = -1 * m[i][2] * m[i][2];
 160         h[i][8] = 1;
 161     }
 162     transpose (DS_SIZE, 9, h, h_trans);
 163     multiply (9, DS_SIZE, 9, h_trans, h, result);
 164     invert (9, result, p_temp1);
 165     multiply (9, 9, DS_SIZE, p_temp1, h_trans, p_temp2);
 166     multiply (9, DS_SIZE, 1, p_temp2, w, p);
 167
 168     temp1[0][0] = 2;
 169     temp1[0][1] = p[3][0];
 170     temp1[0][2] = p[4][0];
 171     temp1[1][0] = p[3][0];
 172     temp1[1][1] = 2 * p[6][0];
 173     temp1[1][2] = p[5][0];
 174     temp1[2][0] = p[4][0];
 175     temp1[2][1] = p[5][0];
 176     temp1[2][2] = 2 * p[7][0];
 177
 178     temp2[0][0] = p[0][0];
 179     temp2[1][0] = p[1][0];
 180     temp2[2][0] = p[2][0];
 181
 182     invert(3, temp1, temp1_inv);
 183     multiply(3, 3, 1, temp1_inv, temp2, offset);
 184     double off_x = offset[0][0];
 185     double off_y = offset[1][0];
 186     double off_z = offset[2][0];
 187
 188
 189     a[0][0] = 1.0 / (p[8][0] + off_x * off_x + p[6][0] * off_y * off_y
 190             + p[7][0] * off_z * off_z + p[3][0] * off_x * off_y
 191             + p[4][0] * off_x * off_z + p[5][0] * off_y * off_z);
 192
 193     a[0][1] = p[3][0] * a[0][0] / 2;
 194     a[0][2] = p[4][0] * a[0][0] / 2;
 195     a[1][2] = p[5][0] * a[0][0] / 2;
 196     a[1][1] = p[6][0] * a[0][0];
 197     a[2][2] = p[7][0] * a[0][0];
 198     a[2][1] = a[1][2];
 199     a[1][0] = a[0][1];
 200     a[2][0] = a[0][2];
 201
 202     double eig1 = 0, eig2 = 0, eig3 = 0;
 203     compute_eigenvalues(a, &eig1, &eig2, &eig3);
 204
 205     sqrt_evals[0][0] = sqrt(eig1);
 206     sqrt_evals[1][0] = 0;
 207     sqrt_evals[2][0] = 0;
 208     sqrt_evals[0][1] = 0;
 209     sqrt_evals[1][1] = sqrt(eig2);
 210     sqrt_evals[2][1] = 0;
 211     sqrt_evals[0][2] = 0;
 212     sqrt_evals[1][2] = 0;
 213     sqrt_evals[2][2] = sqrt(eig3);
 214
 215     calc_evector(a, eig1, evec1);
 216     calc_evector(a, eig2, evec2);
 217     calc_evector(a, eig3, evec3);
 218
 219     evecs[0][0] = evec1[0][0];
 220     evecs[1][0] = evec1[1][0];
 221     evecs[2][0] = evec1[2][0];
 222     evecs[0][1] = evec2[0][0];
 223     evecs[1][1] = evec2[1][0];
 224     evecs[2][1] = evec2[2][0];
 225     evecs[0][2] = evec3[0][0];
 226     evecs[1][2] = evec3[1][0];
 227     evecs[2][2] = evec3[2][0];
 228
 229     multiply (3, 3, 3, evecs, sqrt_evals, temp1);
 230     transpose(3, 3, evecs, evecs_trans);
 231     multiply (3, 3, 3, temp1, evecs_trans, temp);
 232     transpose (3, 3, temp, w_invert);
 233     *bfield = pow(sqrt(1/eig1) * sqrt(1/eig2) * sqrt(1/eig3), 1.0/3.0);
 234     multiply_scalar_inplace(3, 3, w_invert, *bfield);
 235
 236     return 1;
 237 }
 238
 239 static void compass_cal_init (FILE* data_file)
 240 {
 241
 242 #ifdef DBG_RAW_DATA
 243     if (raw_data) {
 244         fclose(raw_data);
 245         raw_data = NULL;
 246     }
 247
 248     if (raw_data_selected) {
 249         fclose(raw_data_selected);
 250         raw_data_selected = NULL;
 251     }
 252
 253     char path[64];
 254     snprintf(path, 64, RAW_DATA_FULL_PATH, file_no);
 255     raw_data = fopen(path,"w+");
 256     snprintf(path, 64, RAW_DATA_SELECTED_PATH, file_no);
 257     raw_data_selected = fopen(path,"w+");
 258     file_no++;
 259     raw_data_count = 0;
 260 #endif
 261
 262     int data_count = 14;
 263     reset_calibration();
 264     calibrated = 0;
 265
 266     if (data_file != NULL) {
 267        int ret = fscanf(data_file, "%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
 268             &calibrated, &cal_data.offset[0][0], &cal_data.offset[1][0], &cal_data.offset[2][0],
 269             &cal_data.w_invert[0][0], &cal_data.w_invert[0][1], &cal_data.w_invert[0][2],
 270             &cal_data.w_invert[1][0], &cal_data.w_invert[1][1], &cal_data.w_invert[1][2],
 271             &cal_data.w_invert[2][0], &cal_data.w_invert[2][1], &cal_data.w_invert[2][2],
 272             &cal_data.bfield);
 273
 274         if (ret != data_count) {
 275             calibrated = 0;
 276         }
 277     }
 278
 279     if (calibrated) {
 280         ALOGI("CompassCalibration: load old data, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f",
 281             cal_data.offset[0][0], cal_data.offset[1][0], cal_data.offset[2][0],
 282             cal_data.w_invert[0][0], cal_data.w_invert[0][1],cal_data.w_invert[0][2],cal_data.w_invert[1][0],
 283             cal_data.w_invert[1][1], cal_data.w_invert[1][2],cal_data.w_invert[2][0],cal_data.w_invert[2][1],
 284             cal_data.w_invert[2][2], cal_data.bfield);
 285
 286     } else {
 287         cal_data.offset[0][0] = 0;
 288         cal_data.offset[1][0] = 0;
 289         cal_data.offset[2][0] = 0;
 290
 291         cal_data.w_invert[0][0] = 1;
 292         cal_data.w_invert[1][0] = 0;
 293         cal_data.w_invert[2][0] = 0;
 294         cal_data.w_invert[0][1] = 0;
 295         cal_data.w_invert[1][1] = 1;
 296         cal_data.w_invert[2][1] = 0;
 297         cal_data.w_invert[0][2] = 0;
 298         cal_data.w_invert[1][2] = 0;
 299         cal_data.w_invert[2][2] = 1;
 300
 301         cal_data.bfield = 0;
 302     }
 303
 304 }
 305
 306 static void compass_store_result(FILE* data_file)
 307 {
 308     if (data_file == NULL)
 309         return;
 310     int ret = fprintf(data_file, "%d %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 311         calibrated, cal_data.offset[0][0], cal_data.offset[1][0], cal_data.offset[2][0],
 312         cal_data.w_invert[0][0], cal_data.w_invert[0][1], cal_data.w_invert[0][2],
 313         cal_data.w_invert[1][0], cal_data.w_invert[1][1], cal_data.w_invert[1][2],
 314         cal_data.w_invert[2][0], cal_data.w_invert[2][1], cal_data.w_invert[2][2],
 315         cal_data.bfield);
 316
 317     if (ret < 0)
 318         ALOGE ("compass calibration - store data failed!");
 319 }
 320
 321 static int compass_collect (struct sensors_event_t* event, int64_t current_time)
 322 {
 323     float data[3] = {event->magnetic.x, event->magnetic.y, event->magnetic.z};
 324     int index,j;
 325     int lookback_count;
 326     float min_diff;
 327
 328 #ifdef DBG_RAW_DATA
 329     if (raw_data && raw_data_count < MAX_RAW_DATA_COUNT) {
 330         fprintf(raw_data, "%f %f %f\n", (double)data[0], (double)data[1],
 331                  (double)data[2]);
 332         raw_data_count++;
 333     }
 334
 335     if (raw_data && raw_data_count >= MAX_RAW_DATA_COUNT) {
 336         fclose(raw_data);
 337         raw_data = NULL;
 338     }
 339 #endif
 340
 341     lookback_count = calibrated ? LOOKBACK_COUNT : FIRST_LOOKBACK_COUNT;
 342     min_diff = calibrated ? MIN_DIFF : FIRST_MIN_DIFF;
 343
 344     // For the current point to be accepted, each x/y/z value must be different enough
 345     // to the last several collected points
 346     if (select_point_count > 0 && select_point_count < DS_SIZE) {
 347         int lookback = lookback_count < select_point_count ? lookback_count : select_point_count;
 348         for (index = 0; index < lookback; index++){
 349             for (j = 0; j < 3; j++) {
 350                 if (fabsf(data[j] - select_points[select_point_count-1-index][j]) < min_diff) {
 351                     ALOGV("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d",
 352                        data[0], data[1], data[2], select_point_count);
 353                        return 0;
 354                 }
 355             }
 356         }
 357     }
 358
 359     if (select_point_count < DS_SIZE) {
 360         memcpy(select_points[select_point_count], data, sizeof(float) * 3);
 361         select_point_count++;
 362         ALOGV("CompassCalibration:point collected [%f,%f,%f], selected_count=%d",
 363             (double)data[0], (double)data[1], (double)data[2], select_point_count);
 364 #ifdef DBG_RAW_DATA
 365         if (raw_data_selected) {
 366             fprintf(raw_data_selected, "%f %f %f\n", (double)data[0], (double)data[1], (double)data[2]);
 367         }
 368 #endif
 369     }
 370     return 1;
 371 }
 372
 373 static void compass_compute_cal (struct sensors_event_t* event)
 374 {
 375     if (!calibrated)
 376         return;
 377
 378     double result[3][1], raw[3][1], diff[3][1];
 379
 380     raw[0][0] = event->magnetic.x;
 381     raw[1][0] = event->magnetic.y;
 382     raw[2][0] = event->magnetic.z;
 383
 384     substract(3, 1, raw, cal_data.offset, diff);
 385     multiply (3, 3, 1, cal_data.w_invert, diff, result);
 386
 387     event->magnetic.x = event->data[0] = result[0][0];
 388     event->magnetic.y = event->data[1] = result[1][0];
 389     event->magnetic.z = event->data[2] = result[2][0];
 390 }
 391
 392 static int compass_ready ()
 393 {
 394     mat_input_t mat;
 395     int i;
 396     float max_sqr_err;
 397
 398     if (select_point_count < DS_SIZE)
 399         return calibrated;
 400
 401     max_sqr_err = calibrated ? MAX_SQR_ERR : FIRST_MAX_SQR_ERR;
 402
 403     /* enough points have been collected, do the ellipsoid calibration */
 404     for (i = 0; i < DS_SIZE; i++) {
 405        mat[i][0] = select_points[i][0];
 406        mat[i][1] = select_points[i][1];
 407        mat[i][2] = select_points[i][2];
 408     }
 409
 410     /* check if result is good */
 411     calibration_data new_cal_data;
 412     if (ellipsoid_fit(mat, new_cal_data.offset, new_cal_data.w_invert, &new_cal_data.bfield)) {
 413         double new_err = calc_square_err (new_cal_data);
 414         ALOGV("new err is %f, max sqr err id %f", new_err,max_sqr_err);
 415         if (new_err < max_sqr_err) {
 416             double err = calc_square_err(cal_data);
 417             if (new_err < err) {
 418                 /* new cal data is better, so we switch to the new */
 419                 cal_data = new_cal_data;
 420                 calibrated = 1;
 421                 ALOGV("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f %f %f %f %f %f, err %f",
 422                     cal_data.offset[0][0], cal_data.offset[1][0], cal_data.offset[2][0], cal_data.w_invert[0][0],
 423                     cal_data.w_invert[0][1], cal_data.w_invert[0][2], cal_data.w_invert[1][0],cal_data.w_invert[1][1],
 424                     cal_data.w_invert[1][2], cal_data.w_invert[2][0], cal_data.w_invert[2][1], cal_data.w_invert[2][2],
 425                     cal_data.bfield, new_err);
 426             }
 427         }
 428     }
 429     reset_calibration();
 430     return calibrated;
 431 }
 432
 433 void calibrate_compass (struct sensors_event_t* event, int64_t current_time)
 434 {
 435     long current_time_ms = current_time / 1000000;
 436     compass_collect (event, current_time_ms);
 437     if (compass_ready()) {
 438         compass_compute_cal (event);
 439         event->magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
 440     } else {
 441         event->magnetic.status = SENSOR_STATUS_ACCURACY_LOW;
 442     }
 443 }
 444
 445 void compass_read_data (const char* config_file)
 446 {
 447     FILE* data_file = fopen (config_file, "r");
 448
 449     compass_cal_init(data_file);
 450     if (data_file)
 451         fclose(data_file);
 452 }
 453
 454 void compass_store_data (const char* config_file)
 455 {
 456     FILE* data_file = fopen (config_file, "w");
 457
 458     compass_store_result(data_file);
 459     if (data_file)
 460         fclose(data_file);
 461
 462 }