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 static unsigned int partition(float* list, unsigned int left,
  19         unsigned int right, unsigned int pivot_index)
  20 {
  21         unsigned int i;
  22         unsigned int store_index = left;
  23         float aux;
  24         float pivot_value = list[pivot_index];
  25
  26         // swap list[pivotIndex] and list[right]
  27         aux = list[pivot_index];
  28         list[pivot_index] = list[right];
  29         list[right] = aux;
  30
  31         for (i = left; i < right; i++)
  32         {
  33                 if (list[i] < pivot_value)
  34                 {
  35                         // swap list[store_index] and list[i]
  36                         aux = list[store_index];
  37                         list[store_index] = list[i];
  38                         list[i] = aux;
  39                         store_index++;
  40                 }
  41         }
  42         //swap list[right] and list[store_index]
  43         aux = list[right];
  44         list[right] = list[store_index];
  45         list[store_index] = aux;
  46         return store_index;
  47 }
  48
  49 /* http://en.wikipedia.org/wiki/Quickselect */
  50 float median(float* queue, unsigned int size)
  51 {
  52         unsigned int left = 0;
  53         unsigned int right = size - 1;
  54         unsigned int pivot_index;
  55         unsigned int median_index = (right / 2);
  56         float temp[size];
  57
  58         memcpy(temp, queue, size * sizeof(float));
  59
  60         /* If the list has only one element return it */
  61         if (left == right)
  62                 return temp[left];
  63
  64         while (left < right) {
  65                 pivot_index = (left + right) / 2;
  66                 pivot_index = partition(temp, left, right, pivot_index);
  67                 if (pivot_index == median_index)
  68                         return temp[median_index];
  69                 else if (pivot_index > median_index)
  70                         right = pivot_index - 1;
  71                 else
  72                         left = pivot_index + 1;
  73         }
  74
  75         return temp[left];
  76 }
  77
  78 void denoise_median_init(int s, unsigned int num_fields,
  79         unsigned int max_samples)
  80 {
  81         struct filter_median* f_data = (struct filter_median*) calloc(1, sizeof(struct filter_median));
  82         f_data->buff = (float*)calloc(max_samples,
  83                 sizeof(float) * num_fields);
  84         f_data->sample_size = max_samples;
  85         f_data->count = 0;
  86         f_data->idx = 0;
  87         sensor_info[s].filter = f_data;
  88 }
  89
  90 static void denoise_median_reset(struct sensor_info_t* info)
  91 {
  92         struct filter_median* f_data = (struct filter_median*) info->filter;
  93
  94         if (!f_data)
  95                 return;
  96
  97         f_data->count = 0;
  98         f_data->idx = 0;
  99 }
 100
 101 void denoise_median_release(int s)
 102 {
 103         if (!sensor_info[s].filter)
 104                 return;
 105
 106         free(((struct filter_median*)sensor_info[s].filter)->buff);
 107         free(sensor_info[s].filter);
 108         sensor_info[s].filter = NULL;
 109 }
 110
 111 void denoise_median(struct sensor_info_t* info, 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 indexes */
 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 #define GLOBAL_HISTORY_SIZE 100
 141
 142 struct recorded_sample_t
 143 {
 144         int sensor;
 145         int motion_trigger;
 146         sensors_event_t data;
 147 };
 148
 149 /*
 150  * This is a circular buffer holding the last GLOBAL_HISTORY_SIZE events,
 151  * covering the entire sensor collection. It is intended as a way to correlate
 152  * data coming from active sensors, no matter the sensor type, over a recent
 153  * window of time. The array is not sorted ; we simply evict the oldest cell
 154  * (by insertion time) and replace its contents. Timestamps don't necessarily
 155  * grow monotonically as they tell the data acquisition type, and that there can
 156  * be a delay between acquisition and insertion into this table.
 157  */
 158
 159 static struct recorded_sample_t global_history[GLOBAL_HISTORY_SIZE];
 160
 161 static int initialized_entries; /* How many of these are initialized          */
 162 static int insertion_index;     /* Index of sample to evict next time         */
 163
 164
 165 void record_sample (int s, const struct sensors_event_t* event)
 166 {
 167         struct recorded_sample_t *cell;
 168         int i;
 169
 170         /* Don't record duplicate samples, as they are not useful for filters */
 171         if (sensor_info[s].report_pending == DATA_DUPLICATE)
 172                 return;
 173
 174         if (initialized_entries == GLOBAL_HISTORY_SIZE) {
 175                 i = insertion_index;
 176                 insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE;
 177         } else {
 178                 i = initialized_entries;
 179                 initialized_entries++;
 180         }
 181
 182         cell = &global_history[i];
 183
 184         cell->sensor            = s;
 185
 186         cell->motion_trigger    = (sensor_info[s].selected_trigger ==
 187                                    sensor_info[s].motion_trigger_name);
 188
 189         memcpy(&cell->data, event, sizeof(sensors_event_t));
 190 }