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