compass-calibration.c

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