driver: bmm150: Add ability to smooth data from compass.

To fight the noise inside the device,
implement a low bypass filter using the exponential moving average method.
http://en.wikipedia.org/wiki/Moving_average#Exponential_moving_average

Define only for axis that need it.

BRANCH=smaug
BUG=chrome-os-partner:45436
TEST=Using androsensor, check the data on the X and Z axis is smoother.

Change-Id: I1724375d0ed92dfe9066db5d710ece30af56d578
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/309966
Reviewed-by: Alec Berg <alecaberg@chromium.org>

6 files changed, 99 insertions, 4 deletions

diff --git a/board/ryu/board.h b/board/ryu/board.h
index 5bcad1d487..4676dc2b8a 100644
--- a/board/ryu/board.h
+++ b/board/ryu/board.h
@@ -176,6 +176,10 @@
 	 (1 << CONFIG_GESTURE_SENSOR_BATTERY_TAP))
 #define CONFIG_MAG_CALIBRATE
 #define CONFIG_MAG_BMI160_BMM150
+/* chrome-os-partner:45436: smooth data on Y and Z axis. */
+#define CONFIG_MAG_Y_SMOOTH_WEIGHT_INV 15
+#define CONFIG_MAG_Z_SMOOTH_WEIGHT_INV 5
+
 #define CONFIG_ALS_SI114X  0x41
 #define CONFIG_ACCELGYRO_BMI160_INT_EVENT TASK_EVENT_CUSTOM(4)
 #define CONFIG_ALS_SI114X_INT_EVENT       TASK_EVENT_CUSTOM(8)
diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c
index 665b86e58d..f60e57b0ba 100644
--- a/driver/mag_bmm150.c
+++ b/driver/mag_bmm150.c
@@ -204,6 +204,23 @@ void bmm150_temp_compensate_z(const struct motion_sensor_t *s,
 		comp[Z] = BMM150_OVERFLOW_OUTPUT;
 }
 
+#ifdef CONFIG_MAG_X_SMOOTH_WEIGHT_INV
+	DECLARE_BMI160_SMOOTH_FUNC(X)
+#else
+	DECLARE_BMI160_NO_SMOOTH_FUNC(X)
+#endif
+#ifdef CONFIG_MAG_Y_SMOOTH_WEIGHT_INV
+	DECLARE_BMI160_SMOOTH_FUNC(Y)
+#else
+	DECLARE_BMI160_NO_SMOOTH_FUNC(Y)
+#endif
+#ifdef CONFIG_MAG_Z_SMOOTH_WEIGHT_INV
+	DECLARE_BMI160_SMOOTH_FUNC(Z)
+#else
+	DECLARE_BMI160_NO_SMOOTH_FUNC(Z)
+#endif
+
+
 void bmm150_normalize(const struct motion_sensor_t *s,
 		      vector_3_t v,
 		      uint8_t *data)
@@ -223,6 +240,10 @@ void bmm150_normalize(const struct motion_sensor_t *s,
 
 	bmm150_temp_compensate_xy(s, raw, v, r);
 	bmm150_temp_compensate_z(s, raw, v, r);
+	v[X] = bmi160_smooth_axis_X(s, v[X]);
+	v[Y] = bmi160_smooth_axis_Y(s, v[Y]);
+	v[Z] = bmi160_smooth_axis_Z(s, v[Z]);
+
 	mag_cal_update(cal, v);
 
 	v[X] += cal->bias[X];
diff --git a/driver/mag_bmm150.h b/driver/mag_bmm150.h
index a76c68cf3d..984ec0c492 100644
--- a/driver/mag_bmm150.h
+++ b/driver/mag_bmm150.h
@@ -91,16 +91,53 @@ struct bmm150_comp_registers {
 	uint16_t     dig_xyz1;
 };
 
-struct bmm150_private_data {
-	struct bmm150_comp_registers comp;
-	struct mag_cal_t             cal;
-};
 #define BMM150_COMP_REG(_s) \
 	(&BMI160_GET_DATA(_s)->compass.comp)
 
 #define BMM150_CAL(_s) \
 	(&BMI160_GET_DATA(_s)->compass.cal)
 
+/* compass data may need to be filtered to remove internal noise */
+#define BMI160_SMOOTH_ACC_NAME(_axis) CONCAT2(smooth_acc_, _axis)
+#define DECLATE_BMI_SMOOTH_AXIS_VAR(_axis) \
+	fp_t BMI160_SMOOTH_ACC_NAME(_axis)
+#define BMI160_SMOOTH_GET_ACC(_s, _axis) \
+	(BMI160_GET_DATA(_s)->compass.BMI160_SMOOTH_ACC_NAME(_axis))
+
+#define BMI160_SMOOTH_ALPA(_axis) \
+	 (fp_div(INT_TO_FP(1), \
+		 INT_TO_FP(CONCAT3(CONFIG_MAG_, _axis, _SMOOTH_WEIGHT_INV))))
+
+#define DECLARE_BMI160_SMOOTH_FUNC(_axis) \
+	static inline int CONCAT2(bmi160_smooth_axis_, _axis)( \
+			const struct motion_sensor_t \
+			*s, int raw_data) { \
+		EXP_MOVING_AVG(BMI160_SMOOTH_GET_ACC(s, _axis), \
+			       BMI160_SMOOTH_ALPA(_axis), raw_data); \
+		return BMI160_SMOOTH_GET_ACC(s, _axis); \
+	}
+
+#define DECLARE_BMI160_NO_SMOOTH_FUNC(_axis) \
+	static inline int CONCAT2(bmi160_smooth_axis_, _axis)( \
+			const struct motion_sensor_t \
+			*s, int raw_data) { \
+		return raw_data; \
+	}
+
+struct bmm150_private_data {
+	struct bmm150_comp_registers comp;
+	struct mag_cal_t             cal;
+#ifdef CONFIG_MAG_X_SMOOTH_WEIGHT_INV
+	DECLATE_BMI_SMOOTH_AXIS_VAR(X);
+#endif
+#ifdef CONFIG_MAG_Y_SMOOTH_WEIGHT_INV
+	DECLATE_BMI_SMOOTH_AXIS_VAR(Y);
+#endif
+#ifdef CONFIG_MAG_Z_SMOOTH_WEIGHT_INV
+	DECLATE_BMI_SMOOTH_AXIS_VAR(Z);
+#endif
+};
+
 /* Specific initialization of BMM150 when behing BMI160 */
 int bmm150_init(const struct motion_sensor_t *s);
 
@@ -115,4 +152,5 @@ int bmm150_set_offset(const struct motion_sensor_t *s,
 int bmm150_get_offset(const struct motion_sensor_t *s,
 		      vector_3_t   offset);
 
+
 #endif /* __CROS_EC_MAG_BMM150_H */
diff --git a/include/config.h b/include/config.h
index 4535896127..29cde1a7b9 100644
--- a/include/config.h
+++ b/include/config.h
@@ -1094,6 +1094,19 @@
 /* Presence of a Bosh Sensortec BMM150 magnetometer behind a BMI160. */
 #undef CONFIG_MAG_BMI160_BMM150
 
+/*
+ * Smooth magnetometer input:
+ * Defined the number of samples a given sample have effect on.
+ * Using expodential moving avarage method, this represenat 1/alpha
+ * where alpha is used as:
+ * out = out * (1 - alpha) + alpha * in.
+ *
+ * It is defined in the axis of the compass, before device rotation.
+ */
+#undef CONFIG_MAG_X_SMOOTH_WEIGHT_INV
+#undef CONFIG_MAG_Y_SMOOTH_WEIGHT_INV
+#undef CONFIG_MAG_Z_SMOOTH_WEIGHT_INV
+
 /* Support MKBP event */
 #undef CONFIG_MKBP_EVENT
 
diff --git a/include/mag_cal.h b/include/mag_cal.h
index 76410a193e..cacddc3c71 100644
--- a/include/mag_cal.h
+++ b/include/mag_cal.h
@@ -14,7 +14,13 @@
 
 #define MAG_CAL_MAX_SAMPLES 0xffff
 #define MAG_CAL_MIN_BATCH_WINDOW_US    SECOND
+#if defined CONFIG_MAG_X_SMOOTH_WEIGHT_INV || \
+	defined CONFIG_MAG_Y_SMOOTH_WEIGHT_INV || \
+	defined CONFIG_MAG_Z_SMOOTH_WEIGHT_INV
+#define MAG_CAL_MIN_BATCH_SIZE      50      /* samples */
+#else
 #define MAG_CAL_MIN_BATCH_SIZE      25      /* samples */
+#endif
 
 struct mag_cal_t {
 	/*
diff --git a/include/math_util.h b/include/math_util.h
index 1116a6566c..429e0ea3c7 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -150,4 +150,17 @@ void rotate(const vector_3_t v, const matrix_3x3_t R, vector_3_t res);
  */
 void rotate_inv(const vector_3_t v, const matrix_3x3_t R, vector_3_t res);
 
+/*
+ * Expodential Moving average:
+ *
+ * Based on
+ * https://en.wikipedia.org/wiki/Moving_average#Exponential_moving_average
+ *
+ * Remove noise from data.
+ * _alpha is a fp_t but _avg and _data are not.
+ */
+#define EXP_MOVING_AVG(_avg, _alpha, _data) \
+	((_avg) = (INT_TO_FP(1) - (_alpha)) * (_avg) + (_alpha) * (_data))
+
+
 #endif /* __CROS_MATH_UTIL_H */