Compass calibration minor fixes

[android-x86/hardware-intel-libsensors.git] / filtering.c

diff --git a/filtering.c b/filtering.c
index 1083fa9..9217de6 100644 (file)
--- a/filtering.c
+++ b/filtering.c
@@ -2,23 +2,18 @@
 #include <hardware/sensors.h>
 #include <math.h>
 #include <pthread.h>
+#include <utils/Log.h>
 #include "common.h"
 #include "filtering.h"
 
-void add_to_buff(struct circ_buff* circ_buff, float val)
-{
-       if (circ_buff->count < circ_buff->size)
-       {
-               circ_buff->buff[circ_buff->count] = val;
-               circ_buff->count++;
-               return;
-       }
 
-       circ_buff->idx = circ_buff->idx % circ_buff->size;
-       circ_buff->buff[circ_buff->idx] = val;
-       circ_buff->idx++;
-       return;
-}
+struct filter_median
+{
+       float* buff;
+       unsigned int idx;
+       unsigned int count;
+       unsigned int sample_size;
+};
 
 static unsigned int partition(float* list, unsigned int left,
        unsigned int right, unsigned int pivot_index)
@@ -80,29 +75,116 @@ float median(float* queue, unsigned int size)
        return temp[left];
 }
 
-void denoise_median(struct sensors_event_t* data, struct sensor_info_t* info)
+void denoise_median_init(int s, unsigned int num_fields,
+       unsigned int max_samples)
+{
+       struct filter_median* f_data = (struct filter_median*) calloc(1, sizeof(struct filter_median));
+       f_data->buff = (float*)calloc(max_samples,
+               sizeof(float) * num_fields);
+       f_data->sample_size = max_samples;
+       f_data->count = 0;
+       f_data->idx = 0;
+       sensor_info[s].filter = f_data;
+}
+
+static void denoise_median_reset(struct sensor_info_t* info)
+{
+       struct filter_median* f_data = (struct filter_median*) info->filter;
+
+       if (!f_data)
+               return;
+
+       f_data->count = 0;
+       f_data->idx = 0;
+}
+
+void denoise_median_release(int s)
+{
+       if (!sensor_info[s].filter)
+               return;
+
+       free(((struct filter_median*)sensor_info[s].filter)->buff);
+       free(sensor_info[s].filter);
+       sensor_info[s].filter = NULL;
+}
+
+void denoise_median(struct sensor_info_t* info, struct sensors_event_t* data,
+                                       unsigned int num_fields)
 {
        float x, y, z;
        float scale;
+       unsigned int field, offset;
 
-       struct filter* f_data = (struct filter*) info->filter;
+       struct filter_median* f_data = (struct filter_median*) info->filter;
        if (!f_data)
                return;
 
-       x = data->data[0];
-       y = data->data[1];
-       z = data->data[2];
+       /* If we are at event count 1 reset the indexes */
+       if (info->event_count == 1)
+               denoise_median_reset(info);
 
-       add_to_buff(f_data->x_buff, x);
-       add_to_buff(f_data->y_buff, y);
-       add_to_buff(f_data->z_buff, z);
+       if (f_data->count < f_data->sample_size)
+               f_data->count++;
 
-       x = median(f_data->x_buff->buff, f_data->x_buff->count);
-       y = median(f_data->y_buff->buff, f_data->y_buff->count);
-       z = median(f_data->z_buff->buff, f_data->z_buff->count);
+       for (field = 0; field < num_fields; field++) {
+               offset = f_data->sample_size * field;
+               f_data->buff[offset + f_data->idx] = data->data[field];
 
-       data->data[0] = x;
-       data->data[1] = y;
-       data->data[2] = z;
+               data->data[field] = median(f_data->buff + offset, f_data->count);
+       }
+
+       f_data->idx = (f_data->idx + 1) % f_data->sample_size;
 }
 
+
+#define GLOBAL_HISTORY_SIZE 100
+
+struct recorded_sample_t
+{
+       int sensor;
+       int motion_trigger;
+       sensors_event_t data;
+};
+
+/*
+ * 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
+ * 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
+ * (by insertion time) and replace its contents. Timestamps don't necessarily
+ * grow monotonically as they tell the data acquisition type, and that there can
+ * be a delay between acquisition and insertion into this table.
+ */
+
+static struct recorded_sample_t global_history[GLOBAL_HISTORY_SIZE];
+
+static int initialized_entries;        /* How many of these are initialized          */
+static int insertion_index;    /* Index of sample to evict next time         */
+
+
+void record_sample (int s, const struct sensors_event_t* event)
+{
+       struct recorded_sample_t *cell;
+       int i;
+
+       /* Don't record duplicate samples, as they are not useful for filters */
+       if (sensor_info[s].report_pending == DATA_DUPLICATE)
+               return;
+
+       if (initialized_entries == GLOBAL_HISTORY_SIZE) {
+               i = insertion_index;
+               insertion_index = (insertion_index+1) % GLOBAL_HISTORY_SIZE;
+       } else {
+               i = initialized_entries;
+               initialized_entries++;
+       }
+
+       cell = &global_history[i];
+
+       cell->sensor            = s;
+
+       cell->motion_trigger    = (sensor_info[s].selected_trigger ==
+                                  sensor_info[s].motion_trigger_name);
+
+       memcpy(&cell->data, event, sizeof(sensors_event_t));
+}