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  /* gyro defines */
  14 #define GYRO_MAX_ERR 0.05
  15 #define GYRO_DS_SIZE 100
  16 #define GYRO_CALIBRATION_PATH "/data/gyro.conf"
  17
  18 static void reset (struct gyro_cal* cal_data)
  19 {
  20         cal_data->count = 0;
  21
  22         cal_data->bias_x = cal_data->bias_y = cal_data->bias_z = 0;
  23
  24         cal_data->min_x  = cal_data->min_y  = cal_data->min_z  = 1.0;
  25         cal_data->max_x  = cal_data->max_y  = cal_data->max_z  =-1.0;
  26 }
  27
  28
  29 void gyro_cal_init (struct sensor_info_t* info)
  30 {
  31         int ret;
  32         struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
  33         FILE* data_file;
  34
  35         info->cal_level = 0;
  36
  37         if (cal_data == NULL)
  38                 return;
  39
  40         reset(cal_data);
  41
  42         data_file = fopen (GYRO_CALIBRATION_PATH, "r");
  43         if (data_file == NULL)
  44                 return;
  45
  46         ret = fscanf(data_file, "%f %f %f",
  47                         &cal_data->bias_x, &cal_data->bias_y, &cal_data->bias_z);
  48         fclose(data_file);
  49 }
  50
  51 void gyro_store_data (struct sensor_info_t* info)
  52 {
  53         int ret;
  54         struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
  55         FILE* data_file;
  56
  57         if (cal_data == NULL)
  58                 return;
  59
  60         data_file = fopen (GYRO_CALIBRATION_PATH, "w");
  61
  62         if (data_file == NULL)
  63                 return;
  64
  65         ret = fprintf(data_file, "%f %f %f",
  66                 cal_data->bias_x, cal_data->bias_y, cal_data->bias_z);
  67
  68         if (ret < 0)
  69                 ALOGE ("Gyro calibration - store data failed!");
  70
  71         fclose(data_file);
  72 }
  73
  74
  75 static int gyro_collect (float x, float y, float z, struct gyro_cal* cal_data)
  76 {
  77         /* Analyze gyroscope data */
  78
  79         if (fabs(x) >= 1 || fabs(y) >= 1 || fabs(z) >= 1) {
  80
  81                 /* We're supposed to be standing still ; start over */
  82                 reset(cal_data);
  83
  84                 return 0; /* Uncalibrated */
  85         }
  86
  87         if (cal_data->count < GYRO_DS_SIZE) {
  88
  89                 if (x < cal_data->min_x)
  90                         cal_data->min_x = x;
  91
  92                 if (y < cal_data->min_y)
  93                         cal_data->min_y = y;
  94
  95                 if (z < cal_data->min_z)
  96                         cal_data->min_z = z;
  97
  98                 if (x > cal_data->max_x)
  99                         cal_data->max_x = x;
 100
 101                 if (y > cal_data->max_y)
 102                         cal_data->max_y = y;
 103
 104                 if (z > cal_data->max_z)
 105                         cal_data->max_z = z;
 106
 107                 if (fabs(cal_data->max_x - cal_data->min_x) <= GYRO_MAX_ERR &&
 108                     fabs(cal_data->max_y - cal_data->min_y) <= GYRO_MAX_ERR &&
 109                     fabs(cal_data->max_z - cal_data->min_z) <= GYRO_MAX_ERR)
 110                         cal_data->count++; /* One more conformant sample */
 111                 else
 112                         reset(cal_data); /* Out of spec sample ; start over */
 113
 114                 return 0; /* Still uncalibrated */
 115         }
 116
 117         /* We got enough stable samples to estimate gyroscope bias */
 118         cal_data->bias_x = (cal_data->max_x + cal_data->min_x) / 2;
 119         cal_data->bias_y = (cal_data->max_y + cal_data->min_y) / 2;
 120         cal_data->bias_z = (cal_data->max_z + cal_data->min_z) / 2;
 121
 122         return 1; /* Calibrated! */
 123 }
 124
 125 void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
 126 {
 127         struct gyro_cal* cal_data = (struct gyro_cal*) info->cal_data;
 128
 129         if (cal_data == NULL)
 130                 return;
 131
 132         /* Attempt gyroscope calibration if we have not reached this state */
 133         if (info->cal_level == 0)
 134                 info->cal_level = gyro_collect(event->data[0], event->data[1],
 135                                                event->data[2], cal_data);
 136
 137         switch (event->type) {
 138                 case SENSOR_TYPE_GYROSCOPE:
 139                         /* For the gyroscope apply the bias */
 140                         event->data[0] = event->data[0] - cal_data->bias_x;
 141                         event->data[1] = event->data[1] - cal_data->bias_y;
 142                         event->data[2] = event->data[2] - cal_data->bias_z;
 143
 144                         if (info->cal_level)
 145                                event->gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 146                         break;
 147
 148                 case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
 149                         /*
 150                          * For the uncalibrated gyroscope don't apply the bias,
 151                          * but tell he Android framework what we think it is.
 152                          */
 153                         event->uncalibrated_gyro.bias[0] = cal_data->bias_x;
 154                         event->uncalibrated_gyro.bias[1] = cal_data->bias_y;
 155                         event->uncalibrated_gyro.bias[2] = cal_data->bias_z;
 156                         break;
 157         }
 158 }