compass-calibration.c

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