filtering.c

   1 /*
   2  * Copyright (C) 2014 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         float x, y, z;
 140         float scale;
 141         unsigned int field, offset;
 142
 143         filter_median_t* f_data = (filter_median_t*) info->filter;
 144         if (!f_data)
 145                 return;
 146
 147         /* If we are at event count 1 reset the indices */
 148         if (info->event_count == 1)
 149                 denoise_median_reset(info);
 150
 151         if (f_data->count < f_data->sample_size)
 152                 f_data->count++;
 153
 154         for (field = 0; field < num_fields; field++) {
 155                 offset = f_data->sample_size * field;
 156                 f_data->buff[offset + f_data->idx] = data->data[field];
 157
 158                 data->data[field] = median(f_data->buff + offset, f_data->count);
 159         }
 160
 161         f_data->idx = (f_data->idx + 1) % f_data->sample_size;
 162 }
 163
 164
 165 static void denoise_average (sensor_info_t* si, sensors_event_t* data)
 166 {
 167         /*
 168          * Smooth out incoming data using a moving average over a number of
 169          * samples. We accumulate one second worth of samples, or max_samples,
 170          * depending on which is lower.
 171          */
 172
 173         int i;
 174         int f;
 175         int sampling_rate = (int) si->sampling_rate;
 176         int history_size;
 177         int history_full = 0;
 178         filter_average_t* filter;
 179
 180         /* Don't denoise anything if we have less than two samples per second */
 181         if (sampling_rate < 2)
 182                 return;
 183
 184         filter = (filter_average_t*) si->filter;
 185
 186         if (!filter)
 187                 return;
 188
 189         /* Restrict window size to the min of sampling_rate and max_samples */
 190         if (sampling_rate > filter->max_samples)
 191                 history_size = filter->max_samples;
 192         else
 193                 history_size = sampling_rate;
 194
 195         /* Reset history if we're operating on an incorrect window size */
 196         if (filter->history_size != history_size) {
 197                 filter->history_size = history_size;
 198                 filter->history_entries = 0;
 199                 filter->history_index = 0;
 200                 filter->history = (float*) realloc(filter->history, filter->history_size * filter->num_fields * sizeof(float));
 201                 if (filter->history) {
 202                         filter->history_sum = (float*) realloc(filter->history_sum, filter->num_fields * sizeof(float));
 203                         if (filter->history_sum)
 204                                 memset(filter->history_sum, 0, filter->num_fields * sizeof(float));
 205                 }
 206         }
 207
 208         if (!filter->history || !filter->history_sum)
 209                 return; /* Unlikely, but still... */
 210
 211         /* Update initialized samples count */
 212         if (filter->history_entries < filter->history_size)
 213                 filter->history_entries++;
 214         else
 215                 history_full = 1;
 216
 217         /* Record new sample and calculate the moving sum */
 218         for (f=0; f < filter->num_fields; f++) {
 219                 /** A field is going to be overwritten if history is full, so decrease the history sum */
 220                 if (history_full)
 221                         filter->history_sum[f] -= filter->history[filter->history_index * filter->num_fields + f];
 222
 223                 filter->history[filter->history_index * filter->num_fields + f] = data->data[f];
 224                 filter->history_sum[f] += data->data[f];
 225
 226                 /* For now simply compute a mobile mean for each field and output filtered data */
 227                 data->data[f] = filter->history_sum[f] / filter->history_entries;
 228         }
 229
 230         /* Update our rolling index (next evicted cell) */
 231         filter->history_index = (filter->history_index + 1) % filter->history_size;
 232 }
 233
 234
 235 void setup_noise_filtering (int s)
 236 {
 237         char filter_buf[MAX_NAME_SIZE];
 238         int num_fields;
 239         char* cursor;
 240         int window_size = 0;
 241
 242         /* By default, don't apply filtering */
 243         sensor[s].filter_type = FILTER_TYPE_NONE;
 244
 245         /* Restrict filtering to a few sensor types for now */
 246         switch (sensor[s].type) {
 247                         case SENSOR_TYPE_ACCELEROMETER:
 248                         case SENSOR_TYPE_GYROSCOPE:
 249                         case SENSOR_TYPE_MAGNETIC_FIELD:
 250                                 num_fields = 3 /* x,y,z */;
 251                                 break;
 252
 253                         default:
 254                                 return; /* No filtering */
 255         }
 256
 257         /* If noisy, start with default filter for sensor type */
 258         if (sensor[s].quirks & QUIRK_NOISY)
 259                 switch (sensor[s].type) {
 260                         case SENSOR_TYPE_GYROSCOPE:
 261                                 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
 262                                 break;
 263
 264                         case SENSOR_TYPE_MAGNETIC_FIELD:
 265                                 sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
 266                                 break;
 267                 }
 268
 269         /* Use whatever was specified if there's an explicit configuration choice for this sensor */
 270
 271         filter_buf[0] = '\0';
 272         sensor_get_st_prop(s, "filter", filter_buf);
 273
 274         cursor = strstr(filter_buf, "median");
 275         if (cursor)
 276                 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
 277         else {
 278                 cursor = strstr(filter_buf, "average");
 279                 if (cursor)
 280                         sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
 281         }
 282
 283         /* Check if an integer is part of the string, and use it as window size */
 284         if (cursor) {
 285                 while (*cursor && !isdigit(*cursor))
 286                         cursor++;
 287
 288                 if (*cursor)
 289                         window_size = atoi(cursor);
 290         }
 291
 292         switch (sensor[s].filter_type) {
 293
 294                 case FILTER_TYPE_MEDIAN:
 295                         denoise_median_init(s, num_fields, window_size ? window_size : 5);
 296                         break;
 297
 298                 case FILTER_TYPE_MOVING_AVERAGE:
 299                         denoise_average_init(s, num_fields, window_size ? window_size: 20);
 300                         break;
 301         }
 302 }
 303
 304
 305 void denoise (int s, sensors_event_t* data)
 306 {
 307         switch (sensor[s].filter_type) {
 308
 309                 case FILTER_TYPE_MEDIAN:
 310                         denoise_median(&sensor[s], data, 3);
 311                         break;
 312
 313                 case FILTER_TYPE_MOVING_AVERAGE:
 314                         denoise_average(&sensor[s], data);
 315                         break;
 316         }
 317 }
 318
 319
 320 void release_noise_filtering_data (int s)
 321 {
 322         void *buf;
 323
 324         if (!sensor[s].filter)
 325                 return;
 326
 327         switch (sensor[s].filter_type) {
 328
 329                 case FILTER_TYPE_MEDIAN:
 330                         buf = ((filter_median_t*) sensor[s].filter)->buff;
 331                         if (buf)
 332                                 free(buf);
 333                         break;
 334
 335                 case FILTER_TYPE_MOVING_AVERAGE:
 336                         buf = ((filter_average_t*) sensor[s].filter)->history;
 337                         if (buf)
 338                                 free(buf);
 339
 340                         buf = ((filter_average_t*) sensor[s].filter)->history_sum;
 341                         if (buf)
 342                                 free(buf);
 343                         break;
 344         }
 345
 346         free(sensor[s].filter);
 347         sensor[s].filter = NULL;
 348 }
 349
 350
 351 #define GLOBAL_HISTORY_SIZE 100
 352
 353 typedef struct
 354 {
 355         int sensor;
 356         int motion_trigger;
 357         sensors_event_t data;
 358 }
 359 recorded_sample_t;
 360
 361 /*
 362  * 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
 363  * 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
 364  * (by insertion time) and replace its contents. Timestamps don't necessarily grow monotonically as they tell the data acquisition type, and that there
 365  * can be a delay between acquisition and insertion into this table.
 366  */
 367
 368 static recorded_sample_t global_history[GLOBAL_HISTORY_SIZE];
 369
 370 static int initialized_entries; /* How many of these are initialized          */
 371 static int insertion_index;     /* Index of sample to evict next time         */
 372
 373
 374 void record_sample (int s, const sensors_event_t* event)
 375 {
 376         recorded_sample_t *cell;
 377         int i;
 378
 379         /* Don't record duplicate samples, as they are not useful for filters */
 380         if (sensor[s].report_pending == DATA_DUPLICATE)
 381                 return;
 382
 383         if (initialized_entries == GLOBAL_HISTORY_SIZE) {
 384                 i = insertion_index;
 385                 insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE;
 386         } else {
 387                 i = initialized_entries;
 388                 initialized_entries++;
 389         }
 390
 391         cell = &global_history[i];
 392
 393         cell->sensor = s;
 394
 395         cell->motion_trigger = (sensor[s].selected_trigger == sensor[s].motion_trigger_name);
 396
 397         memcpy(&cell->data, event, sizeof(sensors_event_t));
 398 }