From 828b55a7358ad5ec8bc27552bfb280eb173dd453 Mon Sep 17 00:00:00 2001 From: Gwendal Grignou Date: Fri, 11 Sep 2015 12:02:26 -0700 Subject: 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 Reviewed-on: https://chromium-review.googlesource.com/299583 Reviewed-by: Anton Staaf --- include/config.h | 3 +++ include/mag_cal.h | 46 ++++++++++++++++++++++++++++++++++++++++++++++ include/mat33.h | 30 ++++++++++++++++++++++++++++++ include/mat44.h | 23 +++++++++++++++++++++++ include/math_util.h | 4 ++-- include/vec3.h | 17 +++++++++++++++++ include/vec4.h | 13 +++++++++++++ 7 files changed, 134 insertions(+), 2 deletions(-) create mode 100644 include/mag_cal.h create mode 100644 include/mat33.h create mode 100644 include/mat44.h create mode 100644 include/vec3.h create mode 100644 include/vec4.h (limited to 'include') 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 */ + -- cgit v1.2.1