+/*
+// 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 <ctype.h>
#include <stdlib.h>
#include <hardware/sensors.h>
-#include <math.h>
-#include <pthread.h>
+#include <utils/Log.h>
#include "common.h"
#include "filtering.h"
+#include "description.h"
-void add_to_buff(struct circ_buff* circ_buff, float val)
+typedef struct
{
- if (circ_buff->count < circ_buff->size)
- {
- circ_buff->buff[circ_buff->count] = val;
- circ_buff->count++;
- return;
- }
+ float* buff;
+ unsigned int idx;
+ unsigned int count;
+ unsigned int sample_size;
+}
+filter_median_t;
+
- circ_buff->idx = circ_buff->idx % circ_buff->size;
- circ_buff->buff[circ_buff->idx] = val;
- circ_buff->idx++;
- return;
+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;
float aux;
float pivot_value = list[pivot_index];
- // swap list[pivotIndex] and list[right]
+ /* Swap list[pivotIndex] and list[right] */
aux = list[pivot_index];
list[pivot_index] = list[right];
list[right] = aux;
{
if (list[i] < pivot_value)
{
- // swap list[store_index] and list[i]
+ /* Swap list[store_index] and list[i] */
aux = list[store_index];
list[store_index] = list[i];
list[i] = aux;
store_index++;
}
}
- //swap list[right] and list[store_index]
+
+ /* Swap list[right] and list[store_index] */
aux = list[right];
list[right] = list[store_index];
list[store_index] = aux;
return store_index;
}
-/* http://en.wikipedia.org/wiki/Quickselect */
-float median(float* queue, unsigned int size)
+
+static float median (float* queue, unsigned int size)
{
+ /* http://en.wikipedia.org/wiki/Quickselect */
+
unsigned int left = 0;
unsigned int right = size - 1;
unsigned int pivot_index;
return temp[left];
}
-void denoise_median(struct sensors_event_t* data, struct sensor_info_t* info)
+
+static void denoise_median_init (int s, unsigned int num_fields, unsigned int max_samples)
+{
+ 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->sample_size = max_samples;
+ f_data->count = 0;
+ f_data->idx = 0;
+ sensor[s].filter = f_data;
+}
+
+
+static void denoise_average_init (int s, unsigned int num_fields, unsigned int max_samples)
+{
+ 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;
+
+ f_data->count = 0;
+ f_data->idx = 0;
+}
+
+
+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* f_data = (struct filter*) info->filter;
+ filter_median_t* f_data = (filter_median_t*) info->filter;
if (!f_data)
return;
- x = data->data[0];
- y = data->data[1];
- z = data->data[2];
+ /* If we are at event count 1 reset the indices */
+ if (info->event_count == 1)
+ denoise_median_reset(info);
+
+ if (f_data->count < f_data->sample_size)
+ f_data->count++;
- add_to_buff(f_data->x_buff, x);
- add_to_buff(f_data->y_buff, y);
- add_to_buff(f_data->z_buff, z);
+ for (field = 0; field < num_fields; field++) {
+ offset = f_data->sample_size * field;
+ f_data->buff[offset + f_data->idx] = data->data[field];
- x = median(f_data->x_buff->buff, f_data->x_buff->count);
- y = median(f_data->y_buff->buff, f_data->y_buff->count);
- z = median(f_data->z_buff->buff, f_data->z_buff->count);
+ data->data[field] = median(f_data->buff + offset, f_data->count);
+ }
- data->data[0] = x;
- data->data[1] = y;
- data->data[2] = z;
+ f_data->idx = (f_data->idx + 1) % f_data->sample_size;
}
+
+static void denoise_average (sensor_info_t* si, sensors_event_t* data)
+{
+ /*
+ * Smooth out incoming data using a moving average over a number of
+ * samples. We accumulate one second worth of samples, or max_samples,
+ * depending on which is lower.
+ */
+
+ 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 > 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 (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 (!filter->history || !filter->history_sum)
+ return; /* Unlikely, but still... */
+
+ /* Update initialized samples count */
+ 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 < filter->num_fields; f++) {
+ /** A field is going to be overwritten if history is full, so decrease the history sum */
+ if (history_full)
+ filter->history_sum[f] -= filter->history[filter->history_index * filter->num_fields + 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] = filter->history_sum[f] / filter->history_entries;
+ }
+
+ /* Update our rolling index (next evicted cell) */
+ 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_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, sensors_event_t* data)
+{
+ switch (sensor[s].filter_type) {
+
+ case FILTER_TYPE_MEDIAN:
+ denoise_median(&sensor[s], data, 3);
+ break;
+
+ case FILTER_TYPE_MOVING_AVERAGE:
+ denoise_average(&sensor[s], data);
+ break;
+ }
+}
+
+
+void release_noise_filtering_data (int s)
+{
+ void *buf;
+
+ if (!sensor[s].filter)
+ return;
+
+ switch (sensor[s].filter_type) {
+
+ 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
+
+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.
+ */
+
+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 sensors_event_t* event)
+{
+ recorded_sample_t *cell;
+ int i;
+
+ /* Don't record duplicate samples, as they are not useful for filters */
+ if (sensor[s].report_pending == DATA_DUPLICATE)
+ return;
+
+ if (initialized_entries == GLOBAL_HISTORY_SIZE) {
+ i = insertion_index;
+ insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE;
+ } else {
+ i = initialized_entries;
+ initialized_entries++;
+ }
+
+ cell = &global_history[i];
+
+ cell->sensor = s;
+
+ cell->motion_trigger = (sensor[s].selected_trigger == sensor[s].motion_trigger_name);
+
+ memcpy(&cell->data, event, sizeof(sensors_event_t));
+}
OSDN Git Service
