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