X-Git-Url: http://git.osdn.net/view?p=android-x86%2Fhardware-intel-libsensors.git;a=blobdiff_plain;f=filtering.c;h=2d605042bc206c718c342506497749465fbc72d0;hp=9c2fae4945f3b19e7536c8ed917fcf745b45b9bd;hb=refs%2Fheads%2Foreo-x86;hpb=3848e248b88fd3a8ab065d7566b02fc7a67a58bf diff --git a/filtering.c b/filtering.c index 9c2fae4..2d60504 100644 --- a/filtering.c +++ b/filtering.c @@ -1,23 +1,51 @@ +/* +// Copyright (c) 2015 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +*/ + +#include #include #include -#include -#include #include #include "common.h" #include "filtering.h" +#include "description.h" - -struct filter_median +typedef struct { float* buff; unsigned int idx; unsigned int count; unsigned int sample_size; -}; +} +filter_median_t; + + +typedef struct +{ + int max_samples; /* Maximum averaging window size */ + int num_fields; /* Number of fields per sample (usually 3) */ + float *history; /* Working buffer containing recorded samples */ + float *history_sum; /* The current sum of the history elements */ + int history_size; /* Number of recorded samples */ + int history_entries; /* How many of these are initialized */ + int history_index; /* Index of sample to evict next time */ +} +filter_average_t; -static unsigned int partition ( float* list, unsigned int left, - unsigned int right, unsigned int pivot_index) +static unsigned int partition (float* list, unsigned int left, unsigned int right, unsigned int pivot_index) { unsigned int i; unsigned int store_index = left; @@ -80,14 +108,11 @@ static float median (float* queue, unsigned int size) } -static void denoise_median_init(int s, unsigned int num_fields, - unsigned int max_samples) +static void denoise_median_init (int s, unsigned int num_fields, unsigned int max_samples) { - struct filter_median* f_data = (struct filter_median*) calloc(1, - sizeof(struct filter_median)); + filter_median_t* f_data = (filter_median_t*) malloc(sizeof(filter_median_t)); - f_data->buff = (float*)calloc(max_samples, - sizeof(float) * num_fields); + f_data->buff = (float*) calloc(max_samples, sizeof(float) * num_fields); f_data->sample_size = max_samples; f_data->count = 0; f_data->idx = 0; @@ -95,9 +120,23 @@ static void denoise_median_init(int s, unsigned int num_fields, } -static void denoise_median_reset (struct sensor_info_t* info) +static void denoise_average_init (int s, unsigned int num_fields, unsigned int max_samples) { - struct filter_median* f_data = (struct filter_median*) info->filter; + filter_average_t* filter = (filter_average_t*) malloc(sizeof(filter_average_t)); + + if (filter) { + memset(filter, 0, sizeof(filter_average_t)); + filter->max_samples = max_samples; + filter->num_fields = num_fields; + } + + sensor[s].filter = filter; +} + + +static void denoise_median_reset (sensor_info_t* info) +{ + filter_median_t* f_data = (filter_median_t*) info->filter; if (!f_data) return; @@ -107,15 +146,11 @@ static void denoise_median_reset (struct sensor_info_t* info) } -static void denoise_median ( struct sensor_info_t* info, - struct sensors_event_t* data, - unsigned int num_fields) +static void denoise_median (sensor_info_t* info, sensors_event_t* data, unsigned int num_fields) { - float x, y, z; - float scale; unsigned int field, offset; - struct filter_median* f_data = (struct filter_median*) info->filter; + filter_median_t* f_data = (filter_median_t*) info->filter; if (!f_data) return; @@ -137,9 +172,7 @@ static void denoise_median ( struct sensor_info_t* info, } -static void denoise_average ( struct sensor_info_t* si, - struct sensors_event_t* data, - int num_fields, int max_samples) +static void denoise_average (sensor_info_t* si, sensors_event_t* data) { /* * Smooth out incoming data using a moving average over a number of @@ -147,88 +180,147 @@ static void denoise_average ( struct sensor_info_t* si, * depending on which is lower. */ - int i; int f; int sampling_rate = (int) si->sampling_rate; int history_size; int history_full = 0; + filter_average_t* filter; /* Don't denoise anything if we have less than two samples per second */ if (sampling_rate < 2) return; + filter = (filter_average_t*) si->filter; + + if (!filter) + return; + /* Restrict window size to the min of sampling_rate and max_samples */ - if (sampling_rate > max_samples) - history_size = max_samples; + if (sampling_rate > filter->max_samples) + history_size = filter->max_samples; else history_size = sampling_rate; /* Reset history if we're operating on an incorrect window size */ - if (si->history_size != history_size) { - si->history_size = history_size; - si->history_entries = 0; - si->history_index = 0; - si->history = (float*) realloc(si->history, - si->history_size * num_fields * sizeof(float)); - if (si->history) { - si->history_sum = (float*) realloc(si->history_sum, - num_fields * sizeof(float)); - if (si->history_sum) - memset(si->history_sum, 0, num_fields * sizeof(float)); + if (filter->history_size != history_size) { + filter->history_size = history_size; + filter->history_entries = 0; + filter->history_index = 0; + filter->history = (float*) realloc(filter->history, filter->history_size * filter->num_fields * sizeof(float)); + if (filter->history) { + filter->history_sum = (float*) realloc(filter->history_sum, filter->num_fields * sizeof(float)); + if (filter->history_sum) + memset(filter->history_sum, 0, filter->num_fields * sizeof(float)); } } - if (!si->history || !si->history_sum) + if (!filter->history || !filter->history_sum) return; /* Unlikely, but still... */ /* Update initialized samples count */ - if (si->history_entries < si->history_size) - si->history_entries++; + if (filter->history_entries < filter->history_size) + filter->history_entries++; else history_full = 1; /* Record new sample and calculate the moving sum */ - for (f=0; f < num_fields; f++) { - /** - * A field is going to be overwritten if - * history is full, so decrease the history sum - */ + for (f=0; f < filter->num_fields; f++) { + /** A field is going to be overwritten if history is full, so decrease the history sum */ if (history_full) - si->history_sum[f] -= - si->history[si->history_index * num_fields + f]; + filter->history_sum[f] -= filter->history[filter->history_index * filter->num_fields + f]; - si->history[si->history_index * num_fields + f] = data->data[f]; - si->history_sum[f] += data->data[f]; + filter->history[filter->history_index * filter->num_fields + f] = data->data[f]; + filter->history_sum[f] += data->data[f]; - /* For now simply compute a mobile mean for each field */ - /* and output filtered data */ - data->data[f] = si->history_sum[f] / si->history_entries; + /* For now simply compute a mobile mean for each field and output filtered data */ + data->data[f] = filter->history_sum[f] / filter->history_entries; } /* Update our rolling index (next evicted cell) */ - si->history_index = (si->history_index + 1) % si->history_size; + filter->history_index = (filter->history_index + 1) % filter->history_size; } void setup_noise_filtering (int s) { + char filter_buf[MAX_NAME_SIZE]; + int num_fields; + char* cursor; + int window_size = 0; + + /* By default, don't apply filtering */ + sensor[s].filter_type = FILTER_TYPE_NONE; + + /* Restrict filtering to a few sensor types for now */ switch (sensor[s].type) { - case SENSOR_TYPE_GYROSCOPE: - denoise_median_init(s, 3, 5); + case SENSOR_TYPE_ACCELEROMETER: + case SENSOR_TYPE_GYROSCOPE: + case SENSOR_TYPE_MAGNETIC_FIELD: + num_fields = 3 /* x,y,z */; + break; + + default: + return; /* No filtering */ + } + + /* If noisy, start with default filter for sensor type */ + if (sensor[s].quirks & QUIRK_NOISY) + switch (sensor[s].type) { + case SENSOR_TYPE_GYROSCOPE: + sensor[s].filter_type = FILTER_TYPE_MEDIAN; + break; + + case SENSOR_TYPE_MAGNETIC_FIELD: + sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE; + break; + } + + /* Use whatever was specified if there's an explicit configuration choice for this sensor */ + + filter_buf[0] = '\0'; + sensor_get_st_prop(s, "filter", filter_buf); + + cursor = strstr(filter_buf, "median"); + if (cursor) + sensor[s].filter_type = FILTER_TYPE_MEDIAN; + else { + cursor = strstr(filter_buf, "average"); + if (cursor) + sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE; + } + + /* Check if an integer is part of the string, and use it as window size */ + if (cursor) { + while (*cursor && !isdigit(*cursor)) + cursor++; + + if (*cursor) + window_size = atoi(cursor); + } + + switch (sensor[s].filter_type) { + + case FILTER_TYPE_MEDIAN: + denoise_median_init(s, num_fields, window_size ? window_size : 5); + break; + + case FILTER_TYPE_MOVING_AVERAGE: + denoise_average_init(s, num_fields, window_size ? window_size: 20); break; } } -void denoise (int s, struct sensors_event_t* data) +void denoise (int s, sensors_event_t* data) { - switch (sensor[s].type) { - case SENSOR_TYPE_GYROSCOPE: + switch (sensor[s].filter_type) { + + case FILTER_TYPE_MEDIAN: denoise_median(&sensor[s], data, 3); break; - case SENSOR_TYPE_MAGNETIC_FIELD: - denoise_average(&sensor[s], data, 3 , 20); + case FILTER_TYPE_MOVING_AVERAGE: + denoise_average(&sensor[s], data); break; } } @@ -236,60 +328,61 @@ void denoise (int s, struct sensors_event_t* data) void release_noise_filtering_data (int s) { - void *buff; - - /* Delete moving average structures */ - if (sensor[s].history) { - free(sensor[s].history); - sensor[s].history = NULL; - sensor[s].history_size = 0; - if (sensor[s].history_sum) { - free(sensor[s].history_sum); - sensor[s].history_sum = NULL; - } - } + void *buf; - /* Delete median filter structures */ - if (sensor[s].filter) { - buff = ((struct filter_median*)sensor[s].filter)->buff; + if (!sensor[s].filter) + return; - if (buff) - free(buff); + switch (sensor[s].filter_type) { - free(sensor[s].filter); - sensor[s].filter = NULL; + case FILTER_TYPE_MEDIAN: + buf = ((filter_median_t*) sensor[s].filter)->buff; + if (buf) + free(buf); + break; + + case FILTER_TYPE_MOVING_AVERAGE: + buf = ((filter_average_t*) sensor[s].filter)->history; + if (buf) + free(buf); + + buf = ((filter_average_t*) sensor[s].filter)->history_sum; + if (buf) + free(buf); + break; } + + free(sensor[s].filter); + sensor[s].filter = NULL; } #define GLOBAL_HISTORY_SIZE 100 -struct recorded_sample_t +typedef struct { int sensor; int motion_trigger; sensors_event_t data; -}; +} +recorded_sample_t; /* - * This is a circular buffer holding the last GLOBAL_HISTORY_SIZE events, - * covering the entire sensor collection. It is intended as a way to correlate - * data coming from active sensors, no matter the sensor type, over a recent - * window of time. The array is not sorted ; we simply evict the oldest cell - * (by insertion time) and replace its contents. Timestamps don't necessarily - * grow monotonically as they tell the data acquisition type, and that there can - * be a delay between acquisition and insertion into this table. + * This is a circular buffer holding the last GLOBAL_HISTORY_SIZE events, covering the entire sensor collection. It is intended as a way to correlate + * data coming from active sensors, no matter the sensor type, over a recent window of time. The array is not sorted ; we simply evict the oldest cell + * (by insertion time) and replace its contents. Timestamps don't necessarily grow monotonically as they tell the data acquisition type, and that there + * can be a delay between acquisition and insertion into this table. */ -static struct recorded_sample_t global_history[GLOBAL_HISTORY_SIZE]; +static recorded_sample_t global_history[GLOBAL_HISTORY_SIZE]; static int initialized_entries; /* How many of these are initialized */ static int insertion_index; /* Index of sample to evict next time */ -void record_sample (int s, const struct sensors_event_t* event) +void record_sample (int s, const sensors_event_t* event) { - struct recorded_sample_t *cell; + recorded_sample_t *cell; int i; /* Don't record duplicate samples, as they are not useful for filters */ @@ -308,8 +401,7 @@ void record_sample (int s, const struct sensors_event_t* event) cell->sensor = s; - cell->motion_trigger = (sensor[s].selected_trigger == - sensor[s].motion_trigger_name); + cell->motion_trigger = (sensor[s].selected_trigger == sensor[s].motion_trigger_name); memcpy(&cell->data, event, sizeof(sensors_event_t)); }