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