filtering.c

   1 /*
   2  * Copyright (C) 2014-2015 Intel Corporation.
   3  */
   4
   5 #include <ctype.h>
   6 #include <stdlib.h>
   7 #include <hardware/sensors.h>
   8 #include <utils/Log.h>
   9 #include "common.h"
  10 #include "filtering.h"
  11 #include "description.h"
  12
  13 typedef struct
  14 {
  15         float* buff;
  16         unsigned int idx;
  17         unsigned int count;
  18         unsigned int sample_size;
  19 }
  20 filter_median_t;
  21
  22
  23 typedef struct
  24 {
  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         */
  32 }
  33 filter_average_t;
  34
  35
  36 static unsigned int partition (float* list, unsigned int left, unsigned int right, unsigned int pivot_index)
  37 {
  38         unsigned int i;
  39         unsigned int store_index = left;
  40         float aux;
  41         float pivot_value = list[pivot_index];
  42
  43         /* Swap list[pivotIndex] and list[right] */
  44         aux = list[pivot_index];
  45         list[pivot_index] = list[right];
  46         list[right] = aux;
  47
  48         for (i = left; i < right; i++)
  49         {
  50                 if (list[i] < pivot_value)
  51                 {
  52                         /* Swap list[store_index] and list[i] */
  53                         aux = list[store_index];
  54                         list[store_index] = list[i];
  55                         list[i] = aux;
  56                         store_index++;
  57                 }
  58         }
  59
  60         /* Swap list[right] and list[store_index] */
  61         aux = list[right];
  62         list[right] = list[store_index];
  63         list[store_index] = aux;
  64         return store_index;
  65 }
  66
  67
  68 static float median (float* queue, unsigned int size)
  69 {
  70         /* http://en.wikipedia.org/wiki/Quickselect */
  71
  72         unsigned int left = 0;
  73         unsigned int right = size - 1;
  74         unsigned int pivot_index;
  75         unsigned int median_index = (right / 2);
  76         float temp[size];
  77
  78         memcpy(temp, queue, size * sizeof(float));
  79
  80         /* If the list has only one element return it */
  81         if (left == right)
  82                 return temp[left];
  83
  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;
  91                 else
  92                         left = pivot_index + 1;
  93         }
  94
  95         return temp[left];
  96 }
  97
  98
  99 static void denoise_median_init (int s, unsigned int num_fields, unsigned int max_samples)
 100 {
 101         filter_median_t* f_data = (filter_median_t*) malloc(sizeof(filter_median_t));
 102
 103         f_data->buff = (float*) calloc(max_samples, sizeof(float) * num_fields);
 104         f_data->sample_size = max_samples;
 105         f_data->count = 0;
 106         f_data->idx = 0;
 107         sensor[s].filter = f_data;
 108 }
 109
 110
 111 static void denoise_average_init (int s, unsigned int num_fields, unsigned int max_samples)
 112 {
 113         filter_average_t* filter = (filter_average_t*) malloc(sizeof(filter_average_t));
 114
 115         if (filter) {
 116                 memset(filter, 0, sizeof(filter_average_t));
 117                 filter->max_samples = max_samples;
 118                 filter->num_fields = num_fields;
 119         }
 120
 121         sensor[s].filter = filter;
 122 }
 123
 124
 125 static void denoise_median_reset (sensor_info_t* info)
 126 {
 127         filter_median_t* f_data = (filter_median_t*) info->filter;
 128
 129         if (!f_data)
 130                 return;
 131
 132         f_data->count = 0;
 133         f_data->idx = 0;
 134 }
 135
 136
 137 static void denoise_median (sensor_info_t* info, sensors_event_t* data, unsigned int num_fields)
 138 {
 139         unsigned int field, offset;
 140
 141         filter_median_t* f_data = (filter_median_t*) info->filter;
 142         if (!f_data)
 143                 return;
 144
 145         /* If we are at event count 1 reset the indices */
 146         if (info->event_count == 1)
 147                 denoise_median_reset(info);
 148
 149         if (f_data->count < f_data->sample_size)
 150                 f_data->count++;
 151
 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];
 155
 156                 data->data[field] = median(f_data->buff + offset, f_data->count);
 157         }
 158
 159         f_data->idx = (f_data->idx + 1) % f_data->sample_size;
 160 }
 161
 162
 163 static void denoise_average (sensor_info_t* si, sensors_event_t* data)
 164 {
 165         /*
 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.
 169          */
 170
 171         int f;
 172         int sampling_rate = (int) si->sampling_rate;
 173         int history_size;
 174         int history_full = 0;
 175         filter_average_t* filter;
 176
 177         /* Don't denoise anything if we have less than two samples per second */
 178         if (sampling_rate < 2)
 179                 return;
 180
 181         filter = (filter_average_t*) si->filter;
 182
 183         if (!filter)
 184                 return;
 185
 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;
 189         else
 190                 history_size = sampling_rate;
 191
 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));
 202                 }
 203         }
 204
 205         if (!filter->history || !filter->history_sum)
 206                 return; /* Unlikely, but still... */
 207
 208         /* Update initialized samples count */
 209         if (filter->history_entries < filter->history_size)
 210                 filter->history_entries++;
 211         else
 212                 history_full = 1;
 213
 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 */
 217                 if (history_full)
 218                         filter->history_sum[f] -= filter->history[filter->history_index * filter->num_fields + f];
 219
 220                 filter->history[filter->history_index * filter->num_fields + f] = data->data[f];
 221                 filter->history_sum[f] += data->data[f];
 222
 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;
 225         }
 226
 227         /* Update our rolling index (next evicted cell) */
 228         filter->history_index = (filter->history_index + 1) % filter->history_size;
 229 }
 230
 231
 232 void setup_noise_filtering (int s)
 233 {
 234         char filter_buf[MAX_NAME_SIZE];
 235         int num_fields;
 236         char* cursor;
 237         int window_size = 0;
 238
 239         /* By default, don't apply filtering */
 240         sensor[s].filter_type = FILTER_TYPE_NONE;
 241
 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 */;
 248                                 break;
 249
 250                         default:
 251                                 return; /* No filtering */
 252         }
 253
 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;
 259                                 break;
 260
 261                         case SENSOR_TYPE_MAGNETIC_FIELD:
 262                                 sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
 263                                 break;
 264                 }
 265
 266         /* Use whatever was specified if there's an explicit configuration choice for this sensor */
 267
 268         filter_buf[0] = '\0';
 269         sensor_get_st_prop(s, "filter", filter_buf);
 270
 271         cursor = strstr(filter_buf, "median");
 272         if (cursor)
 273                 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
 274         else {
 275                 cursor = strstr(filter_buf, "average");
 276                 if (cursor)
 277                         sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
 278         }
 279
 280         /* Check if an integer is part of the string, and use it as window size */
 281         if (cursor) {
 282                 while (*cursor && !isdigit(*cursor))
 283                         cursor++;
 284
 285                 if (*cursor)
 286                         window_size = atoi(cursor);
 287         }
 288
 289         switch (sensor[s].filter_type) {
 290
 291                 case FILTER_TYPE_MEDIAN:
 292                         denoise_median_init(s, num_fields, window_size ? window_size : 5);
 293                         break;
 294
 295                 case FILTER_TYPE_MOVING_AVERAGE:
 296                         denoise_average_init(s, num_fields, window_size ? window_size: 20);
 297                         break;
 298         }
 299 }
 300
 301
 302 void denoise (int s, sensors_event_t* data)
 303 {
 304         switch (sensor[s].filter_type) {
 305
 306                 case FILTER_TYPE_MEDIAN:
 307                         denoise_median(&sensor[s], data, 3);
 308                         break;
 309
 310                 case FILTER_TYPE_MOVING_AVERAGE:
 311                         denoise_average(&sensor[s], data);
 312                         break;
 313         }
 314 }
 315
 316
 317 void release_noise_filtering_data (int s)
 318 {
 319         void *buf;
 320
 321         if (!sensor[s].filter)
 322                 return;
 323
 324         switch (sensor[s].filter_type) {
 325
 326                 case FILTER_TYPE_MEDIAN:
 327                         buf = ((filter_median_t*) sensor[s].filter)->buff;
 328                         if (buf)
 329                                 free(buf);
 330                         break;
 331
 332                 case FILTER_TYPE_MOVING_AVERAGE:
 333                         buf = ((filter_average_t*) sensor[s].filter)->history;
 334                         if (buf)
 335                                 free(buf);
 336
 337                         buf = ((filter_average_t*) sensor[s].filter)->history_sum;
 338                         if (buf)
 339                                 free(buf);
 340                         break;
 341         }
 342
 343         free(sensor[s].filter);
 344         sensor[s].filter = NULL;
 345 }
 346
 347
 348 #define GLOBAL_HISTORY_SIZE 100
 349
 350 typedef struct
 351 {
 352         int sensor;
 353         int motion_trigger;
 354         sensors_event_t data;
 355 }
 356 recorded_sample_t;
 357
 358 /*
 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.
 363  */
 364
 365 static recorded_sample_t global_history[GLOBAL_HISTORY_SIZE];
 366
 367 static int initialized_entries; /* How many of these are initialized          */
 368 static int insertion_index;     /* Index of sample to evict next time         */
 369
 370
 371 void record_sample (int s, const sensors_event_t* event)
 372 {
 373         recorded_sample_t *cell;
 374         int i;
 375
 376         /* Don't record duplicate samples, as they are not useful for filters */
 377         if (sensor[s].report_pending == DATA_DUPLICATE)
 378                 return;
 379
 380         if (initialized_entries == GLOBAL_HISTORY_SIZE) {
 381                 i = insertion_index;
 382                 insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE;
 383         } else {
 384                 i = initialized_entries;
 385                 initialized_entries++;
 386         }
 387
 388         cell = &global_history[i];
 389
 390         cell->sensor = s;
 391
 392         cell->motion_trigger = (sensor[s].selected_trigger == sensor[s].motion_trigger_name);
 393
 394         memcpy(&cell->data, event, sizeof(sensors_event_t));
 395 }