From 44c2da6de1f402139998151efb13cdcc9327075f Mon Sep 17 00:00:00 2001 From: Patrick Porlan Date: Fri, 13 Feb 2015 16:18:32 +0100 Subject: [PATCH] Experimental accelerometer calibration routine Only activated if quirk=biased is specified. Change-Id: Ie8faa11c62ba0d58e5529de4209e49a19075d4e6 Signed-off-by: Patrick Porlan Reviewed-on: https://android.intel.com:443/332405 --- Android.mk | 1 + accel-calibration.c | 207 ++++++++++++++++++++++++++++++++++++++++++++++++++++ calibration.h | 29 ++++++++ control.c | 21 +++++- description.c | 3 + description.h | 1 + enumeration.c | 8 +- transform.c | 2 + 8 files changed, 267 insertions(+), 5 deletions(-) create mode 100644 accel-calibration.c diff --git a/Android.mk b/Android.mk index 37dcf60..7a276de 100644 --- a/Android.mk +++ b/Android.mk @@ -22,6 +22,7 @@ src_files := $(src_path)/entry.c \ $(src_path)/gyro-calibration.c \ $(src_path)/filtering.c \ $(src_path)/discovery.c \ + $(src_path)/accel-calibration.c \ LOCAL_C_INCLUDES += $(LOCAL_PATH) vendor/intel/hardware/iio-sensors ifeq ($(HAL_AUTODETECT),true) diff --git a/accel-calibration.c b/accel-calibration.c new file mode 100644 index 0000000..aa67ece --- /dev/null +++ b/accel-calibration.c @@ -0,0 +1,207 @@ +/* + * Copyright (C) 2014-2015 Intel Corporation. + */ + +#include +#include +#include +#include +#include +#include "common.h" +#include "calibration.h" +#include "utils.h" + +/* + * This implements a crude way of estimating the accelerometer manufacturing mounting bias. We monitor x y and z distribution around a few strategic spots that + * should represent most hits when the device is stable (on earth!). We then try to derive an estimation of the accelerometer bias for each of these axes from + * that data, continuously. This is a very rough method that should only be used as a last resort for now. + */ + +static float bucket_center[BUCKET_COUNT] = { -9.8, 0, 9.8 }; /* The spots we are most interested in */ + +#define ACCEL_CALIB_DATA_VERSION 1 /* Update this whenever the stored data structure changes */ + +#define ACCEL_CALIBRATION_PATH "/data/accel.conf" /* Location of saved calibration data */ + +#define REFRESH_INTERVAL 1000*1000*1000 /* Recompute bias estimation every second */ + + +static void ascribe_sample (accel_cal_t* cal_data, int channel, float value) +{ + /* Check if this falls within one of our ranges of interest */ + float range_min; + float range_max; + int i; + int slice; + + for (i=0; i= range_min && value <= range_max) { + /* Find suitable bucket */ + slice = (int) ((value-range_min) / (range_max-range_min) * (SLICES-1)); + + /* Increment counters */ + cal_data->bucket[channel][i][slice]++; + cal_data->bucket_usage[channel][i]++; + return; + } + } +} + + +static float estimate_bias (accel_cal_t* cal_data, int channel) +{ + /* + * The long term distribution within the bucket, for each of the buckets, should be centered (samples evenly distributed). + * Try to determine the position in the bucket that separates it in two portions holding as many samples, then compute an estimated bias for that axis + * (channel) based on that data. + */ + + int i; + uint64_t half_of_the_samples; + uint64_t count; + float median; + float estimated_bucket_bias[BUCKET_COUNT] = {0}; + uint64_t bias_weight[BUCKET_COUNT]; + uint64_t total_weight; + float range_min; + float range_max; + float estimated_bias; + int slice; + + for (i=0; ibucket_usage[channel][i] / 2; + count = 0; + + for (slice = 0; slice < SLICES; slice++) { + count += cal_data->bucket[channel][i][slice]; + + if (count >= half_of_the_samples) { + range_min = bucket_center[i] - BUCKET_TOLERANCE; + range_max = bucket_center[i] + BUCKET_TOLERANCE; + + median = range_min + ((float) slice) / (SLICES-1) * (range_max-range_min); + + estimated_bucket_bias[i] = median - bucket_center[i]; + + bias_weight[i] = count; + break; + } + } + } + + /* Weight each of the estimated bucket bias values based on the number of samples collected */ + + total_weight = 0; + + for (i=0; idata[0]; + y = event->data[1]; + z = event->data[2]; + + /* Analyze sample */ + ascribe_sample(cal_data, 0, x); + ascribe_sample(cal_data, 1, y); + ascribe_sample(cal_data, 2, z); + + current_ts = get_timestamp_boot(); + + /* Estimate bias using accumulated data, from time to time*/ + if (current_ts >= cal_data->last_estimation_ts + REFRESH_INTERVAL) { + cal_data->last_estimation_ts = current_ts; + + cal_data->accel_bias_x = estimate_bias(cal_data, 0); + cal_data->accel_bias_y = estimate_bias(cal_data, 1); + cal_data->accel_bias_z = estimate_bias(cal_data, 2); + } + + ALOGV("Compensating for estimated accelerometer bias: x=%g, y=%g, z=%g\n", cal_data->accel_bias_x, cal_data->accel_bias_y, cal_data->accel_bias_z); + + /* Apply compensation */ + event->data[0] = x - cal_data->accel_bias_x; + event->data[1] = y - cal_data->accel_bias_y; + event->data[2] = z - cal_data->accel_bias_z; +} + + +void accel_cal_init (int s) +{ + int fd; + int n; + + accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data; + + if (cal_data == NULL) + return; + + if (cal_data->last_estimation_ts) + return; /* No need to overwrite perfectly good data at reenable time */ + + fd = open(ACCEL_CALIBRATION_PATH, O_RDONLY); + + if (fd != -1) { + n = read(fd, cal_data, sizeof(accel_cal_t)); + + close(fd); + + if (n == sizeof(accel_cal_t) && + cal_data->version == ((ACCEL_CALIB_DATA_VERSION << 16) + sizeof(accel_cal_t)) && + cal_data->bucket_count == BUCKET_COUNT && + cal_data->slices == SLICES && + cal_data->bucket_tolerance == BUCKET_TOLERANCE) { + cal_data->last_estimation_ts = 0; + return; /* We successfully loaded previously saved accelerometer calibration data */ + } + } + + /* Fall back to initial values */ + memset(cal_data, sizeof(accel_cal_t), 0); + + /* Store the parameters that are used with that data set, so we can check them against future version of the code to prevent inadvertent reuse */ + cal_data->version = (ACCEL_CALIB_DATA_VERSION << 16) + sizeof(accel_cal_t); + cal_data->bucket_count = BUCKET_COUNT; + cal_data->slices = SLICES; + cal_data->bucket_tolerance = BUCKET_TOLERANCE; +} + + +void accel_cal_store (int s) +{ + int fd; + accel_cal_t* cal_data = (accel_cal_t*) sensor[s].cal_data; + + if (cal_data == NULL) + return; + + fd = open(ACCEL_CALIBRATION_PATH, O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR); + + write(fd, cal_data, sizeof(accel_cal_t)); + + close(fd); +} diff --git a/calibration.h b/calibration.h index d6c8616..d353d43 100644 --- a/calibration.h +++ b/calibration.h @@ -36,6 +36,31 @@ typedef struct { } gyro_cal_t; +/* Accelerometer bias estimation and compensation */ + +#define BUCKET_COUNT 3 +#define BUCKET_TOLERANCE 1 /* Maximum monitoring distance from value of interest, in m/s² */ +#define SLICES 100 /* We currently have 3 buckets per axis, and 100 slices per bucket ; then we distribute incoming samples among slices */ + +typedef struct +{ + int version; + int bucket_count; + int slices; + float bucket_tolerance; + + uint64_t bucket[3][BUCKET_COUNT][SLICES]; /* How many samples fell in each of the slices, per axis */ + uint64_t bucket_usage[3][BUCKET_COUNT]; /* How many samples fell in each of the buckets (= sum of the slice counts) */ + + /* Estimated bias, according to accumulated data */ + float accel_bias_x; + float accel_bias_y; + float accel_bias_z; + + uint64_t last_estimation_ts; +} +accel_cal_t; + typedef double mat_input_t[MAGN_DS_SIZE][3]; @@ -48,4 +73,8 @@ void calibrate_gyro (sensors_event_t* event, sensor_info_t* info); void gyro_cal_init (sensor_info_t* info); void gyro_store_data (sensor_info_t* info); +void calibrate_accel (int s, sensors_event_t* event); +void accel_cal_init (int s); +void accel_cal_store (int s); + #endif diff --git a/control.c b/control.c index b8fdeab..781d648 100644 --- a/control.c +++ b/control.c @@ -373,6 +373,10 @@ int adjust_counters (int s, int enabled, int from_virtual) ALOGI("Enabling sensor %d (iio device %d: %s)\n", s, dev_num, sensor[s].friendly_name); switch (sensor[s].type) { + case SENSOR_TYPE_ACCELEROMETER: + accel_cal_init(s); + break; + case SENSOR_TYPE_MAGNETIC_FIELD: compass_read_data(&sensor[s]); break; @@ -387,11 +391,20 @@ int adjust_counters (int s, int enabled, int from_virtual) /* Sensor disabled, lower report available flag */ sensor[s].report_pending = 0; - if (sensor[s].type == SENSOR_TYPE_MAGNETIC_FIELD) - compass_store_data(&sensor[s]); + /* Save calibration data to persistent storage */ + switch (sensor[s].type) { + case SENSOR_TYPE_ACCELEROMETER: + accel_cal_store(s); + break; + + case SENSOR_TYPE_MAGNETIC_FIELD: + compass_store_data(&sensor[s]); + break; - if (sensor[s].type == SENSOR_TYPE_GYROSCOPE) - gyro_store_data(&sensor[s]); + case SENSOR_TYPE_GYROSCOPE: + gyro_store_data(&sensor[s]); + break; + } } /* We changed the state of a sensor: adjust device ref counts */ diff --git a/description.c b/description.c index 6d3239e..cc3bdb7 100644 --- a/description.c +++ b/description.c @@ -371,6 +371,9 @@ uint32_t sensor_get_quirks (int s) if (strstr(quirks_buf, "noisy")) sensor[s].quirks |= QUIRK_NOISY; + if (strstr(quirks_buf, "biased")) + sensor[s].quirks |= QUIRK_BIASED; + sensor[s].quirks |= QUIRK_ALREADY_DECODED; } diff --git a/description.h b/description.h index 2e62673..ab45c2b 100644 --- a/description.h +++ b/description.h @@ -13,6 +13,7 @@ #define QUIRK_TERSE_DRIVER 0x08 /* Force duplicate events generation */ #define QUIRK_NOISY 0x10 /* High noise level on readings */ #define QUIRK_FORCE_CONTINUOUS 0x20 /* Force usage of continuous trigger */ +#define QUIRK_BIASED 0x40 /* Biased sensor, requires compensation */ #ifdef __LP64__ typedef uint64_t flag_t; diff --git a/enumeration.c b/enumeration.c index e151e53..e5703e7 100644 --- a/enumeration.c +++ b/enumeration.c @@ -598,7 +598,13 @@ static void add_sensor (int dev_num, int catalog_index, int mode) } switch (sensor_type) { - case SENSOR_TYPE_GYROSCOPE: + case SENSOR_TYPE_ACCELEROMETER: + /* Only engage accelerometer bias compensation if really needed */ + if (sensor_get_quirks(s) & QUIRK_BIASED) + sensor[s].cal_data = calloc(1, sizeof(accel_cal_t)); + break; + + case SENSOR_TYPE_GYROSCOPE: sensor[s].cal_data = malloc(sizeof(gyro_cal_t)); break; diff --git a/transform.c b/transform.c index 74d5ef1..25094f6 100644 --- a/transform.c +++ b/transform.c @@ -299,6 +299,8 @@ static int finalize_sample_default (int s, sensors_event_t* data) case SENSOR_TYPE_ACCELEROMETER: /* Always consider the accelerometer accurate */ data->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; + if (sensor[s].quirks & QUIRK_BIASED) + calibrate_accel(s, data); denoise(s, data); break; -- 2.11.0