common: Add magnetometer online calibration.

Code for hard iron calibration: Every seconds (or faster if enough
samples), find a sphere that fit the compass data.

Based on Android code.

BRANCH=smaug
BUG=chrome-os-partner:39900
TEST=Check hard-iron bias is removed. Works better outside.

Change-Id: Iab479d5113b6560b4f01b0fd87373d2eecdb9b54
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/299583
Reviewed-by: Anton Staaf <robotboy@chromium.org>

4 files changed, 50 insertions, 17 deletions

diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index d32ac3c582..e248203098 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -352,6 +352,9 @@ static int set_data_rate(const struct motion_sensor_t *s,
 	int ret, val, normalized_rate;
 	uint8_t ctrl_reg, reg_val;
 	struct accelgyro_saved_data_t *data = BMI160_GET_SAVED_DATA(s);
+#ifdef CONFIG_MAG_BMI160_BMM150
+	struct mag_cal_t              *moc = BMM150_CAL(s);
+#endif
 
 	if (rate == 0) {
 #ifdef CONFIG_ACCEL_FIFO
@@ -363,6 +366,10 @@ static int set_data_rate(const struct motion_sensor_t *s,
 				 BMI160_CMD_MODE_SUSPEND(s->type));
 		msleep(3);
 		data->odr = 0;
+#ifdef CONFIG_MAG_BMI160_BMM150
+		if (s->type == MOTIONSENSE_TYPE_MAG)
+			moc->batch_size = 0;
+#endif
 		return ret;
 	} else if (data->odr == 0) {
 		/* back from suspend mode. */
@@ -430,6 +437,22 @@ static int set_data_rate(const struct motion_sensor_t *s,
 	/* Now that we have set the odr, update the driver's value. */
 	data->odr = normalized_rate;
 
+#ifdef CONFIG_MAG_BMI160_BMM150
+	if (s->type == MOTIONSENSE_TYPE_MAG) {
+		/* Reset the calibration */
+		init_mag_cal(moc);
+		/*
+		 * We need at least MIN_BATCH_SIZE amd we must have collected
+		 * for at least MIN_BATCH_WINDOW_US.
+		 * Given odr is in mHz, multiply by 1000x
+		 */
+		moc->batch_size = MAX(
+			MAG_CAL_MIN_BATCH_SIZE,
+			(data->odr * 1000) / (MAG_CAL_MIN_BATCH_WINDOW_US));
+		CPRINTS("Batch size: %d", moc->batch_size);
+	}
+#endif
+
 #ifdef CONFIG_ACCEL_FIFO
 	/*
 	 * FIFO start collecting events.
diff --git a/driver/accelgyro_bmi160.h b/driver/accelgyro_bmi160.h
index 6c57a3c4d3..4c98644691 100644
--- a/driver/accelgyro_bmi160.h
+++ b/driver/accelgyro_bmi160.h
@@ -438,7 +438,7 @@ struct bmi160_drv_data_t {
 	uint8_t              enabled_activities;
 	uint8_t              disabled_activities;
 #ifdef CONFIG_MAG_BMI160_BMM150
-	struct bmm150_comp_registers comp_regs;
+	struct bmm150_private_data compass;
 #endif
 };
 
diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c
index 83f650213b..0f8bc8b102 100644
--- a/driver/mag_bmm150.c
+++ b/driver/mag_bmm150.c
@@ -79,6 +79,7 @@ int bmm150_init(const struct motion_sensor_t *s)
 	int ret;
 	int val;
 	struct bmm150_comp_registers *regs = BMM150_COMP_REG(s);
+	struct mag_cal_t             *moc = BMM150_CAL(s);
 
 	/* Set the compass from Suspend to Sleep */
 	ret = raw_mag_write8(s->addr, BMM150_PWR_CTRL, BMM150_PWR_ON);
@@ -129,6 +130,8 @@ int bmm150_init(const struct motion_sensor_t *s)
 	ret = raw_mag_write8(s->addr, BMM150_OP_CTRL,
 			BMM150_OP_MODE_FORCED << BMM150_OP_MODE_OFFSET);
 
+	init_mag_cal(moc);
+	moc->radius = 0.0f;
 	return ret;
 }
 
@@ -207,7 +210,7 @@ void bmm150_normalize(const struct motion_sensor_t *s,
 {
 	uint16_t r;
 	vector_3_t raw;
-	struct bmm150_comp_registers *regs = BMM150_COMP_REG(s);
+	struct mag_cal_t *cal = BMM150_CAL(s);
 
 	/* X and Y are two's complement 13 bits vectors */
 	raw[X] = ((int16_t)(data[0] | (data[1] << 8))) >> 3;
@@ -220,27 +223,29 @@ 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] += regs->offset[X];
-	v[Y] += regs->offset[Y];
-	v[Z] += regs->offset[Z];
+	mag_cal_update(cal, v);
+
+	v[X] += cal->bias[X];
+	v[Y] += cal->bias[Y];
+	v[Z] += cal->bias[Z];
 }
 
 int bmm150_set_offset(const struct motion_sensor_t *s,
 		      const vector_3_t offset)
 {
-	struct bmm150_comp_registers *regs = BMM150_COMP_REG(s);
-	regs->offset[X] = offset[X];
-	regs->offset[Y] = offset[Y];
-	regs->offset[Z] = offset[Z];
+	struct mag_cal_t *cal = BMM150_CAL(s);
+	cal->bias[X] = offset[X];
+	cal->bias[Y] = offset[Y];
+	cal->bias[Z] = offset[Z];
 	return EC_SUCCESS;
 }
 
 int bmm150_get_offset(const struct motion_sensor_t *s,
 		      vector_3_t offset)
 {
-	struct bmm150_comp_registers *regs = BMM150_COMP_REG(s);
-	offset[X] = regs->offset[X];
-	offset[Y] = regs->offset[Y];
-	offset[Z] = regs->offset[Z];
+	struct mag_cal_t *cal = BMM150_CAL(s);
+	offset[X] = cal->bias[X];
+	offset[Y] = cal->bias[Y];
+	offset[Z] = cal->bias[Z];
 	return EC_SUCCESS;
 }
diff --git a/driver/mag_bmm150.h b/driver/mag_bmm150.h
index f3a0a38f50..4a6af7ee0d 100644
--- a/driver/mag_bmm150.h
+++ b/driver/mag_bmm150.h
@@ -9,6 +9,7 @@
 #define __CROS_EC_MAG_BMM150_H
 
 #include "accelgyro.h"
+#include "mag_cal.h"
 
 #define BMM150_ADDR0             0x20
 #define BMM150_ADDR1             0x22
@@ -86,13 +87,17 @@ struct bmm150_comp_registers {
 	int8_t       dig_xy2;
 
 	uint16_t     dig_xyz1;
-
-  /* Factory or online calibration */
-	int16_t      offset[3];
 };
 
+struct bmm150_private_data {
+	struct bmm150_comp_registers comp;
+	struct mag_cal_t             cal;
+};
 #define BMM150_COMP_REG(_s) \
-	(&BMI160_GET_DATA(_s)->comp_regs)
+	(&BMI160_GET_DATA(_s)->compass.comp)
+
+#define BMM150_CAL(_s) \
+	(&BMI160_GET_DATA(_s)->compass.cal)
 
 /* Specific initialization of BMM150 when behing BMI160 */
 int bmm150_init(const struct motion_sensor_t *s);