Gyro Calibration

Tries to detect steady state and computes bias based
on that detection.
Steadyness is detected by monitoring the gyro data for a fixed
number of events. If the data is extremely close it gathers the bias.
Theoretically, applying a constant rotation on all axes could also
provide very close data and that would provide a wrong bias but this is
not possible by human interaction with a mobile device (perhaps only
with an automated robotic solution).

Issue: GMIN-1185
Signed-off-by: Adriana Reus <adriana.reus@intel.com>
Change-Id: If1db69dcbda9c9c3b4434ac27ab500a199023f73

Conflicts:
	enumeration.c

diff --git a/Android.mk b/Android.mk
index 2b02b6a..46638cb 100644 (file)
--- a/Android.mk
+++ b/Android.mk
@@ -17,6 +17,7 @@ src_files := $(src_path)/entry.c \
             $(src_path)/transform.c \
             $(src_path)/compass-calibration.c \
             $(src_path)/matrix-ops.c \
+            $(src_path)/gyro-calibration.c \
 
 LOCAL_C_INCLUDES += $(LOCAL_PATH) vendor/intel/hardware/iio-sensors
 LOCAL_MODULE := sensors.$(TARGET_BOARD_PLATFORM)

diff --git a/calibration.h b/calibration.h
index dd67ed3..052998c 100644 (file)
--- a/calibration.h
+++ b/calibration.h
@@ -5,6 +5,9 @@
 #ifndef __CALIBRATION_H__
 #define __CALIBRATION_H__
 
+#include "common.h"
+
+/* compass defines */
 #define COMPASS_CALIBRATION_PATH "/data/compass.conf"
 #define DS_SIZE 48
 #define EPSILON 0.000000001
@@ -27,6 +30,10 @@
 #define RAW_DATA_SELECTED_PATH "/data/raw_compass_data_selected_%d.txt"
 #endif
 
+/* gyro defines */
+#define GYRO_MAX_ERR 0.05f
+#define GYRO_DS_SIZE 8
+
 typedef struct {
     /* hard iron offsets */
     double offset[3][1];
@@ -39,10 +46,19 @@ typedef struct {
 
 } calibration_data;
 
+struct gyro_cal {
+    float bias[3];
+    int start;
+    int count;
+    float sample[GYRO_DS_SIZE][3];
+};
+
 typedef double mat_input_t[DS_SIZE][3];
 
 void calibrate_compass (struct sensors_event_t* event, int64_t time);
 void compass_read_data (const char* config_path);
 void compass_store_data (const char* config_path);
 
+void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info);
+void gyro_cal_init(struct sensor_info_t* info);
 #endif

diff --git a/common.h b/common.h
index 670aa88..11e1264 100644 (file)
--- a/common.h
+++ b/common.h
@@ -144,6 +144,8 @@ struct sensor_info_t
 
        struct sample_ops_t ops;
 
+       int calibrated;
+       void* cal_data;
        /* Note: we may have to explicitely serialize access to some fields */
 };
 

diff --git a/control.c b/control.c
index c5d6608..b6dcac4 100644 (file)
--- a/control.c
+++ b/control.c
@@ -214,6 +214,9 @@ int adjust_counters (int s, int enabled)
                } else {
                        if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
                                compass_read_data(COMPASS_CALIBRATION_PATH);
+
+                       if (sensor_type == SENSOR_TYPE_GYROSCOPE)
+                               gyro_cal_init(&sensor_info[s]);
                }
        } else {
                if (sensor_info[s].enable_count == 0)

diff --git a/enumeration.c b/enumeration.c
index d72b023..5c2b9a9 100644 (file)
--- a/enumeration.c
+++ b/enumeration.c
@@ -3,6 +3,7 @@
  */
 
 #include <dirent.h>
+#include <stdlib.h>
 #include <utils/Log.h>
 #include <hardware/sensors.h>
 #include "enumeration.h"
@@ -11,6 +12,7 @@
 #include "transform.h"
 #include "description.h"
 #include "control.h"
+#include "calibration.h"
 
 /*
  * This table maps syfs entries in scan_elements directories to sensor types,
@@ -170,6 +172,10 @@ static void add_sensor (int dev_num, int catalog_index, int use_polling)
                strcpy(sensor_info[s].internal_name, "(null)");
        }
 
+       if (sensor_catalog[catalog_index].type == SENSOR_TYPE_GYROSCOPE) {
+               struct gyro_cal* calibration_data = calloc(1, sizeof(struct gyro_cal));
+               sensor_info[s].cal_data = calibration_data;
+       }
        /* Select one of the available sensor sample processing styles */
        select_transform(s);
 
@@ -348,6 +354,20 @@ void enumerate_sensors (void)
 
 void delete_enumeration_data (void)
 {
+
+       int i;
+       for (i = 0; i < sensor_count; i++)
+       switch (sensor_catalog[sensor_info[i].catalog_index].type) {
+               case SENSOR_TYPE_GYROSCOPE:
+                       if (sensor_info[i].cal_data != NULL) {
+                               free(sensor_info[i].cal_data);
+                               sensor_info[i].cal_data = NULL;
+                               sensor_info[i].calibrated = 0;
+                       }
+                       break;
+               default:
+                       break;
+       }
        /* Reset sensor count */
        sensor_count = 0;
 }

diff --git a/gyro-calibration.c b/gyro-calibration.c
new file mode 100644 (file)
index 0000000..e13bd59
--- /dev/null
+++ b/gyro-calibration.c
@@ -0,0 +1,90 @@
+/*
+ * Copyright (C) 2014 Intel Corporation.
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include <sys/time.h>
+#include <utils/Log.h>
+#include <hardware/sensors.h>
+#include "common.h"
+#include "calibration.h"
+
+
+void gyro_cal_init(struct sensor_info_t* info)
+{
+       info->calibrated = 0;
+
+       struct gyro_cal* gyro_data = (struct gyro_cal*) info->cal_data;
+
+       if (gyro_data == NULL)
+               return;
+
+       gyro_data->start = 0;
+       gyro_data->count = 0;
+       gyro_data->bias[0] = gyro_data->bias[1] = gyro_data->bias[2] = 0;
+}
+
+/* Collect points - circular queue for the last GYRO_DS_SIZE
+   elements. If the points are sufficiently close compute bias */
+static void gyro_collect(float x, float y, float z, struct sensor_info_t* info)
+{
+       int offset, k;
+       float min[3], max[3];
+
+       struct gyro_cal* gyro_data = (struct gyro_cal*) info->cal_data;
+
+       if (gyro_data == NULL)
+               return;
+
+       int pos = (gyro_data->start + gyro_data->count) % GYRO_DS_SIZE;
+       gyro_data->sample[pos][0] = x;
+       gyro_data->sample[pos][1] = y;
+       gyro_data->sample[pos][2] = z;
+
+       if (gyro_data->count < GYRO_DS_SIZE)
+               gyro_data->count++;
+       else
+               gyro_data->start = (gyro_data->start + 1) % GYRO_DS_SIZE;
+
+       for (k = 0; k < 3; k++)
+               min[k] = max[k] = gyro_data->sample[gyro_data->start][k];
+
+       /* Search for min-max values */
+       for (offset = 1; offset < gyro_data->count; offset++) {
+               int pos2 = (gyro_data->start + offset) % GYRO_DS_SIZE;
+               for (k = 0; k < 3; k++) {
+                       if (min[k] > gyro_data->sample[pos2][k])
+                               min[k] = gyro_data->sample[pos2][k];
+                       else if (max[k] < gyro_data->sample[pos2][k])
+                               max[k] = gyro_data->sample[pos2][k];
+               }
+       }
+
+       if (gyro_data->count == GYRO_DS_SIZE &&
+               fabs(max[0] - min[0]) < GYRO_MAX_ERR &&
+               fabs(max[1] - min[1]) < GYRO_MAX_ERR &&
+               fabs(max[2] - min[2]) < GYRO_MAX_ERR) {
+               info->calibrated = 1;
+               gyro_data->bias[0] = (max[0] + min[0]) / 2;
+               gyro_data->bias[1] = (max[1] + min[1]) / 2;
+               gyro_data->bias[2] = (max[2] + min[2]) / 2;
+       }
+}
+
+void calibrate_gyro(struct sensors_event_t* event, struct sensor_info_t* info)
+{
+       if (!info->calibrated) {
+               gyro_collect(event->data[0], event->data[1], event->data[2], info);
+       }
+       else {
+               struct gyro_cal* gyro_data = (struct gyro_cal*) info->cal_data;
+
+               if (gyro_data == NULL)
+                       return;
+
+               event->data[0] = event->gyro.x = event->data[0] - gyro_data->bias[0];
+               event->data[1] = event->gyro.y = event->data[1] - gyro_data->bias[1];
+               event->data[2] = event->gyro.z = event->data[2] - gyro_data->bias[2];
+       }
+}
\ No newline at end of file

diff --git a/transform.c b/transform.c
index d1600e9..2144291 100644 (file)
--- a/transform.c
+++ b/transform.c
@@ -206,13 +206,11 @@ static void finalize_sample_default(int s, struct sensors_event_t* data)
                        y = data->data[1];
                        z = -data->data[2];
 
-                       /* Limit drift */
-                       if (fabs(x) < 0.1 && fabs(y) < 0.1 && fabs(z) < 0.1)
-                               x = y = z = 0;
-
                        data->data[0] = x;
                        data->data[1] = y;
                        data->data[2] = z;
+
+                       calibrate_gyro(data, &sensor_info[s]);
                        break;
 
                case SENSOR_TYPE_AMBIENT_TEMPERATURE: