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