Integrate Noise Filter Providied by Yamaha

This patch contains a software filter to reduce compass noise observed in LTE devices.
A new set of filter parameters provided by Yamaha has been applied.
A median filter has replaced the lowpass filter.
Filter parameters for Rev 0.2 daughter board have been applied.

Signed-off-by: Rosa Chow <rchow@invensense.com>
Signed-off-by: Jinkyu Song <jksong@sta.samsung.com>

Change-Id: I1662ce146373d302e2af50d0d9708d904c188560

3 files changed, 189 insertions, 2 deletions

diff --git a/mlsdk/mllite/compass.c b/mlsdk/mllite/compass.c
index e1a91f3..4bc469a 100644
--- a/mlsdk/mllite/compass.c
+++ b/mlsdk/mllite/compass.c
@@ -38,7 +38,7 @@
 /* ------------------ */
 
 #include <string.h>
-
+#include <stdlib.h>
 #include "compass.h"
 
 #include "ml.h"
@@ -77,7 +77,139 @@
 /* - Functions. - */
 /* -------------- */
 
-/** 
+static float square(float data)
+{
+    return data * data;
+}
+
+static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise)
+{
+    int i;
+
+    adap_filter->num = 0;
+    adap_filter->index = 0;
+    adap_filter->noise = noise;
+    adap_filter->len = len;
+
+    for (i = 0; i < adap_filter->len; ++i) {
+        adap_filter->sequence[i] = 0;
+    }
+}
+
+static int cmpfloat(const void *p1, const void *p2)
+{
+    return *(float*)p1 - *(float*)p2;
+}
+
+
+static float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in)
+{
+    float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN];
+    int i;
+
+    if (adap_filter->len <= 1) {
+        return in;
+    }
+    if (adap_filter->num < adap_filter->len) {
+        adap_filter->sequence[adap_filter->index++] = in;
+        adap_filter->num++;
+        return in;
+    }
+    if (adap_filter->len <= adap_filter->index) {
+        adap_filter->index = 0;
+    }
+    adap_filter->sequence[adap_filter->index++] = in;
+
+    avg = 0;
+    for (i = 0; i < adap_filter->len; i++) {
+        avg += adap_filter->sequence[i];
+    }
+    avg /= adap_filter->len;
+
+    memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float));
+    qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat);
+    median = sorted[adap_filter->len/2];
+
+    sum = 0;
+    for (i = 0; i < adap_filter->len; i++) {
+        sum += square(avg - adap_filter->sequence[i]);
+    }
+    sum /= adap_filter->len;
+
+    if (sum <= adap_filter->noise) {
+        return median;
+    }
+
+    return ((in - avg) * (sum - adap_filter->noise) / sum + avg);
+}
+
+static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold)
+{
+    thresh_filter->threshold = threshold;
+    thresh_filter->last = 0;
+}
+
+static float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in)
+{
+    if (in < thresh_filter->last - thresh_filter->threshold
+            || thresh_filter->last + thresh_filter->threshold < in) {
+        thresh_filter->last = in;
+        return in;
+    }
+    else {
+        return thresh_filter->last;
+    }
+}
+
+static int init(yas_filter_handle_t *t)
+{
+    float noise[] = {
+        YAS_DEFAULT_FILTER_NOISE,
+        YAS_DEFAULT_FILTER_NOISE,
+        YAS_DEFAULT_FILTER_NOISE,
+    };
+    int i;
+
+    if (t == NULL) {
+        return -1;
+    }
+
+    for (i = 0; i < 3; i++) {
+        adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]);
+        thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH);
+    }
+
+    return 0;
+}
+
+static int update(yas_filter_handle_t *t, float *input, float *output)
+{
+    int i;
+
+    if (t == NULL || input == NULL || output == NULL) {
+        return -1;
+    }
+
+    for (i = 0; i < 3; i++) {
+        output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]);
+        output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]);
+    }
+
+    return 0;
+}
+
+int yas_filter_init(yas_filter_if_s *f)
+{
+    if (f == NULL) {
+        return -1;
+    }
+    f->init = init;
+    f->update = update;
+
+    return 0;
+}
+
+/**
  *  @brief  Used to determine if a compass is
  *          configured and used by the MPL.
  *  @return INV_SUCCESS if the compass is present.
diff --git a/mlsdk/mllite/compass.h b/mlsdk/mllite/compass.h
index ebdc816..9201580 100644
--- a/mlsdk/mllite/compass.h
+++ b/mlsdk/mllite/compass.h
@@ -37,10 +37,38 @@ extern "C" {
     /* - Defines. - */
     /* ------------ */
 
+#define YAS_MAX_FILTER_LEN (20)
+#define YAS_DEFAULT_FILTER_LEN (20)
+#define YAS_DEFAULT_FILTER_THRESH (700) /* 700 nT */
+#define YAS_DEFAULT_FILTER_NOISE (5000 * 5000) /* standard deviation 5000 nT */
+
     /* --------------- */
     /* - Structures. - */
     /* --------------- */
 
+struct yas_adaptive_filter {
+    int num;
+    int index;
+    int len;
+    float noise;
+    float sequence[YAS_MAX_FILTER_LEN];
+};
+
+struct yas_thresh_filter {
+    float threshold;
+    float last;
+};
+
+typedef struct {
+    struct yas_adaptive_filter adap_filter[3];
+    struct yas_thresh_filter thresh_filter[3];
+} yas_filter_handle_t;
+
+typedef struct {
+    int (*init)(yas_filter_handle_t *t);
+    int (*update)(yas_filter_handle_t *t, float *input, float *output);
+} yas_filter_if_s;
+
     /* --------------------- */
     /* - Function p-types. - */
     /* --------------------- */
@@ -56,6 +84,8 @@ extern "C" {
     inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val);
     inv_error_t inv_compass_read_scale(long *val);
 
+    int yas_filter_init(yas_filter_if_s *f);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/mlsdk/mllite/mlsupervisor.c b/mlsdk/mllite/mlsupervisor.c
index 512eae8..cbd4ac4 100644
--- a/mlsdk/mllite/mlsupervisor.c
+++ b/mlsdk/mllite/mlsupervisor.c
@@ -60,6 +60,9 @@ static int accCount = 0;
 static int compassCalStableCount = 0;
 static int compassCalCount = 0;
 
+static yas_filter_if_s f;
+static yas_filter_handle_t handle;
+
 #define SUPERVISOR_DEBUG 0
 
 struct inv_supervisor_cb_obj ml_supervisor_cb = { 0 };
@@ -75,6 +78,10 @@ void inv_init_sensor_fusion_supervisor(void)
     accCount = 0;
     compassCalStableCount = 0;
     compassCalCount = 0;
+
+    yas_filter_init(&f);
+    f.init(&handle);
+
 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
 	defined CONFIG_MPU_SENSORS_MPU6050B1
     if (inv_compass_present()) {
@@ -358,6 +365,10 @@ inv_error_t inv_accel_compass_supervisor(void)
     long accSF = 1073741824;
     static double magFB = 0;
     long magSensorData[3];
+    float fcin[3];
+    float fcout[3];
+    
+
     if (inv_compass_present()) {    /* check for compass data */
         int i, j;
         long long tmp[3] = { 0 };
@@ -420,6 +431,20 @@ inv_error_t inv_accel_compass_supervisor(void)
                     }
                 }
 
+                if (inv_get_compass_id() == COMPASS_ID_YAS530)
+                {
+                    fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f);
+                    fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f);
+                    fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f);
+
+                    f.update(&handle, fcin, fcout);
+
+                    inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f);
+                    inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f);
+                    inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f);
+                }
+
+
                 if (SUPERVISOR_DEBUG) {
                     MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n",
                              (float)inv_obj.compass_calibrated_data[0] /