filtering.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation.
   3  */
   4
   5 #include <hardware/sensors.h>
   6 #include <utils/Log.h>
   7 #include "common.h"
   8 #include "filtering.h"
   9 #include "description.h"
  10
  11 typedef struct
  12 {
  13         float* buff;
  14         unsigned int idx;
  15         unsigned int count;
  16         unsigned int sample_size;
  17 }
  18 filter_median_t;
  19
  20
  21 static unsigned int partition (float* list, unsigned int left, unsigned int right, unsigned int pivot_index)
  22 {
  23         unsigned int i;
  24         unsigned int store_index = left;
  25         float aux;
  26         float pivot_value = list[pivot_index];
  27
  28         /* Swap list[pivotIndex] and list[right] */
  29         aux = list[pivot_index];
  30         list[pivot_index] = list[right];
  31         list[right] = aux;
  32
  33         for (i = left; i < right; i++)
  34         {
  35                 if (list[i] < pivot_value)
  36                 {
  37                         /* Swap list[store_index] and list[i] */
  38                         aux = list[store_index];
  39                         list[store_index] = list[i];
  40                         list[i] = aux;
  41                         store_index++;
  42                 }
  43         }
  44
  45         /* Swap list[right] and list[store_index] */
  46         aux = list[right];
  47         list[right] = list[store_index];
  48         list[store_index] = aux;
  49         return store_index;
  50 }
  51
  52
  53 static float median (float* queue, unsigned int size)
  54 {
  55         /* http://en.wikipedia.org/wiki/Quickselect */
  56
  57         unsigned int left = 0;
  58         unsigned int right = size - 1;
  59         unsigned int pivot_index;
  60         unsigned int median_index = (right / 2);
  61         float temp[size];
  62
  63         memcpy(temp, queue, size * sizeof(float));
  64
  65         /* If the list has only one element return it */
  66         if (left == right)
  67                 return temp[left];
  68
  69         while (left < right) {
  70                 pivot_index = (left + right) / 2;
  71                 pivot_index = partition(temp, left, right, pivot_index);
  72                 if (pivot_index == median_index)
  73                         return temp[median_index];
  74                 else if (pivot_index > median_index)
  75                         right = pivot_index - 1;
  76                 else
  77                         left = pivot_index + 1;
  78         }
  79
  80         return temp[left];
  81 }
  82
  83
  84 static void denoise_median_init (int s, unsigned int num_fields, unsigned int max_samples)
  85 {
  86         filter_median_t* f_data = (filter_median_t*) malloc(sizeof(filter_median_t));
  87
  88         f_data->buff = (float*) calloc(max_samples, sizeof(float) * num_fields);
  89         f_data->sample_size = max_samples;
  90         f_data->count = 0;
  91         f_data->idx = 0;
  92         sensor[s].filter = f_data;
  93 }
  94
  95
  96 static void denoise_median_reset (sensor_info_t* info)
  97 {
  98         filter_median_t* f_data = (filter_median_t*) info->filter;
  99
 100         if (!f_data)
 101                 return;
 102
 103         f_data->count = 0;
 104         f_data->idx = 0;
 105 }
 106
 107
 108 static void denoise_median (sensor_info_t* info, sensors_event_t* data, unsigned int num_fields)
 109 {
 110         float x, y, z;
 111         float scale;
 112         unsigned int field, offset;
 113
 114         filter_median_t* f_data = (filter_median_t*) info->filter;
 115         if (!f_data)
 116                 return;
 117
 118         /* If we are at event count 1 reset the indices */
 119         if (info->event_count == 1)
 120                 denoise_median_reset(info);
 121
 122         if (f_data->count < f_data->sample_size)
 123                 f_data->count++;
 124
 125         for (field = 0; field < num_fields; field++) {
 126                 offset = f_data->sample_size * field;
 127                 f_data->buff[offset + f_data->idx] = data->data[field];
 128
 129                 data->data[field] = median(f_data->buff + offset, f_data->count);
 130         }
 131
 132         f_data->idx = (f_data->idx + 1) % f_data->sample_size;
 133 }
 134
 135
 136 static void denoise_average (sensor_info_t* si, sensors_event_t* data, int num_fields, int max_samples)
 137 {
 138         /*
 139          * Smooth out incoming data using a moving average over a number of
 140          * samples. We accumulate one second worth of samples, or max_samples,
 141          * depending on which is lower.
 142          */
 143
 144         int i;
 145         int f;
 146         int sampling_rate = (int) si->sampling_rate;
 147         int history_size;
 148         int history_full = 0;
 149
 150         /* Don't denoise anything if we have less than two samples per second */
 151         if (sampling_rate < 2)
 152                 return;
 153
 154         /* Restrict window size to the min of sampling_rate and max_samples */
 155         if (sampling_rate > max_samples)
 156                 history_size = max_samples;
 157         else
 158                 history_size = sampling_rate;
 159
 160         /* Reset history if we're operating on an incorrect window size */
 161         if (si->history_size != history_size) {
 162                 si->history_size = history_size;
 163                 si->history_entries = 0;
 164                 si->history_index = 0;
 165                 si->history = (float*) realloc(si->history, si->history_size * num_fields * sizeof(float));
 166                 if (si->history) {
 167                         si->history_sum = (float*) realloc(si->history_sum, num_fields * sizeof(float));
 168                         if (si->history_sum)
 169                                 memset(si->history_sum, 0, num_fields * sizeof(float));
 170                 }
 171         }
 172
 173         if (!si->history || !si->history_sum)
 174                 return; /* Unlikely, but still... */
 175
 176         /* Update initialized samples count */
 177         if (si->history_entries < si->history_size)
 178                 si->history_entries++;
 179         else
 180                 history_full = 1;
 181
 182         /* Record new sample and calculate the moving sum */
 183         for (f=0; f < num_fields; f++) {
 184                 /** A field is going to be overwritten if history is full, so decrease the history sum */
 185                 if (history_full)
 186                         si->history_sum[f] -=
 187                                 si->history[si->history_index * num_fields + f];
 188
 189                 si->history[si->history_index * num_fields + f] = data->data[f];
 190                 si->history_sum[f] += data->data[f];
 191
 192                 /* For now simply compute a mobile mean for each field and output filtered data */
 193                 data->data[f] = si->history_sum[f] / si->history_entries;
 194         }
 195
 196         /* Update our rolling index (next evicted cell) */
 197         si->history_index = (si->history_index + 1) % si->history_size;
 198 }
 199
 200
 201 void setup_noise_filtering (int s)
 202 {
 203         char filter_buf[MAX_NAME_SIZE];
 204
 205         /* By default, don't apply filtering */
 206         sensor[s].filter_type = FILTER_TYPE_NONE;
 207
 208         /* If noisy, start with default filter for sensor type */
 209         if (sensor[s].quirks & QUIRK_NOISY)
 210                 switch (sensor[s].type) {
 211                         case SENSOR_TYPE_GYROSCOPE:
 212                                 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
 213                                 break;
 214
 215                         case SENSOR_TYPE_MAGNETIC_FIELD:
 216                                 sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
 217                                 break;
 218                 }
 219
 220         /* Use whatever was specified if there's an explicit configuration choice for this sensor */
 221
 222         filter_buf[0] = '\0';
 223         sensor_get_st_prop(s, "filter", filter_buf);
 224
 225         if (strstr(filter_buf, "median"))
 226                 sensor[s].filter_type = FILTER_TYPE_MEDIAN;
 227
 228         if (strstr(filter_buf, "average"))
 229                 sensor[s].filter_type = FILTER_TYPE_MOVING_AVERAGE;
 230
 231         switch (sensor[s].filter_type) {
 232                 case FILTER_TYPE_MEDIAN:
 233                         denoise_median_init(s, 3, 5);
 234                         break;
 235         }
 236 }
 237
 238
 239 void denoise (int s, sensors_event_t* data)
 240 {
 241         switch (sensor[s].filter_type) {
 242                 case FILTER_TYPE_MEDIAN:
 243                         denoise_median(&sensor[s], data, 3);
 244                         break;
 245
 246                 case FILTER_TYPE_MOVING_AVERAGE:
 247                         denoise_average(&sensor[s], data, 3 , 20);
 248                         break;
 249         }
 250 }
 251
 252
 253 void release_noise_filtering_data (int s)
 254 {
 255         void *buff;
 256
 257         /* Delete moving average structures */
 258         if (sensor[s].history) {
 259                 free(sensor[s].history);
 260                 sensor[s].history = NULL;
 261                 sensor[s].history_size = 0;
 262                 if (sensor[s].history_sum) {
 263                         free(sensor[s].history_sum);
 264                         sensor[s].history_sum = NULL;
 265                 }
 266         }
 267
 268         /* Delete median filter structures */
 269         if (sensor[s].filter) {
 270                 buff = ((filter_median_t*) sensor[s].filter)->buff;
 271
 272                 if (buff)
 273                         free(buff);
 274
 275                 free(sensor[s].filter);
 276                 sensor[s].filter = NULL;
 277         }
 278 }
 279
 280
 281 #define GLOBAL_HISTORY_SIZE 100
 282
 283 typedef struct
 284 {
 285         int sensor;
 286         int motion_trigger;
 287         sensors_event_t data;
 288 }
 289 recorded_sample_t;
 290
 291 /*
 292  * 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
 293  * 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
 294  * (by insertion time) and replace its contents. Timestamps don't necessarily grow monotonically as they tell the data acquisition type, and that there
 295  * can be a delay between acquisition and insertion into this table.
 296  */
 297
 298 static recorded_sample_t global_history[GLOBAL_HISTORY_SIZE];
 299
 300 static int initialized_entries; /* How many of these are initialized          */
 301 static int insertion_index;     /* Index of sample to evict next time         */
 302
 303
 304 void record_sample (int s, const sensors_event_t* event)
 305 {
 306         recorded_sample_t *cell;
 307         int i;
 308
 309         /* Don't record duplicate samples, as they are not useful for filters */
 310         if (sensor[s].report_pending == DATA_DUPLICATE)
 311                 return;
 312
 313         if (initialized_entries == GLOBAL_HISTORY_SIZE) {
 314                 i = insertion_index;
 315                 insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE;
 316         } else {
 317                 i = initialized_entries;
 318                 initialized_entries++;
 319         }
 320
 321         cell = &global_history[i];
 322
 323         cell->sensor = s;
 324
 325         cell->motion_trigger = (sensor[s].selected_trigger == sensor[s].motion_trigger_name);
 326
 327         memcpy(&cell->data, event, sizeof(sensors_event_t));
 328 }