From 05610d964b10d99230311d226c925e87a0d031d6 Mon Sep 17 00:00:00 2001 From: Adriana Reus Date: Thu, 29 May 2014 16:29:16 +0300 Subject: [PATCH] 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 Change-Id: If1db69dcbda9c9c3b4434ac27ab500a199023f73 Conflicts: enumeration.c --- Android.mk | 1 + calibration.h | 16 ++++++++++ common.h | 2 ++ control.c | 3 ++ enumeration.c | 20 ++++++++++++ gyro-calibration.c | 90 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ transform.c | 6 ++-- 7 files changed, 134 insertions(+), 4 deletions(-) create mode 100644 gyro-calibration.c diff --git a/Android.mk b/Android.mk index 2b02b6a..46638cb 100644 --- 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 --- 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 --- 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 --- 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 --- a/enumeration.c +++ b/enumeration.c @@ -3,6 +3,7 @@ */ #include +#include #include #include #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 index 0000000..e13bd59 --- /dev/null +++ b/gyro-calibration.c @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2014 Intel Corporation. + */ + +#include +#include +#include +#include +#include +#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 --- 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: -- 2.11.0