GMIN-2992: Start with -dev trigger before switching to -any-motion

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

diff --git a/transform.c b/transform.c
index c79ba1b..e86fe1a 100644 (file)
--- a/transform.c
+++ b/transform.c
@@ -7,10 +7,11 @@
 #include <utils/Log.h>
 #include <cutils/properties.h>
 #include <hardware/sensors.h>
+#include "calibration.h"
 #include "common.h"
+#include "description.h"
 #include "transform.h"
 #include "utils.h"
-#include "calibration.h"
 
 /*----------------------------------------------------------------------------*/
 
@@ -123,11 +124,11 @@ inline float convert_from_vtf_format(int size, int exponent, unsigned int value)
 
 static int64_t sample_as_int64(unsigned char* sample, struct datum_info_t* type)
 {
-       uint16_t u16;
-       uint32_t u32;
        uint64_t u64;
        int i;
        int zeroed_bits = type->storagebits - type->realbits;
+       uint64_t sign_mask;
+       uint64_t value_mask;
 
        u64 = 0;
 
@@ -143,14 +144,15 @@ static int64_t sample_as_int64(unsigned char* sample, struct datum_info_t* type)
        if (type->sign == 'u')
                return (int64_t) u64; /* We don't handle unsigned 64 bits int */
 
+       /* Signed integer */
+
        switch (type->realbits) {
+               case 0 ... 1:
+                       return 0;
+
                case 8:
                        return (int64_t) (int8_t) u64;
 
-               case 12:
-                       return (int64_t)  (u64 >>  11) ?
-                                       (((int64_t)-1) ^ 0xfff) | u64 : u64;
-
                case 16:
                        return (int64_t) (int16_t) u64;
 
@@ -159,10 +161,17 @@ static int64_t sample_as_int64(unsigned char* sample, struct datum_info_t* type)
 
                case 64:
                        return (int64_t) u64;
-       }
 
-       ALOGE("Unhandled sample storage size\n");
-       return 0;
+               default:
+                       sign_mask = 1 << (type->realbits-1);
+                       value_mask = sign_mask - 1;
+
+                       if (u64 & sign_mask)
+                               /* Negative value: return 2-complement */
+                               return - ((~u64 & value_mask) + 1);
+                       else
+                               return (int64_t) u64; /* Positive value */
+       }
 }
 
 
@@ -179,26 +188,87 @@ static void reorder_fields(float* data,   unsigned char map[MAX_CHANNELS])
 }
 
 
+static void denoise (struct sensor_info_t* si, struct sensors_event_t* data,
+                    int num_fields)
+{
+       int i;
+       float total;
+       int f;
+       int sampling_rate = (int) si->sampling_rate;
+       int history_size;
+
+       /* Don't denoise anything if we have less than two samples per second */
+       if (sampling_rate < 2)
+               return;
+
+       /* Restrict window size in case of a very high sampling rate */
+       if (sampling_rate > 100)
+               history_size = 100;
+       else
+               history_size = sampling_rate;
+
+       /* Reset history if we're operating on an incorrect window size */
+       if (si->history_size != history_size) {
+               si->history_size = history_size;
+               si->history_entries = 0;
+               si->history_index = 0;
+               si->history = (float*) realloc(si->history,
+                               si->history_size * num_fields * sizeof(float));
+       }
+
+       if (!si->history)
+               return; /* Unlikely, but still... */
+
+       /* Update initialized samples count */
+       if (si->history_entries < si->history_size)
+               si->history_entries++;
+
+       /* Record new sample */
+       for (f=0; f < num_fields; f++)
+               si->history[si->history_index * num_fields + f] = data->data[f];
+
+       /* Update our rolling index (next evicted cell) */
+       si->history_index = (si->history_index + 1) % si->history_size;
+
+       /* For now simply compute a mobile mean for each field */
+       for (f=0; f < num_fields; f++) {
+               total = 0;
+
+               for (i=0; i < si->history_entries; i++)
+                               total += si->history[i * num_fields + f];
+
+               /* Output filtered data */
+               data->data[f] = total / si->history_entries;
+       }
+}
+
+
 static int finalize_sample_default(int s, struct sensors_event_t* data)
 {
        int i           = sensor_info[s].catalog_index;
        int sensor_type = sensor_catalog[i].type;
 
        /* Swap fields if we have a custom channel ordering on this sensor */
-       if (sensor_info[s].flags & FLAG_FIELD_ORDERING)
+       if (sensor_info[s].quirks & QUIRK_FIELD_ORDERING)
                reorder_fields(data->data, sensor_info[s].order);
 
        switch (sensor_type) {
                case SENSOR_TYPE_ACCELEROMETER:
+                       if (sensor_info[s].quirks & QUIRK_NOISY)
+                               denoise(&sensor_info[s], data, 3);
                        break;
 
                case SENSOR_TYPE_MAGNETIC_FIELD:
                        calibrate_compass (data, &sensor_info[s], get_timestamp());
+                       if (sensor_info[s].quirks & QUIRK_NOISY)
+                               denoise(&sensor_info[s], data, 3);
                        break;
 
                case SENSOR_TYPE_GYROSCOPE:
                case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
                        calibrate_gyro(data, &sensor_info[s]);
+                       if (sensor_info[s].quirks & QUIRK_NOISY)
+                               denoise(&sensor_info[s], data, 3);
                        break;
 
                case SENSOR_TYPE_LIGHT:
@@ -207,6 +277,9 @@ static int finalize_sample_default(int s, struct sensors_event_t* data)
                        /* Only keep two decimals for these readings */
                        data->data[0] = 0.01 * ((int) (data->data[0] * 100));
 
+                       /* ... fall through ... */
+
+               case SENSOR_TYPE_PROXIMITY:
                        /*
                         * These are on change sensors ; drop the sample if it
                         * has the same value as the previously reported one.
@@ -244,7 +317,7 @@ static int finalize_sample_ISH(int s, struct sensors_event_t* data)
        float pitch, roll, yaw;
 
        /* Swap fields if we have a custom channel ordering on this sensor */
-       if (sensor_info[s].flags & FLAG_FIELD_ORDERING)
+       if (sensor_info[s].quirks & QUIRK_FIELD_ORDERING)
                reorder_fields(data->data, sensor_info[s].order);
 
        if (sensor_type == SENSOR_TYPE_ORIENTATION) {