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