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 static void reset (struct gyro_cal* cal_data)
  14 {
  15         cal_data->count = 0;
  16
  17         cal_data->bias_x = cal_data->bias_y = cal_data->bias_z = 0;
  18
  19         cal_data->min_x  = cal_data->min_y  = cal_data->min_z  = 1.0;
  20         cal_data->max_x  = cal_data->max_y  = cal_data->max_z  =-1.0;
  21 }
  22
  23
  24 void gyro_cal_init (struct sensor_info_t* info)
  25 {
  26         info->cal_level = 0;
  27
  28         if (info->cal_data)
  29                 reset((struct gyro_cal*) info->cal_data);
  30 }
  31
  32
  33 static int gyro_collect (float x, float y, float z, struct gyro_cal* cal_data)
  34 {
  35         /* Analyze gyroscope data */
  36
  37         if (fabs(x) >= 1 || fabs(y) >= 1 || fabs(z) >= 1) {
  38
  39                 /* We're supposed to be standing still ; start over */
  40                 reset(cal_data);
  41
  42                 return 0; /* Uncalibrated */
  43         }
  44
  45         if (cal_data->count < GYRO_DS_SIZE) {
  46
  47                 if (x < cal_data->min_x)
  48                         cal_data->min_x = x;
  49
  50                 if (y < cal_data->min_y)
  51                         cal_data->min_y = y;
  52
  53                 if (z < cal_data->min_z)
  54                         cal_data->min_z = z;
  55
  56                 if (x > cal_data->max_x)
  57                         cal_data->max_x = x;
  58
  59                 if (y > cal_data->max_y)
  60                         cal_data->max_y = y;
  61
  62                 if (z > cal_data->max_z)
  63                         cal_data->max_z = z;
  64
  65                 if (fabs(cal_data->max_x - cal_data->min_x) <= GYRO_MAX_ERR &&
  66                     fabs(cal_data->max_y - cal_data->min_y) <= GYRO_MAX_ERR &&
  67                     fabs(cal_data->max_z - cal_data->min_z) <= GYRO_MAX_ERR)
  68                         cal_data->count++; /* One more conformant sample */
  69                 else
  70                         reset(cal_data); /* Out of spec sample ; start over */
  71
  72                 return 0; /* Still uncalibrated */
  73         }
  74
  75         /* We got enough stable samples to estimate gyroscope bias */
  76         cal_data->bias_x = (cal_data->max_x + cal_data->min_x) / 2;
  77         cal_data->bias_y = (cal_data->max_y + cal_data->min_y) / 2;
  78         cal_data->bias_z = (cal_data->max_z + cal_data->min_z) / 2;
  79
  80         return 1; /* Calibrated! */
  81 }
  82
  83 void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
  84 {
  85         struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
  86
  87         event->gyro.status = SENSOR_STATUS_ACCURACY_MEDIUM;
  88
  89         if (cal_data == NULL)
  90                 return;
  91
  92         /* Attempt gyroscope calibration if we have not reached this state */
  93         if (info->cal_level == 0)
  94                 info->cal_level = gyro_collect(event->data[0], event->data[1],
  95                                                event->data[2], cal_data);
  96
  97         if (info->cal_level)
  98                 event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
  99
 100         switch (event->type) {
 101                 case SENSOR_TYPE_GYROSCOPE:
 102                         /* For the gyroscope apply the bias */
 103                         event->data[0] = event->data[0] - cal_data->bias_x;
 104                         event->data[1] = event->data[1] - cal_data->bias_y;
 105                         event->data[2] = event->data[2] - cal_data->bias_z;
 106                         break;
 107
 108                 case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
 109                         /*
 110                          * For the uncalibrated gyroscope don't apply the bias,
 111                          * but tell he Android framework what we think it is.
 112                          */
 113                         event->uncalibrated_gyro.bias[0] = cal_data->bias_x;
 114                         event->uncalibrated_gyro.bias[1] = cal_data->bias_y;
 115                         event->uncalibrated_gyro.bias[2] = cal_data->bias_z;
 116                         break;
 117         }
 118 }