filtering.c

   1 #include <stdlib.h>
   2 #include <hardware/sensors.h>
   3 #include <math.h>
   4 #include <pthread.h>
   5 #include "common.h"
   6 #include "filtering.h"
   7
   8 void add_to_buff(struct circ_buff* circ_buff, float val)
   9 {
  10         if (circ_buff->count < circ_buff->size)
  11         {
  12                 circ_buff->buff[circ_buff->count] = val;
  13                 circ_buff->count++;
  14                 return;
  15         }
  16
  17         circ_buff->idx = circ_buff->idx % circ_buff->size;
  18         circ_buff->buff[circ_buff->idx] = val;
  19         circ_buff->idx++;
  20         return;
  21 }
  22
  23 static unsigned int partition(float* list, unsigned int left,
  24         unsigned int right, unsigned int pivot_index)
  25 {
  26         unsigned int i;
  27         unsigned int store_index = left;
  28         float aux;
  29         float pivot_value = list[pivot_index];
  30
  31         // swap list[pivotIndex] and list[right]
  32         aux = list[pivot_index];
  33         list[pivot_index] = list[right];
  34         list[right] = aux;
  35
  36         for (i = left; i < right; i++)
  37         {
  38                 if (list[i] < pivot_value)
  39                 {
  40                         // swap list[store_index] and list[i]
  41                         aux = list[store_index];
  42                         list[store_index] = list[i];
  43                         list[i] = aux;
  44                         store_index++;
  45                 }
  46         }
  47         //swap list[right] and list[store_index]
  48         aux = list[right];
  49         list[right] = list[store_index];
  50         list[store_index] = aux;
  51         return store_index;
  52 }
  53
  54 /* http://en.wikipedia.org/wiki/Quickselect */
  55 float median(float* queue, unsigned int size)
  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 void denoise_median(struct sensors_event_t* data, struct sensor_info_t* info)
  84 {
  85         float x, y, z;
  86         float scale;
  87
  88         struct filter* f_data = (struct filter*) info->filter;
  89         if (!f_data)
  90                 return;
  91
  92         x = data->data[0];
  93         y = data->data[1];
  94         z = data->data[2];
  95
  96         add_to_buff(f_data->x_buff, x);
  97         add_to_buff(f_data->y_buff, y);
  98         add_to_buff(f_data->z_buff, z);
  99
 100         x = median(f_data->x_buff->buff, f_data->x_buff->count);
 101         y = median(f_data->y_buff->buff, f_data->y_buff->count);
 102         z = median(f_data->z_buff->buff, f_data->z_buff->count);
 103
 104         data->data[0] = x;
 105         data->data[1] = y;
 106         data->data[2] = z;
 107 }
 108