filtering.c

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