accel-calibration.c

   1 /*
   2  * Copyright (C) 2014-2015 Intel Corporation.
   3  */
   4
   5 #include <sys/types.h>
   6 #include <sys/stat.h>
   7 #include <fcntl.h>
   8 #include <utils/Log.h>
   9 #include <hardware/sensors.h>
  10 #include "common.h"
  11 #include "calibration.h"
  12 #include "utils.h"
  13
  14 /*
  15  * This implements a crude way of estimating the accelerometer manufacturing mounting bias. We monitor x y and z distribution around a few strategic spots that
  16  * should represent most hits when the device is stable (on earth!). We then try to derive an estimation of the accelerometer bias for each of these axes from
  17  * that data, continuously. This is a very rough method that should only be used as a last resort for now.
  18  */
  19
  20 static float bucket_center[BUCKET_COUNT] = { -9.8, 0, 9.8 };    /* The spots we are most interested in */
  21
  22 #define ACCEL_CALIB_DATA_VERSION        1                       /* Update this whenever the stored data structure changes */
  23
  24 #define ACCEL_CALIBRATION_PATH    "/data/accel.conf"            /* Location of saved calibration data */
  25
  26 #define REFRESH_INTERVAL        1000*1000*1000                  /* Recompute bias estimation every second */
  27
  28
  29 static void ascribe_sample (accel_cal_t* cal_data, int channel, float value)
  30 {
  31         /* Check if this falls within one of our ranges of interest */
  32         float range_min;
  33         float range_max;
  34         int i;
  35         int slice;
  36
  37         for (i=0; i<BUCKET_COUNT; i++) {
  38                 range_min = bucket_center[i] - BUCKET_TOLERANCE;
  39                 range_max = bucket_center[i] + BUCKET_TOLERANCE;
  40
  41                 if (value >= range_min && value <= range_max) {
  42                         /* Find suitable bucket */
  43                         slice = (int) ((value-range_min) / (range_max-range_min) * (SLICES-1));
  44
  45                         /* Increment counters */
  46                         cal_data->bucket[channel][i][slice]++;
  47                         cal_data->bucket_usage[channel][i]++;
  48                         return;
  49                 }
  50         }
  51 }
  52
  53
  54 static float estimate_bias (accel_cal_t* cal_data, int channel)
  55 {
  56         /*
  57          * The long term distribution within the bucket, for each of the buckets, should be centered (samples evenly distributed).
  58          * Try to determine the position in the bucket that separates it in two portions holding as many samples, then compute an estimated bias for that axis
  59          * (channel) based on that data.
  60          */
  61
  62         int i;
  63         uint64_t half_of_the_samples;
  64         uint64_t count;
  65         float median;
  66         float estimated_bucket_bias[BUCKET_COUNT] = {0};
  67         uint64_t bias_weight[BUCKET_COUNT];
  68         uint64_t total_weight;
  69         float range_min;
  70         float range_max;
  71         float estimated_bias;
  72         int slice;
  73
  74         for (i=0; i<BUCKET_COUNT; i++) {
  75                 half_of_the_samples = cal_data->bucket_usage[channel][i] / 2;
  76                 count = 0;
  77
  78                 for (slice = 0; slice < SLICES; slice++) {
  79                         count += cal_data->bucket[channel][i][slice];
  80
  81                         if (count >= half_of_the_samples) {
  82                                 range_min = bucket_center[i] - BUCKET_TOLERANCE;
  83                                 range_max = bucket_center[i] + BUCKET_TOLERANCE;
  84
  85                                 median = range_min + ((float) slice) / (SLICES-1) * (range_max-range_min);
  86
  87                                 estimated_bucket_bias[i] = median - bucket_center[i];
  88
  89                                 bias_weight[i] = count;
  90                                 break;
  91                         }
  92                 }
  93         }
  94
  95         /* Weight each of the estimated bucket bias values based on the number of samples collected */
  96
  97         total_weight = 0;
  98
  99         for (i=0; i<BUCKET_COUNT; i++)
 100                 total_weight += bias_weight[i];
 101
 102         if (total_weight == 0)
 103                 return 0.0;
 104
 105         estimated_bias = 0;
 106
 107         for (i=0; i<BUCKET_COUNT; i++)
 108                 if (bias_weight[i])
 109                         estimated_bias += estimated_bucket_bias[i] * (float) bias_weight[i] / (float) total_weight;
 110
 111         return estimated_bias;
 112 }
 113
 114
 115 void calibrate_accel (int s, sensors_event_t* event)
 116 {
 117         accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data;
 118         uint64_t current_ts;
 119         float x, y, z;
 120
 121         if (cal_data == NULL)
 122                 return;
 123
 124         x = event->data[0];
 125         y = event->data[1];
 126         z = event->data[2];
 127
 128         /* Analyze sample */
 129         ascribe_sample(cal_data, 0, x);
 130         ascribe_sample(cal_data, 1, y);
 131         ascribe_sample(cal_data, 2, z);
 132
 133         current_ts = get_timestamp_boot();
 134
 135         /* Estimate bias using accumulated data, from time to time*/
 136         if (current_ts >= cal_data->last_estimation_ts + REFRESH_INTERVAL) {
 137                 cal_data->last_estimation_ts = current_ts;
 138
 139                 cal_data->accel_bias_x = estimate_bias(cal_data, 0);
 140                 cal_data->accel_bias_y = estimate_bias(cal_data, 1);
 141                 cal_data->accel_bias_z = estimate_bias(cal_data, 2);
 142         }
 143
 144         ALOGV("Compensating for estimated accelerometer bias: x=%g, y=%g, z=%g\n", cal_data->accel_bias_x, cal_data->accel_bias_y, cal_data->accel_bias_z);
 145
 146         /* Apply compensation */
 147         event->data[0] = x - cal_data->accel_bias_x;
 148         event->data[1] = y - cal_data->accel_bias_y;
 149         event->data[2] = z - cal_data->accel_bias_z;
 150 }
 151
 152
 153 void accel_cal_init (int s)
 154 {
 155         int fd;
 156         int n;
 157
 158         accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data;
 159
 160         if (cal_data == NULL)
 161                 return;
 162
 163         if (cal_data->last_estimation_ts)
 164                 return; /* No need to overwrite perfectly good data at reenable time */
 165
 166         fd = open(ACCEL_CALIBRATION_PATH, O_RDONLY);
 167
 168         if (fd != -1) {
 169                 n = read(fd, cal_data, sizeof(accel_cal_t));
 170
 171                 close(fd);
 172
 173                 if (n == sizeof(accel_cal_t) &&
 174                         cal_data->version == ((ACCEL_CALIB_DATA_VERSION << 16) + sizeof(accel_cal_t)) &&
 175                         cal_data->bucket_count == BUCKET_COUNT &&
 176                         cal_data->slices == SLICES &&
 177                         cal_data->bucket_tolerance == BUCKET_TOLERANCE) {
 178                                 cal_data->last_estimation_ts = 0;
 179                                 return; /* We successfully loaded previously saved accelerometer calibration data */
 180                         }
 181         }
 182
 183         /* Fall back to initial values */
 184         memset(cal_data, 0, sizeof(accel_cal_t));
 185
 186         /* Store the parameters that are used with that data set, so we can check them against future version of the code to prevent inadvertent reuse */
 187         cal_data->version          = (ACCEL_CALIB_DATA_VERSION << 16) + sizeof(accel_cal_t);
 188         cal_data->bucket_count     = BUCKET_COUNT;
 189         cal_data->slices           = SLICES;
 190         cal_data->bucket_tolerance = BUCKET_TOLERANCE;
 191 }
 192
 193
 194 void accel_cal_store (int s)
 195 {
 196         int fd;
 197         accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data;
 198
 199         if (cal_data == NULL)
 200                 return;
 201
 202         fd = open(ACCEL_CALIBRATION_PATH, O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR);
 203
 204         if (fd != -1) {
 205                 write(fd, cal_data, sizeof(accel_cal_t));
 206                 close(fd);
 207         }
 208 }