filtering.c

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