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.
Change-Id: I3685a4aa76a7366e318e83a9149cfde7b23757a1
Signed-off-by: Rosa Chow <rchow@invensense.com>
Signed-off-by: Jinkyu Song <jksong@sta.samsung.com>
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 @@
/* - 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 @@
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 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 @@
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 @@
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 @@
}
}
+ 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] /
