2 * Copyright (C) 2014-2015 Intel Corporation.
7 #include <hardware/sensors.h>
10 #include "filtering.h"
11 #include "description.h"
18 unsigned int sample_size;
25 int max_samples; /* Maximum averaging window size */
26 int num_fields; /* Number of fields per sample (usually 3) */
27 float *history; /* Working buffer containing recorded samples */
28 float *history_sum; /* The current sum of the history elements */
29 int history_size; /* Number of recorded samples */
30 int history_entries; /* How many of these are initialized */
31 int history_index; /* Index of sample to evict next time */
36 static unsigned int partition (float* list, unsigned int left, unsigned int right, unsigned int pivot_index)
39 unsigned int store_index = left;
41 float pivot_value = list[pivot_index];
43 /* Swap list[pivotIndex] and list[right] */
44 aux = list[pivot_index];
45 list[pivot_index] = list[right];
48 for (i = left; i < right; i++)
50 if (list[i] < pivot_value)
52 /* Swap list[store_index] and list[i] */
53 aux = list[store_index];
54 list[store_index] = list[i];
60 /* Swap list[right] and list[store_index] */
62 list[right] = list[store_index];
63 list[store_index] = aux;
68 static float median (float* queue, unsigned int size)
70 /* http://en.wikipedia.org/wiki/Quickselect */
72 unsigned int left = 0;
73 unsigned int right = size - 1;
74 unsigned int pivot_index;
75 unsigned int median_index = (right / 2);
78 memcpy(temp, queue, size * sizeof(float));
80 /* If the list has only one element return it */
84 while (left < right) {
85 pivot_index = (left + right) / 2;
86 pivot_index = partition(temp, left, right, pivot_index);
87 if (pivot_index == median_index)
88 return temp[median_index];
89 else if (pivot_index > median_index)
90 right = pivot_index - 1;
92 left = pivot_index + 1;
99 static void denoise_median_init (int s, unsigned int num_fields, unsigned int max_samples)
101 filter_median_t* f_data = (filter_median_t*) malloc(sizeof(filter_median_t));
103 f_data->buff = (float*) calloc(max_samples, sizeof(float) * num_fields);
104 f_data->sample_size = max_samples;
107 sensor[s].filter = f_data;
111 static void denoise_average_init (int s, unsigned int num_fields, unsigned int max_samples)
113 filter_average_t* filter = (filter_average_t*) malloc(sizeof(filter_average_t));
116 memset(filter, 0, sizeof(filter_average_t));
117 filter->max_samples = max_samples;
118 filter->num_fields = num_fields;
121 sensor[s].filter = filter;
125 static void denoise_median_reset (sensor_info_t* info)
127 filter_median_t* f_data = (filter_median_t*) info->filter;
137 static void denoise_median (sensor_info_t* info, sensors_event_t* data, unsigned int num_fields)
139 unsigned int field, offset;
141 filter_median_t* f_data = (filter_median_t*) info->filter;
145 /* If we are at event count 1 reset the indices */
146 if (info->event_count == 1)
147 denoise_median_reset(info);
149 if (f_data->count < f_data->sample_size)
152 for (field = 0; field < num_fields; field++) {
153 offset = f_data->sample_size * field;
154 f_data->buff[offset + f_data->idx] = data->data[field];
156 data->data[field] = median(f_data->buff + offset, f_data->count);
159 f_data->idx = (f_data->idx + 1) % f_data->sample_size;
163 static void denoise_average (sensor_info_t* si, sensors_event_t* data)
166 * Smooth out incoming data using a moving average over a number of
167 * samples. We accumulate one second worth of samples, or max_samples,
168 * depending on which is lower.
172 int sampling_rate = (int) si->sampling_rate;
174 int history_full = 0;
175 filter_average_t* filter;
177 /* Don't denoise anything if we have less than two samples per second */
178 if (sampling_rate < 2)
181 filter = (filter_average_t*) si->filter;
186 /* Restrict window size to the min of sampling_rate and max_samples */
187 if (sampling_rate > filter->max_samples)
188 history_size = filter->max_samples;
190 history_size = sampling_rate;
192 /* Reset history if we're operating on an incorrect window size */
193 if (filter->history_size != history_size) {
194 filter->history_size = history_size;
195 filter->history_entries = 0;
196 filter->history_index = 0;
197 filter->history = (float*) realloc(filter->history, filter->history_size * filter->num_fields * sizeof(float));
198 if (filter->history) {
199 filter->history_sum = (float*) realloc(filter->history_sum, filter->num_fields * sizeof(float));
200 if (filter->history_sum)
201 memset(filter->history_sum, 0, filter->num_fields * sizeof(float));
205 if (!filter->history || !filter->history_sum)
206 return; /* Unlikely, but still... */
208 /* Update initialized samples count */
209 if (filter->history_entries < filter->history_size)
210 filter->history_entries++;
214 /* Record new sample and calculate the moving sum */
215 for (f=0; f < filter->num_fields; f++) {
216 /** A field is going to be overwritten if history is full, so decrease the history sum */
218 filter->history_sum[f] -= filter->history[filter->history_index * filter->num_fields + f];
220 filter->history[filter->history_index * filter->num_fields + f] = data->data[f];
221 filter->history_sum[f] += data->data[f];
223 /* For now simply compute a mobile mean for each field and output filtered data */
224 data->data[f] = filter->history_sum[f] / filter->history_entries;
227 /* Update our rolling index (next evicted cell) */
228 filter->history_index = (filter->history_index + 1) % filter->history_size;
232 void setup_noise_filtering (int s)
234 char filter_buf[MAX_NAME_SIZE];
239 /* By default, don't apply filtering */
240 sensor[s].filter_type = FILTER_TYPE_NONE;
242 /* Restrict filtering to a few sensor types for now */
243 switch (sensor[s].type) {
244 case SENSOR_TYPE_ACCELEROMETER:
245 case SENSOR_TYPE_GYROSCOPE:
246 case SENSOR_TYPE_MAGNETIC_FIELD:
247 num_fields = 3 /* x,y,z */;
251 return; /* No filtering */
254 /* If noisy, start with default filter for sensor type */
255 if (sensor[s].quirks & QUIRK_NOISY)
256 switch (sensor[s].type) {
257 case SENSOR_TYPE_GYROSCOPE:
258 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
261 case SENSOR_TYPE_MAGNETIC_FIELD:
262 sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
266 /* Use whatever was specified if there's an explicit configuration choice for this sensor */
268 filter_buf[0] = '\0';
269 sensor_get_st_prop(s, "filter", filter_buf);
271 cursor = strstr(filter_buf, "median");
273 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
275 cursor = strstr(filter_buf, "average");
277 sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
280 /* Check if an integer is part of the string, and use it as window size */
282 while (*cursor && !isdigit(*cursor))
286 window_size = atoi(cursor);
289 switch (sensor[s].filter_type) {
291 case FILTER_TYPE_MEDIAN:
292 denoise_median_init(s, num_fields, window_size ? window_size : 5);
295 case FILTER_TYPE_MOVING_AVERAGE:
296 denoise_average_init(s, num_fields, window_size ? window_size: 20);
302 void denoise (int s, sensors_event_t* data)
304 switch (sensor[s].filter_type) {
306 case FILTER_TYPE_MEDIAN:
307 denoise_median(&sensor[s], data, 3);
310 case FILTER_TYPE_MOVING_AVERAGE:
311 denoise_average(&sensor[s], data);
317 void release_noise_filtering_data (int s)
321 if (!sensor[s].filter)
324 switch (sensor[s].filter_type) {
326 case FILTER_TYPE_MEDIAN:
327 buf = ((filter_median_t*) sensor[s].filter)->buff;
332 case FILTER_TYPE_MOVING_AVERAGE:
333 buf = ((filter_average_t*) sensor[s].filter)->history;
337 buf = ((filter_average_t*) sensor[s].filter)->history_sum;
343 free(sensor[s].filter);
344 sensor[s].filter = NULL;
348 #define GLOBAL_HISTORY_SIZE 100
354 sensors_event_t data;
359 * 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
360 * 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
361 * (by insertion time) and replace its contents. Timestamps don't necessarily grow monotonically as they tell the data acquisition type, and that there
362 * can be a delay between acquisition and insertion into this table.
365 static recorded_sample_t global_history[GLOBAL_HISTORY_SIZE];
367 static int initialized_entries; /* How many of these are initialized */
368 static int insertion_index; /* Index of sample to evict next time */
371 void record_sample (int s, const sensors_event_t* event)
373 recorded_sample_t *cell;
376 /* Don't record duplicate samples, as they are not useful for filters */
377 if (sensor[s].report_pending == DATA_DUPLICATE)
380 if (initialized_entries == GLOBAL_HISTORY_SIZE) {
382 insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE;
384 i = initialized_entries;
385 initialized_entries++;
388 cell = &global_history[i];
392 cell->motion_trigger = (sensor[s].selected_trigger == sensor[s].motion_trigger_name);
394 memcpy(&cell->data, event, sizeof(sensors_event_t));