diff options
author | Gwendal Grignou <gwendal@chromium.org> | 2015-09-11 12:02:26 -0700 |
---|---|---|
committer | chrome-bot <chrome-bot@chromium.org> | 2015-10-13 05:28:54 -0700 |
commit | 828b55a7358ad5ec8bc27552bfb280eb173dd453 (patch) | |
tree | b9430d2bf624ee2b6976eed211696b50ae56decb /common/mat33.c | |
parent | 0647f66f81de880af603a7fb70f57159519782ac (diff) | |
download | chrome-ec-828b55a7358ad5ec8bc27552bfb280eb173dd453.tar.gz |
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>
Diffstat (limited to 'common/mat33.c')
-rw-r--r-- | common/mat33.c | 170 |
1 files changed, 170 insertions, 0 deletions
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; +} + + |