diff options
| -rw-r--r-- | board/ryu/board.h | 1 | ||||
| -rw-r--r-- | board/ryu/ec.tasklist | 2 | ||||
| -rw-r--r-- | common/build.mk | 1 | ||||
| -rw-r--r-- | common/mag_cal.c | 221 | ||||
| -rw-r--r-- | common/mat33.c | 170 | ||||
| -rw-r--r-- | common/mat44.c | 85 | ||||
| -rw-r--r-- | common/vec3.c | 33 | ||||
| -rw-r--r-- | core/cortex-m/include/math.h | 11 | ||||
| -rw-r--r-- | driver/accelgyro_bmi160.c | 23 | ||||
| -rw-r--r-- | driver/accelgyro_bmi160.h | 2 | ||||
| -rw-r--r-- | driver/mag_bmm150.c | 29 | ||||
| -rw-r--r-- | driver/mag_bmm150.h | 13 | ||||
| -rw-r--r-- | include/config.h | 3 | ||||
| -rw-r--r-- | include/mag_cal.h | 46 | ||||
| -rw-r--r-- | include/mat33.h | 30 | ||||
| -rw-r--r-- | include/mat44.h | 23 | ||||
| -rw-r--r-- | include/math_util.h | 4 | ||||
| -rw-r--r-- | include/vec3.h | 17 | ||||
| -rw-r--r-- | include/vec4.h | 13 |
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 */ + |