gyro-calibration.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation.
   3  */
   4
   5 #include <stdio.h>
   6 #include <math.h>
   7 #include <sys/time.h>
   8 #include <utils/Log.h>
   9 #include <hardware/sensors.h>
  10 #include "common.h"
  11 #include "calibration.h"
  12
  13
  14 void gyro_cal_init(struct sensor_info_t* info)
  15 {
  16         info->calibrated = 0;
  17
  18         struct gyro_cal* gyro_data = (struct gyro_cal*) info->cal_data;
  19
  20         if (gyro_data == NULL)
  21                 return;
  22
  23         gyro_data->start = 0;
  24         gyro_data->count = 0;
  25         gyro_data->bias[0] = gyro_data->bias[1] = gyro_data->bias[2] = 0;
  26 }
  27
  28 /* Collect points - circular queue for the last GYRO_DS_SIZE
  29    elements. If the points are sufficiently close compute bias */
  30 static void gyro_collect(float x, float y, float z, struct sensor_info_t* info)
  31 {
  32         int offset, k;
  33         float min[3], max[3];
  34
  35         struct gyro_cal* gyro_data = (struct gyro_cal*) info->cal_data;
  36
  37         if (gyro_data == NULL)
  38                 return;
  39
  40         int pos = (gyro_data->start + gyro_data->count) % GYRO_DS_SIZE;
  41         gyro_data->sample[pos][0] = x;
  42         gyro_data->sample[pos][1] = y;
  43         gyro_data->sample[pos][2] = z;
  44
  45         if (gyro_data->count < GYRO_DS_SIZE)
  46                 gyro_data->count++;
  47         else
  48                 gyro_data->start = (gyro_data->start + 1) % GYRO_DS_SIZE;
  49
  50         for (k = 0; k < 3; k++)
  51                 min[k] = max[k] = gyro_data->sample[gyro_data->start][k];
  52
  53         /* Search for min-max values */
  54         for (offset = 1; offset < gyro_data->count; offset++) {
  55                 int pos2 = (gyro_data->start + offset) % GYRO_DS_SIZE;
  56                 for (k = 0; k < 3; k++) {
  57                         if (min[k] > gyro_data->sample[pos2][k])
  58                                 min[k] = gyro_data->sample[pos2][k];
  59                         else if (max[k] < gyro_data->sample[pos2][k])
  60                                 max[k] = gyro_data->sample[pos2][k];
  61                 }
  62         }
  63
  64         if (gyro_data->count == GYRO_DS_SIZE &&
  65                 fabs(max[0] - min[0]) < GYRO_MAX_ERR &&
  66                 fabs(max[1] - min[1]) < GYRO_MAX_ERR &&
  67                 fabs(max[2] - min[2]) < GYRO_MAX_ERR) {
  68                 info->calibrated = 1;
  69                 gyro_data->bias[0] = (max[0] + min[0]) / 2;
  70                 gyro_data->bias[1] = (max[1] + min[1]) / 2;
  71                 gyro_data->bias[2] = (max[2] + min[2]) / 2;
  72         }
  73 }
  74
  75 void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
  76 {
  77         if (!info->calibrated) {
  78                 gyro_collect(event->data[0], event->data[1], event->data[2], info);
  79         }
  80         else {
  81                 struct gyro_cal* gyro_data = (struct gyro_cal*) info->cal_data;
  82
  83                 if (gyro_data == NULL)
  84                         return;
  85
  86                 event->data[0] = event->gyro.x = event->data[0] - gyro_data->bias[0];
  87                 event->data[1] = event->gyro.y = event->data[1] - gyro_data->bias[1];
  88                 event->data[2] = event->gyro.z = event->data[2] - gyro_data->bias[2];
  89         }
  90 }