2 // Copyright (c) 2015 Intel Corporation
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
8 // http://www.apache.org/licenses/LICENSE-2.0
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
19 #include <hardware/sensors.h>
20 #include <utils/Log.h>
22 #include "filtering.h"
23 #include "description.h"
30 unsigned int sample_size;
37 int max_samples; /* Maximum averaging window size */
38 int num_fields; /* Number of fields per sample (usually 3) */
39 float *history; /* Working buffer containing recorded samples */
40 float *history_sum; /* The current sum of the history elements */
41 int history_size; /* Number of recorded samples */
42 int history_entries; /* How many of these are initialized */
43 int history_index; /* Index of sample to evict next time */
48 static unsigned int partition (float* list, unsigned int left, unsigned int right, unsigned int pivot_index)
51 unsigned int store_index = left;
53 float pivot_value = list[pivot_index];
55 /* Swap list[pivotIndex] and list[right] */
56 aux = list[pivot_index];
57 list[pivot_index] = list[right];
60 for (i = left; i < right; i++)
62 if (list[i] < pivot_value)
64 /* Swap list[store_index] and list[i] */
65 aux = list[store_index];
66 list[store_index] = list[i];
72 /* Swap list[right] and list[store_index] */
74 list[right] = list[store_index];
75 list[store_index] = aux;
80 static float median (float* queue, unsigned int size)
82 /* http://en.wikipedia.org/wiki/Quickselect */
84 unsigned int left = 0;
85 unsigned int right = size - 1;
86 unsigned int pivot_index;
87 unsigned int median_index = (right / 2);
90 memcpy(temp, queue, size * sizeof(float));
92 /* If the list has only one element return it */
96 while (left < right) {
97 pivot_index = (left + right) / 2;
98 pivot_index = partition(temp, left, right, pivot_index);
99 if (pivot_index == median_index)
100 return temp[median_index];
101 else if (pivot_index > median_index)
102 right = pivot_index - 1;
104 left = pivot_index + 1;
111 static void denoise_median_init (int s, unsigned int num_fields, unsigned int max_samples)
113 filter_median_t* f_data = (filter_median_t*) malloc(sizeof(filter_median_t));
115 f_data->buff = (float*) calloc(max_samples, sizeof(float) * num_fields);
116 f_data->sample_size = max_samples;
119 sensor[s].filter = f_data;
123 static void denoise_average_init (int s, unsigned int num_fields, unsigned int max_samples)
125 filter_average_t* filter = (filter_average_t*) malloc(sizeof(filter_average_t));
128 memset(filter, 0, sizeof(filter_average_t));
129 filter->max_samples = max_samples;
130 filter->num_fields = num_fields;
133 sensor[s].filter = filter;
137 static void denoise_median_reset (sensor_info_t* info)
139 filter_median_t* f_data = (filter_median_t*) info->filter;
149 static void denoise_median (sensor_info_t* info, sensors_event_t* data, unsigned int num_fields)
151 unsigned int field, offset;
153 filter_median_t* f_data = (filter_median_t*) info->filter;
157 /* If we are at event count 1 reset the indices */
158 if (info->event_count == 1)
159 denoise_median_reset(info);
161 if (f_data->count < f_data->sample_size)
164 for (field = 0; field < num_fields; field++) {
165 offset = f_data->sample_size * field;
166 f_data->buff[offset + f_data->idx] = data->data[field];
168 data->data[field] = median(f_data->buff + offset, f_data->count);
171 f_data->idx = (f_data->idx + 1) % f_data->sample_size;
175 static void denoise_average (sensor_info_t* si, sensors_event_t* data)
178 * Smooth out incoming data using a moving average over a number of
179 * samples. We accumulate one second worth of samples, or max_samples,
180 * depending on which is lower.
184 int sampling_rate = (int) si->sampling_rate;
186 int history_full = 0;
187 filter_average_t* filter;
189 /* Don't denoise anything if we have less than two samples per second */
190 if (sampling_rate < 2)
193 filter = (filter_average_t*) si->filter;
198 /* Restrict window size to the min of sampling_rate and max_samples */
199 if (sampling_rate > filter->max_samples)
200 history_size = filter->max_samples;
202 history_size = sampling_rate;
204 /* Reset history if we're operating on an incorrect window size */
205 if (filter->history_size != history_size) {
206 filter->history_size = history_size;
207 filter->history_entries = 0;
208 filter->history_index = 0;
209 filter->history = (float*) realloc(filter->history, filter->history_size * filter->num_fields * sizeof(float));
210 if (filter->history) {
211 filter->history_sum = (float*) realloc(filter->history_sum, filter->num_fields * sizeof(float));
212 if (filter->history_sum)
213 memset(filter->history_sum, 0, filter->num_fields * sizeof(float));
217 if (!filter->history || !filter->history_sum)
218 return; /* Unlikely, but still... */
220 /* Update initialized samples count */
221 if (filter->history_entries < filter->history_size)
222 filter->history_entries++;
226 /* Record new sample and calculate the moving sum */
227 for (f=0; f < filter->num_fields; f++) {
228 /** A field is going to be overwritten if history is full, so decrease the history sum */
230 filter->history_sum[f] -= filter->history[filter->history_index * filter->num_fields + f];
232 filter->history[filter->history_index * filter->num_fields + f] = data->data[f];
233 filter->history_sum[f] += data->data[f];
235 /* For now simply compute a mobile mean for each field and output filtered data */
236 data->data[f] = filter->history_sum[f] / filter->history_entries;
239 /* Update our rolling index (next evicted cell) */
240 filter->history_index = (filter->history_index + 1) % filter->history_size;
244 void setup_noise_filtering (int s)
246 char filter_buf[MAX_NAME_SIZE];
251 /* By default, don't apply filtering */
252 sensor[s].filter_type = FILTER_TYPE_NONE;
254 /* Restrict filtering to a few sensor types for now */
255 switch (sensor[s].type) {
256 case SENSOR_TYPE_ACCELEROMETER:
257 case SENSOR_TYPE_GYROSCOPE:
258 case SENSOR_TYPE_MAGNETIC_FIELD:
259 num_fields = 3 /* x,y,z */;
263 return; /* No filtering */
266 /* If noisy, start with default filter for sensor type */
267 if (sensor[s].quirks & QUIRK_NOISY)
268 switch (sensor[s].type) {
269 case SENSOR_TYPE_GYROSCOPE:
270 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
273 case SENSOR_TYPE_MAGNETIC_FIELD:
274 sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
278 /* Use whatever was specified if there's an explicit configuration choice for this sensor */
280 filter_buf[0] = '\0';
281 sensor_get_st_prop(s, "filter", filter_buf);
283 cursor = strstr(filter_buf, "median");
285 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
287 cursor = strstr(filter_buf, "average");
289 sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
292 /* Check if an integer is part of the string, and use it as window size */
294 while (*cursor && !isdigit(*cursor))
298 window_size = atoi(cursor);
301 switch (sensor[s].filter_type) {
303 case FILTER_TYPE_MEDIAN:
304 denoise_median_init(s, num_fields, window_size ? window_size : 5);
307 case FILTER_TYPE_MOVING_AVERAGE:
308 denoise_average_init(s, num_fields, window_size ? window_size: 20);
314 void denoise (int s, sensors_event_t* data)
316 switch (sensor[s].filter_type) {
318 case FILTER_TYPE_MEDIAN:
319 denoise_median(&sensor[s], data, 3);
322 case FILTER_TYPE_MOVING_AVERAGE:
323 denoise_average(&sensor[s], data);
329 void release_noise_filtering_data (int s)
333 if (!sensor[s].filter)
336 switch (sensor[s].filter_type) {
338 case FILTER_TYPE_MEDIAN:
339 buf = ((filter_median_t*) sensor[s].filter)->buff;
344 case FILTER_TYPE_MOVING_AVERAGE:
345 buf = ((filter_average_t*) sensor[s].filter)->history;
349 buf = ((filter_average_t*) sensor[s].filter)->history_sum;
355 free(sensor[s].filter);
356 sensor[s].filter = NULL;
360 #define GLOBAL_HISTORY_SIZE 100
366 sensors_event_t data;
371 * 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
372 * 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
373 * (by insertion time) and replace its contents. Timestamps don't necessarily grow monotonically as they tell the data acquisition type, and that there
374 * can be a delay between acquisition and insertion into this table.
377 static recorded_sample_t global_history[GLOBAL_HISTORY_SIZE];
379 static int initialized_entries; /* How many of these are initialized */
380 static int insertion_index; /* Index of sample to evict next time */
383 void record_sample (int s, const sensors_event_t* event)
385 recorded_sample_t *cell;
388 /* Don't record duplicate samples, as they are not useful for filters */
389 if (sensor[s].report_pending == DATA_DUPLICATE)
392 if (initialized_entries == GLOBAL_HISTORY_SIZE) {
394 insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE;
396 i = initialized_entries;
397 initialized_entries++;
400 cell = &global_history[i];
404 cell->motion_trigger = (sensor[s].selected_trigger == sensor[s].motion_trigger_name);
406 memcpy(&cell->data, event, sizeof(sensors_event_t));