summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--board/ryu/board.h1
-rw-r--r--board/ryu/ec.tasklist2
-rw-r--r--common/build.mk1
-rw-r--r--common/mag_cal.c221
-rw-r--r--common/mat33.c170
-rw-r--r--common/mat44.c85
-rw-r--r--common/vec3.c33
-rw-r--r--core/cortex-m/include/math.h11
-rw-r--r--driver/accelgyro_bmi160.c23
-rw-r--r--driver/accelgyro_bmi160.h2
-rw-r--r--driver/mag_bmm150.c29
-rw-r--r--driver/mag_bmm150.h13
-rw-r--r--include/config.h3
-rw-r--r--include/mag_cal.h46
-rw-r--r--include/mat33.h30
-rw-r--r--include/mat44.h23
-rw-r--r--include/math_util.h4
-rw-r--r--include/vec3.h17
-rw-r--r--include/vec4.h13
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 */
+