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>

1 files changed, 221 insertions, 0 deletions

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;
+}
+