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