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>

19 files changed, 707 insertions, 20 deletions

diff --git a/board/ryu/board.h b/board/ryu/board.h
index f765d1258c..833a131eb4 100644
--- a/board/ryu/board.h
+++ b/board/ryu/board.h
@@ -169,6 +169,7 @@
 #define CONFIG_GESTURE_DETECTION_MASK \
 	((1 << CONFIG_GESTURE_SIGMO) | \
 	 (1 << CONFIG_GESTURE_SENSOR_BATTERY_TAP))
+#define CONFIG_MAG_CALIBRATE
 #define CONFIG_MAG_BMI160_BMM150
 #define CONFIG_ALS_SI114X  0x41
 #define CONFIG_ACCELGYRO_BMI160_INT_EVENT TASK_EVENT_CUSTOM(4)
diff --git a/board/ryu/ec.tasklist b/board/ryu/ec.tasklist
index 19438fef29..9d49b2c126 100644
--- a/board/ryu/ec.tasklist
+++ b/board/ryu/ec.tasklist
@@ -22,7 +22,7 @@
 		    SMALLER_TASK_STACK_SIZE) \
 	TASK_NOTEST(LIGHTBAR, lightbar_task, NULL, LARGER_TASK_STACK_SIZE) \
 	TASK_ALWAYS(CHARGER, charger_task, NULL, TASK_STACK_SIZE) \
-	TASK_NOTEST(MOTIONSENSE, motion_sense_task, NULL, LARGER_TASK_STACK_SIZE) \
+	TASK_NOTEST(MOTIONSENSE, motion_sense_task, NULL, VENTI_TASK_STACK_SIZE) \
 	TASK_NOTEST(CHIPSET, chipset_task, NULL, LARGER_TASK_STACK_SIZE) \
 	TASK_NOTEST(HOSTCMD, host_command_task, NULL, VENTI_TASK_STACK_SIZE) \
 	TASK_ALWAYS(CONSOLE, console_task, NULL, LARGER_TASK_STACK_SIZE) \
diff --git a/common/build.mk b/common/build.mk
index cf2e1403a2..2a44c5124b 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -57,6 +57,7 @@ common-$(CONFIG_LID_ANGLE)+=motion_lid.o math_util.o
 common-$(CONFIG_LID_ANGLE_UPDATE)+=lid_angle.o
 common-$(CONFIG_LID_SWITCH)+=lid_switch.o
 common-$(CONFIG_LPC)+=acpi.o port80.o
+common-$(CONFIG_MAG_CALIBRATE)+= mag_cal.o math_util.o vec3.o mat33.o mat44.o
 common-$(CONFIG_MKBP_EVENT)+=mkbp_event.o
 common-$(CONFIG_ONEWIRE)+=onewire.o
 common-$(CONFIG_POWER_BUTTON)+=power_button.o
diff --git a/common/mag_cal.c b/common/mag_cal.c
new file mode 100644
index 0000000000..3ac2a7bcea
--- /dev/null
+++ b/common/mag_cal.c
@@ -0,0 +1,221 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "common.h"
+#include "console.h"
+#include "mag_cal.h"
+#include "mat33.h"
+#include "mat44.h"
+
+#include "math.h"
+#include "math_util.h"
+#include "util.h"
+
+/* Data from sensor is in 16th of uT */
+#define MAG_CAL_RAW_UT      16
+
+#define MAX_EIGEN_RATIO     25.0f
+#define MAX_EIGEN_MAG       (80.0f * MAG_CAL_RAW_UT)
+#define MIN_EIGEN_MAG       (10.0f * MAG_CAL_RAW_UT)
+
+#define MAX_FIT_MAG         MAX_EIGEN_MAG
+#define MIN_FIT_MAG         MIN_EIGEN_MAG
+
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+#define PRINTF_FLOAT(x)  ((int)((x) * 100.0f))
+
+/*
+ * eigen value magnitude and ratio test
+ *
+ * Using the magnetometer information, caculate the 3 eigen values/vectors
+ * for the transformation. Check the eigen values are sane.
+ */
+static int moc_eigen_test(struct mag_cal_t *moc)
+{
+	mat33_t S;
+	vec3_t eigenvals;
+	mat33_t eigenvecs;
+	float evmax, evmin, evmag;
+	int eigen_pass;
+
+	/* covariance matrix */
+	S[0][0] = moc->acc[0][0] - moc->acc[0][3] * moc->acc[0][3];
+	S[0][1] = S[1][0] = moc->acc[0][1] - moc->acc[0][3] * moc->acc[1][3];
+	S[0][2] = S[2][0] = moc->acc[0][2] - moc->acc[0][3] * moc->acc[2][3];
+	S[1][1] = moc->acc[1][1] - moc->acc[1][3] * moc->acc[1][3];
+	S[1][2] = S[2][1] = moc->acc[1][2] - moc->acc[1][3] * moc->acc[2][3];
+	S[2][2] = moc->acc[2][2] - moc->acc[2][3] * moc->acc[2][3];
+
+	mat33_get_eigenbasis(S, eigenvals, eigenvecs);
+
+	evmax = (eigenvals[X] > eigenvals[Y]) ? eigenvals[X] : eigenvals[Y];
+	evmax = (eigenvals[Z] > evmax) ? eigenvals[Z] : evmax;
+
+	evmin = (eigenvals[X] < eigenvals[Y]) ? eigenvals[X] : eigenvals[Y];
+	evmin = (eigenvals[Z] < evmin) ? eigenvals[Z] : evmin;
+
+	evmag = sqrtf(eigenvals[X] + eigenvals[Y] + eigenvals[Z]);
+
+	eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax)
+		&& (evmag > MIN_EIGEN_MAG)
+		&& (evmag < MAX_EIGEN_MAG);
+
+#if 0
+	CPRINTF("mag eigenvalues: (%d %d %d), ",
+		PRINTF_FLOAT(eigenvals[X]),
+		PRINTF_FLOAT(eigenvals[Y]),
+		PRINTF_FLOAT(eigenvals[Z]));
+
+	CPRINTF("ratio %d, mag %d: pass %d\r\n",
+		PRINTF_FLOAT(evmax / evmin),
+		PRINTF_FLOAT(evmag),
+		PRINTF_FLOAT(eigen_pass));
+#endif
+
+	return eigen_pass;
+}
+
+/*
+ * Kasa sphere fitting with normal equation
+ */
+static int moc_fit(struct mag_cal_t *moc, vec3_t bias, float *radius)
+{
+	size4_t pivot;
+	vec4_t out;
+	int success = 0;
+
+	/*
+	 * To reduce stack size, moc->acc is A,
+	 * moc->acc_w is b: we are looking for out, where:
+	 *
+	 *    A    *   out   =    b
+	 * (4 x 4)   (4 x 1)   (4 x 1)
+	 */
+	/* complete the matrix: */
+	moc->acc[1][0] = moc->acc[0][1];
+	moc->acc[2][0] = moc->acc[0][2];
+	moc->acc[2][1] = moc->acc[1][2];
+	moc->acc[3][0] = moc->acc[0][3];
+	moc->acc[3][1] = moc->acc[1][3];
+	moc->acc[3][2] = moc->acc[2][3];
+	moc->acc[3][3] = 1.0f;
+
+	moc->acc_w[X] *= -1;
+	moc->acc_w[Y] *= -1;
+	moc->acc_w[Z] *= -1;
+	moc->acc_w[W] *= -1;
+
+	mat44_decompose_lup(moc->acc, pivot);
+
+	mat44_solve(moc->acc, out, moc->acc_w, pivot);
+
+	/*
+	 * spherei is defined by:
+	 * (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
+	 *
+	 * Where r is:
+	 * xc = -out[X] / 2, yc = -out[Y] / 2, zc = -out[Z] / 2
+	 * r = sqrt(xc^2 + yc^2 + zc^2 - out[W])
+	 */
+
+	memcpy(bias, out, sizeof(vec3_t));
+	vec3_scalar_mul(bias, -0.5f);
+
+	*radius = sqrtf(vec3_dot(bias, bias) - out[W]);
+
+#if 0
+	CPRINTF("mag cal: bias (%d, %d, %d), R %d uT\n",
+		PRINTF_FLOAT(bias[X] / MAG_CAL_RAW_UT),
+		PRINTF_FLOAT(bias[Y] / MAG_CAL_RAW_UT),
+		PRINTF_FLOAT(bias[Z] / MAG_CAL_RAW_UT),
+		PRINTF_FLOAT(*radius / MAG_CAL_RAW_UT));
+#endif
+
+	/* TODO (menghsuan): bound on bias as well? */
+	if (*radius > MIN_FIT_MAG && *radius < MAX_FIT_MAG)
+		success = 1;
+
+	return success;
+}
+
+void init_mag_cal(struct mag_cal_t *moc)
+{
+	memset(moc->acc, 0, sizeof(moc->acc));
+	memset(moc->acc_w, 0, sizeof(moc->acc_w));
+	moc->nsamples = 0;
+}
+
+int mag_cal_update(struct mag_cal_t *moc, const vector_3_t v)
+{
+	int new_bias = 0;
+
+	/* 1. run accumulators */
+	float w = v[X] * v[X] + v[Y] * v[Y] + v[Z] * v[Z];
+
+	moc->acc[0][3] += v[X];
+	moc->acc[1][3] += v[Y];
+	moc->acc[2][3] += v[Z];
+	moc->acc_w[W] += w;
+
+	moc->acc[0][0] += v[X] * v[X];
+	moc->acc[0][1] += v[X] * v[Y];
+	moc->acc[0][2] += v[X] * v[Z];
+	moc->acc_w[X] += v[X] * w;
+
+	moc->acc[1][1] += v[Y] * v[Y];
+	moc->acc[1][2] += v[Y] * v[Z];
+	moc->acc_w[Y] += v[Y] * w;
+
+	moc->acc[2][2] += v[Z] * v[Z];
+	moc->acc_w[Z] += v[Z] * w;
+
+	if (moc->nsamples < MAG_CAL_MAX_SAMPLES)
+		moc->nsamples++;
+
+	/* 2. batch has enough samples? */
+	if (moc->batch_size > 0 && moc->nsamples >= moc->batch_size) {
+		float inv = 1.0f / moc->nsamples;
+
+		moc->acc[0][3] *= inv;
+		moc->acc[1][3] *= inv;
+		moc->acc[2][3] *= inv;
+		moc->acc_w[W] *= inv;
+
+		moc->acc[0][0] *= inv;
+		moc->acc[0][1] *= inv;
+		moc->acc[0][2] *= inv;
+		moc->acc_w[X] *= inv;
+
+		moc->acc[1][1] *= inv;
+		moc->acc[1][2] *= inv;
+		moc->acc_w[Y] *= inv;
+
+		moc->acc[2][2] *= inv;
+		moc->acc_w[Z] *= inv;
+
+		/* 3. eigen test */
+		if (moc_eigen_test(moc)) {
+			vec3_t bias;
+			float radius;
+
+			/* 4. Kasa sphere fitting */
+			if (moc_fit(moc, bias, &radius)) {
+
+				moc->bias[X] = bias[X] * -1;
+				moc->bias[Y] = bias[Y] * -1;
+				moc->bias[Z] = bias[Z] * -1;
+
+				moc->radius = radius;
+
+				new_bias = 1;
+			}
+		}
+		/* 5. reset for next batch */
+		init_mag_cal(moc);
+	}
+
+	return new_bias;
+}
+
diff --git a/common/mat33.c b/common/mat33.c
new file mode 100644
index 0000000000..8317ee8a97
--- /dev/null
+++ b/common/mat33.c
@@ -0,0 +1,170 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "common.h"
+#include "mat33.h"
+#include "math.h"
+#include "util.h"
+
+#define K_EPSILON 1E-5f
+
+void init_zero_matrix(mat33_t A)
+{
+	memset(A, 0, sizeof(mat33_t));
+}
+
+void init_diagonal_matrix(mat33_t A, float x)
+{
+	size_t i;
+	init_zero_matrix(A);
+
+	for (i = 0; i < 3; ++i)
+		A[i][i] = x;
+}
+
+void mat33_scalar_mul(mat33_t A, float c)
+{
+	size_t i;
+	for (i = 0; i < 3; ++i) {
+		size_t j;
+		for (j = 0; j < 3; ++j)
+			A[i][j] *= c;
+	}
+}
+
+void mat33_swap_rows(mat33_t A, const size_t i, const size_t j)
+{
+	const size_t N = 3;
+	size_t k;
+
+	if (i == j)
+		return;
+
+	for (k = 0; k < N; ++k) {
+		float tmp = A[i][k];
+		A[i][k] = A[j][k];
+		A[j][k] = tmp;
+	}
+}
+
+/*
+ * Returns the eigenvalues and corresponding eigenvectors of the _symmetric_
+ * matrix.
+ * The i-th eigenvalue corresponds to the eigenvector in the i-th _row_ of
+ * "eigenvecs".
+ */
+void mat33_get_eigenbasis(mat33_t S, vec3_t e_vals, mat33_t e_vecs)
+{
+	const size_t N = 3;
+	size3_t ind;
+	size_t i, j, k, l, m;
+
+	for (k = 0; k < N; ++k) {
+		ind[k] = mat33_maxind(S, k);
+		e_vals[k] = S[k][k];
+	}
+
+	init_diagonal_matrix(e_vecs, 1.0f);
+
+	for (;;) {
+		float y, t, s, c, p, sum;
+		m = 0;
+		for (k = 1; k + 1 < N; ++k) {
+			if (fabsf(S[k][ind[k]]) >
+			    fabsf(S[m][ind[m]])) {
+				m = k;
+			}
+		}
+
+		k = m;
+		l = ind[m];
+		p = S[k][l];
+
+		if (fabsf(p) < K_EPSILON)
+			break;
+
+		y = (e_vals[l] - e_vals[k]) * 0.5f;
+
+		t = fabsf(y) + sqrtf(p * p + y * y);
+		s = sqrtf(p * p + t * t);
+		c = t / s;
+		s = p / s;
+		t = p * p / t;
+
+		if (y < 0.0f) {
+			s = -s;
+			t = -t;
+		}
+
+		S[k][l] = 0.0f;
+
+		e_vals[k] -= t;
+		e_vals[l] += t;
+
+		for (i = 0; i < k; ++i)
+			mat33_rotate(S, c, s, i, k, i, l);
+
+		for (i = k + 1; i < l; ++i)
+			mat33_rotate(S, c, s, k, i, i, l);
+
+		for (i = l + 1; i < N; ++i)
+			mat33_rotate(S, c, s, k, i, l, i);
+
+		for (i = 0; i < N; ++i) {
+			float tmp = c * e_vecs[k][i] - s * e_vecs[l][i];
+			e_vecs[l][i] = s * e_vecs[k][i] + c * e_vecs[l][i];
+			e_vecs[k][i] = tmp;
+		}
+
+		ind[k] = mat33_maxind(S, k);
+		ind[l] = mat33_maxind(S, l);
+
+		sum = 0.0f;
+		for (i = 0; i < N; ++i)
+			for (j = i + 1; j < N; ++j)
+				sum += fabsf(S[i][j]);
+
+		if (sum < K_EPSILON)
+			break;
+	}
+
+	for (k = 0; k < N; ++k) {
+		m = k;
+		for (l = k + 1; l < N; ++l)
+			if (e_vals[l] > e_vals[m])
+				m = l;
+
+		if (k != m) {
+			float tmp = e_vals[k];
+			e_vals[k] = e_vals[m];
+			e_vals[m] = tmp;
+
+			mat33_swap_rows(e_vecs, k, m);
+		}
+	}
+}
+
+/* index of largest off-diagonal element in row k */
+size_t mat33_maxind(mat33_t A, size_t k)
+{
+	const size_t N = 3;
+	size_t i, m = k + 1;
+
+	for (i = k + 2; i < N; ++i)
+		if (fabsf(A[k][i]) > fabsf(A[k][m]))
+			m = i;
+
+	return m;
+}
+
+void mat33_rotate(mat33_t A, float c, float s,
+		  size_t k, size_t l, size_t i, size_t j)
+{
+	float tmp = c * A[k][l] - s * A[i][j];
+	A[i][j] = s * A[k][l] + c * A[i][j];
+	A[k][l] = tmp;
+}
+
+
diff --git a/common/mat44.c b/common/mat44.c
new file mode 100644
index 0000000000..0f7f371461
--- /dev/null
+++ b/common/mat44.c
@@ -0,0 +1,85 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "common.h"
+#include "mat44.h"
+#include "math.h"
+#include "util.h"
+
+#define K_EPSILON 1E-5f
+
+void mat44_decompose_lup(mat44_t LU, size4_t pivot)
+{
+	const size_t N = 4;
+	size_t i, j, k;
+
+	for (k = 0; k < N; ++k) {
+		float max = fabsf(LU[k][k]);
+		pivot[k] = k;
+		for (j = k + 1; j < N; ++j) {
+			if (max < fabsf(LU[j][k])) {
+				max = fabsf(LU[j][k]);
+				pivot[k] = j;
+			}
+		}
+
+		if (pivot[k] != k)
+			mat44_swap_rows(LU, k, pivot[k]);
+
+		if (fabsf(LU[k][k]) < K_EPSILON)
+			continue;
+
+		for (j = k + 1; j < N; ++j)
+			LU[k][j] /= LU[k][k];
+
+		for (i = k + 1; i < N; ++i)
+			for (j = k + 1; j < N; ++j)
+				LU[i][j] -= LU[i][k] * LU[k][j];
+	}
+}
+
+void mat44_swap_rows(mat44_t A, const size_t i, const size_t j)
+{
+	const size_t N = 4;
+	size_t k;
+
+	if (i == j)
+		return;
+
+	for (k = 0; k < N; ++k) {
+		float tmp = A[i][k];
+		A[i][k] = A[j][k];
+		A[j][k] = tmp;
+	}
+}
+
+void mat44_solve(mat44_t A, vec4_t x, const vec4_t b, const size4_t pivot)
+{
+	const size_t N = 4;
+	vec4_t b_copy;
+	size_t i, k;
+
+	memcpy(b_copy, b, sizeof(vec4_t));
+
+	for (k = 0; k < N; ++k) {
+		if (pivot[k] != k) {
+			float tmp = b_copy[k];
+			b_copy[k] = b_copy[pivot[k]];
+			b_copy[pivot[k]] = tmp;
+		}
+
+		x[k] = b_copy[k];
+		for (i = 0; i < k; ++i)
+			x[k] -= x[i] * A[k][i];
+		x[k] /= A[k][k];
+	}
+
+	for (k = N; k-- > 0;)
+		for (i = k + 1; i < N; ++i)
+			x[k] -= x[i] * A[k][i];
+}
+
+
+
diff --git a/common/vec3.c b/common/vec3.c
new file mode 100644
index 0000000000..12c33179c2
--- /dev/null
+++ b/common/vec3.c
@@ -0,0 +1,33 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "common.h"
+#include "math.h"
+#include "math_util.h"
+#include "vec3.h"
+#include "util.h"
+
+void vec3_scalar_mul(vec3_t v, float c)
+{
+	v[X] *= c;
+	v[Y] *= c;
+	v[Z] *= c;
+}
+
+float vec3_dot(const vec3_t v, const vec3_t w)
+{
+	return v[X] * w[X] + v[Y] * w[Y] + v[Z] * w[Z];
+}
+
+float vec3_norm_squared(const vec3_t v)
+{
+	return vec3_dot(v, v);
+}
+
+float vec3_norm(const vec3_t v)
+{
+	return sqrtf(vec3_norm_squared(v));
+}
+
diff --git a/core/cortex-m/include/math.h b/core/cortex-m/include/math.h
index fbcd06e233..ac6acf1ef0 100644
--- a/core/cortex-m/include/math.h
+++ b/core/cortex-m/include/math.h
@@ -19,6 +19,17 @@ static inline float sqrtf(float v)
 	);
 	return root;
 }
+
+static inline float fabsf(float v)
+{
+	float root;
+	asm volatile(
+		"fabss %0, %1"
+		: "=w" (root)
+		: "w" (v)
+	);
+	return root;
+}
 #endif  /* CONFIG_FPU */
 
 #endif  /* __CROS_EC_MATH_H */
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);
diff --git a/include/config.h b/include/config.h
index db74a4c507..76bae7e9a2 100644
--- a/include/config.h
+++ b/include/config.h
@@ -1271,6 +1271,9 @@
 /* Need for a math library */
 #undef CONFIG_MATH_UTIL
 
+/* Include code to do online compass calibration */
+#undef CONFIG_MAG_CALIBRATE
+
 /* Presence of a Bosh Sensortec BMM150 magnetometer behind a BMI160. */
 #undef CONFIG_MAG_BMI160_BMM150
 
diff --git a/include/mag_cal.h b/include/mag_cal.h
new file mode 100644
index 0000000000..76410a193e
--- /dev/null
+++ b/include/mag_cal.h
@@ -0,0 +1,46 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Online magnetometer calibration */
+
+#ifndef __CROS_EC_MAG_CAL_H
+#define __CROS_EC_MAG_CAL_H
+
+#include "math_util.h"
+#include "mat44.h"
+#include "vec4.h"
+
+#define MAG_CAL_MAX_SAMPLES 0xffff
+#define MAG_CAL_MIN_BATCH_WINDOW_US    SECOND
+#define MAG_CAL_MIN_BATCH_SIZE      25      /* samples */
+
+struct mag_cal_t {
+	/*
+	 * Matric for sphere fitting:
+	 * +----+----+----+----+
+	 * | xx | xy | xz | x  |
+	 * +----+----+----+----+
+	 * | xy | yy | yz | y  |
+	 * +----+----+----+----+
+	 * | xz | yz | zz | z  |
+	 * +----+----+----+----+
+	 * | x  | y  | z  | 1  |
+	 * +----+----+----+----+
+	 */
+	mat44_t acc;
+	vec4_t acc_w;
+	float radius;
+
+	vector_3_t bias;
+
+	/* number of samples needed to calibrate */
+	uint16_t batch_size;
+	uint16_t nsamples;
+};
+
+void init_mag_cal(struct mag_cal_t *moc);
+
+int mag_cal_update(struct mag_cal_t *moc, const vector_3_t v);
+#endif  /* __CROS_EC_MAG_CAL_H */
diff --git a/include/mat33.h b/include/mat33.h
new file mode 100644
index 0000000000..de7debbbc1
--- /dev/null
+++ b/include/mat33.h
@@ -0,0 +1,30 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifndef __CROS_EC_MAT_33_H
+
+#define __CROS_EC_MAT_33_H
+
+#include "vec3.h"
+#include "util.h"
+
+typedef float mat33_t[3][3];
+typedef size_t size3_t[3];
+
+void init_zero_matrix(mat33_t A);
+void init_diagonal_matrix(mat33_t A, float x);
+
+void mat33_scalar_mul(mat33_t A, float c);
+
+void mat33_swap_rows(mat33_t A, const size_t i, const size_t j);
+
+void mat33_get_eigenbasis(mat33_t S, vec3_t eigenvals, mat33_t eigenvecs);
+
+size_t mat33_maxind(mat33_t A, size_t k);
+
+void mat33_rotate(mat33_t A, float c, float s,
+		  size_t k, size_t l, size_t i, size_t j);
+
+#endif  /* __CROS_EC_MAT_33_H */
diff --git a/include/mat44.h b/include/mat44.h
new file mode 100644
index 0000000000..ae8055d5fb
--- /dev/null
+++ b/include/mat44.h
@@ -0,0 +1,23 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Header file for common math functions. */
+#ifndef __CROS_EC_MAT_44_H
+
+#define __CROS_EC_MAT_44_H
+
+#include "vec4.h"
+#include "util.h"
+
+typedef float mat44_t[4][4];
+typedef size_t size4_t[4];
+
+void mat44_decompose_lup(mat44_t LU, size4_t pivot);
+
+void mat44_swap_rows(mat44_t A, const size_t i, const size_t j);
+
+void mat44_solve(mat44_t A, vec4_t x, const vec4_t b, const size4_t pivot);
+
+#endif  /* __CROS_EC_MAT_44_H */
diff --git a/include/math_util.h b/include/math_util.h
index 0dd11c230b..385c044c6f 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -99,9 +99,9 @@ typedef fp_t matrix_3x3_t[3][3];
 /* Integer vector */
 typedef int vector_3_t[3];
 
-/* For vector_3_t, define which coordinates are in which location. */
+/* For vectors, define which coordinates are in which location. */
 enum {
-	X, Y, Z
+	X, Y, Z, W
 };
 /*
  * Return absolute value of x.  Note that as a macro expansion, this may have
diff --git a/include/vec3.h b/include/vec3.h
new file mode 100644
index 0000000000..68ec1a5c72
--- /dev/null
+++ b/include/vec3.h
@@ -0,0 +1,17 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Header file for common math functions. */
+#ifndef __CROS_EC_VEC_3_H
+#define __CROS_EC_VEC_3_H
+
+typedef float vec3_t[3];
+
+void vec3_scalar_mul(vec3_t v, float c);
+float vec3_dot(const vec3_t v, const vec3_t w);
+float vec3_norm_squared(const vec3_t v);
+float vec3_norm(const vec3_t v);
+
+#endif  /* __CROS_EC_VEC_3_H */
diff --git a/include/vec4.h b/include/vec4.h
new file mode 100644
index 0000000000..14e290c96e
--- /dev/null
+++ b/include/vec4.h
@@ -0,0 +1,13 @@
+/* Copyright 2015 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifndef __CROS_EC_VEC_4_H
+
+#define __CROS_EC_VEC_4_H
+
+typedef float vec4_t[4];
+
+#endif  /* __CROS_EC_VEC_4_H */
+