gyro-calibration.c

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