GMIN-3044: Implement a simple filtering routine for noisy sensors

Keep a window of samples worth a second, and output the moving
average of the last recorded samples in order to smooth out
readings from extremely noisy sensors such as the BMM050 magneto.

We may want to use more advanced filters in the future.

Issue: GMIN-3044

Change-Id: I10bc0ddb960ed31e89228bc69fcf17f38fa85b97
Signed-off-by: Patrick Porlan <patrick.porlan@intel.com>

diff --git a/common.h b/common.h
index 63b5cb0..25a5049 100644 (file)
--- a/common.h
+++ b/common.h
@@ -184,6 +184,12 @@ struct sensor_info_t
         * a sample containing 4 fields.
         */
        unsigned char order[MAX_CHANNELS];
+
+       /* A few variables used for data filtering */
+       float *history;         /* Working buffer containing recorded samples */
+       int history_size;       /* Number of recorded samples                 */
+       int history_entries;    /* How many of these are initialized          */
+       int history_index;      /* Index of sample to evict next time         */
 };
 
 /* Reference a few commonly used variables... */

diff --git a/control.c b/control.c
index 90bfde9..1eff290 100644 (file)
--- a/control.c
+++ b/control.c
@@ -558,6 +558,12 @@ int sensor_activate(int s, int enabled)
                                close(dev_fd);
                                device_fd[dev_num] = -1;
                        }
+
+               /* If we recorded a trail of samples for filtering, delete it */
+               if (sensor_info[s].history) {
+                       free(sensor_info[s].history);
+                       sensor_info[s].history = NULL;
+               }
                return 0;
        }
 

diff --git a/transform.c b/transform.c
index e8fa707..ed66613 100644 (file)
--- a/transform.c
+++ b/transform.c
@@ -188,6 +188,62 @@ 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;
+
+       /* We're recording 1s worth of samples ; need suitable sampling rate */
+       if (sampling_rate < 1)
+               return;
+
+       /* Reset history if a new sampling rate is detected */
+       if (si->history_size != sampling_rate) {
+               si->history_size = sampling_rate;
+               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... */
+
+       /* Populate beginning of array as we go */
+       if (si->history_entries < si->history_size) {
+               for (f=0; f<num_fields; f++)
+                       si->history[si->history_entries * num_fields + f] =
+                               data->data[f];
+
+               si->history_entries++;
+       }
+
+       /* Once we get enough data, start filtering */
+       if (si->history_entries == si->history_size) {
+
+               /* For now simply compute a mobile mean */
+               for (f=0; f<num_fields; f++) {
+                       total = 0;
+
+                       for (i=0; i<si->history_size; i++)
+                               total += si->history[i * num_fields + f];
+
+                       si->history[si->history_index * num_fields + f] =
+                               data->data[f];
+
+                       /* Output filtered data */
+                       data->data[f] = total / si->history_size;
+               }
+
+               /* Update our rolling index (next evicted cell) */
+               si->history_index = (si->history_index + 1) % si->history_size;
+       }
+}
+
+
 static int finalize_sample_default(int s, struct sensors_event_t* data)
 {
        int i           = sensor_info[s].catalog_index;
@@ -203,6 +259,8 @@ static int finalize_sample_default(int s, struct sensors_event_t* data)
 
                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: