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>

7 files changed, 134 insertions, 2 deletions

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 */
+