coreboot-mirror
/
chrome-ec
mirror of https://review.coreboot.org/chrome-ec.git
1
0
Fork 0
Code Issues Releases Wiki Activity

common: mag_cal: update magnetometer to leverage kasa 

Update magnetometer calibration algorithm to leverage
the new kasa standalone code.

BUG=b:138303429,chromium:1023858
TEST=added unit test
BRANCH=None

Change-Id: I5c0403b66d9fe7c2925b2ec6244cf9e32ad5ea5f
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1931464
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
This commit is contained in:
Yuval Peress 2019-11-22 15:22:51 -07:00 committed by Commit Bot
parent 1b4ce5849d
commit 994af4a65f
7 changed files with 161 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
common/build.mk
View File

@ -101,7 +101,8 @@ 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_HOSTCMD_X86)+=acpi.o port80.o ec_features.o
common-$(CONFIG_MAG_CALIBRATE)+= mag_cal.o math_util.o vec3.o mat33.o mat44.o
common-$(CONFIG_MAG_CALIBRATE)+= mag_cal.o math_util.o vec3.o mat33.o mat44.o \
kasa.o
common-$(CONFIG_MKBP_EVENT)+=mkbp_event.o
common-$(CONFIG_ONEWIRE)+=onewire.o
common-$(CONFIG_PECI_COMMON)+=peci.o

175
common/mag_cal.c
View File

@ -26,6 +26,18 @@
#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
#define PRINTF_FLOAT(x) ((int)((x) * 100.0f))
/**
* Compute the covariance element: (avg(ab) - avg(a)*avg(b))
*
* @param sq The accumulated sum of a*b
* @param a The accumulated sum of a
* @param b The accumulated sum of b
* @return (sq - ((a * b) * inv)) * inv
*/
static inline fp_t covariance_element(fp_t sq, fp_t a, fp_t b, fp_t inv)
{
return fp_mul(sq - fp_mul(fp_mul(a, b), inv), inv);
}
/*
* eigen value magnitude and ratio test
*
@ -38,18 +50,35 @@ static int moc_eigen_test(struct mag_cal_t *moc)
fpv3_t eigenvals;
mat33_fp_t eigenvecs;
fp_t evmax, evmin, evmag;
fp_t inv = fp_div_dbz(FLOAT_TO_FP(1.0f),
INT_TO_FP((int) moc->kasa_fit.nsamples));
int eigen_pass;
/* covariance matrix */
S[0][0] = moc->acc[0][0] - fp_sq(moc->acc[0][3]);
S[0][1] = S[1][0] =
moc->acc[0][1] - fp_mul(moc->acc[0][3], moc->acc[1][3]);
S[0][2] = S[2][0] =
moc->acc[0][2] - fp_mul(moc->acc[0][3], moc->acc[2][3]);
S[1][1] = moc->acc[1][1] - fp_sq(moc->acc[1][3]);
S[1][2] = S[2][1] =
moc->acc[1][2] - fp_mul(moc->acc[1][3], moc->acc[2][3]);
S[2][2] = moc->acc[2][2] - fp_sq(moc->acc[2][3]);
S[0][0] = covariance_element(moc->kasa_fit.acc_xx,
moc->kasa_fit.acc_x,
moc->kasa_fit.acc_x,
inv);
S[0][1] = S[1][0] = covariance_element(moc->kasa_fit.acc_xy,
moc->kasa_fit.acc_x,
moc->kasa_fit.acc_y,
inv);
S[0][2] = S[2][0] = covariance_element(moc->kasa_fit.acc_xz,
moc->kasa_fit.acc_x,
moc->kasa_fit.acc_z,
inv);
S[1][1] = covariance_element(moc->kasa_fit.acc_yy,
moc->kasa_fit.acc_y,
moc->kasa_fit.acc_y,
inv);
S[1][2] = S[2][1] = covariance_element(moc->kasa_fit.acc_yz,
moc->kasa_fit.acc_y,
moc->kasa_fit.acc_z,
inv);
S[2][2] = covariance_element(moc->kasa_fit.acc_zz,
moc->kasa_fit.acc_z,
moc->kasa_fit.acc_z,
inv);
mat33_fp_get_eigenbasis(S, eigenvals, eigenvecs);
@ -66,88 +95,23 @@ static int moc_eigen_test(struct mag_cal_t *moc)
&& (evmag < MAX_EIGEN_MAG);
#if 0
CPRINTF("mag eigenvalues: (%d %d %d), ",
CPRINTF("mag eigenvalues: (%.02d %.02d %.02d), ",
PRINTF_FLOAT(eigenvals[X]),
PRINTF_FLOAT(eigenvals[Y]),
PRINTF_FLOAT(eigenvals[Z]));
CPRINTF("ratio %d, mag %d: pass %d\r\n",
CPRINTF("ratio %.02d, mag %.02d: pass %d\r\n",
PRINTF_FLOAT(evmax / evmin),
PRINTF_FLOAT(evmag),
PRINTF_FLOAT(eigen_pass));
eigen_pass);
#endif
return eigen_pass;
}
/*
* Kasa sphere fitting with normal equation
*/
static int moc_fit(struct mag_cal_t *moc, fpv3_t bias, fp_t *radius)
{
sizev4_t pivot;
fpv4_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] = FLOAT_TO_FP(1.0f);
moc->acc_w[X] = fp_mul(moc->acc_w[X], FLOAT_TO_FP(-1));
moc->acc_w[Y] = fp_mul(moc->acc_w[Y], FLOAT_TO_FP(-1));
moc->acc_w[Z] = fp_mul(moc->acc_w[Z], FLOAT_TO_FP(-1));
moc->acc_w[W] = fp_mul(moc->acc_w[W], FLOAT_TO_FP(-1));
mat44_fp_decompose_lup(moc->acc, pivot);
mat44_fp_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(fpv3_t));
fpv3_scalar_mul(bias, FLOAT_TO_FP(-0.5f));
*radius = fp_sqrtf(fpv3_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;
kasa_reset(&moc->kasa_fit);
}
int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
@ -155,61 +119,22 @@ int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
int new_bias = 0;
/* 1. run accumulators */
fp_t w = fp_sq(v[X]) + fp_sq(v[Y]) + fp_sq(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] += fp_sq(v[X]);
moc->acc[0][1] += fp_mul(v[X], v[Y]);
moc->acc[0][2] += fp_mul(v[X], v[Z]);
moc->acc_w[X] += fp_mul(v[X], w);
moc->acc[1][1] += fp_sq(v[Y]);
moc->acc[1][2] += fp_mul(v[Y], v[Z]);
moc->acc_w[Y] += fp_mul(v[Y], w);
moc->acc[2][2] += fp_sq(v[Z]);
moc->acc_w[Z] += fp_mul(v[Z], w);
if (moc->nsamples < MAG_CAL_MAX_SAMPLES)
moc->nsamples++;
kasa_accumulate(&moc->kasa_fit, INT_TO_FP(v[X]), INT_TO_FP(v[Y]),
INT_TO_FP(v[Z]));
/* 2. batch has enough samples? */
if (moc->batch_size > 0 && moc->nsamples >= moc->batch_size) {
fp_t inv = fp_div_dbz(FLOAT_TO_FP(1.0f),
INT_TO_FP((int)moc->nsamples));
moc->acc[0][3] = fp_mul(moc->acc[0][3], inv);
moc->acc[1][3] = fp_mul(moc->acc[1][3], inv);
moc->acc[2][3] = fp_mul(moc->acc[2][3], inv);
moc->acc_w[W] = fp_mul(moc->acc_w[W], inv);
moc->acc[0][0] = fp_mul(moc->acc[0][0], inv);
moc->acc[0][1] = fp_mul(moc->acc[0][1], inv);
moc->acc[0][2] = fp_mul(moc->acc[0][2], inv);
moc->acc_w[X] = fp_mul(moc->acc_w[X], inv);
moc->acc[1][1] = fp_mul(moc->acc[1][1], inv);
moc->acc[1][2] = fp_mul(moc->acc[1][2], inv);
moc->acc_w[Y] = fp_mul(moc->acc_w[Y], inv);
moc->acc[2][2] = fp_mul(moc->acc[2][2], inv);
moc->acc_w[Z] = fp_mul(moc->acc_w[Z], inv);
if (moc->batch_size > 0 && moc->kasa_fit.nsamples >= moc->batch_size) {
/* 3. eigen test */
if (moc_eigen_test(moc)) {
fpv3_t bias;
fp_t radius;
/* 4. Kasa sphere fitting */
if (moc_fit(moc, bias, &radius)) {
moc->bias[X] = fp_mul(bias[X], FLOAT_TO_FP(-1));
moc->bias[Y] = fp_mul(bias[Y], FLOAT_TO_FP(-1));
moc->bias[Z] = fp_mul(bias[Z], FLOAT_TO_FP(-1));
kasa_compute(&moc->kasa_fit, bias, &radius);
if (radius > MIN_FIT_MAG && radius < MAX_FIT_MAG) {
moc->bias[X] = FP_TO_INT(bias[X]);
moc->bias[Y] = FP_TO_INT(bias[Y]);
moc->bias[Z] = FP_TO_INT(bias[Z]);
moc->radius = radius;

17
include/mag_cal.h
View File

@ -11,33 +11,20 @@
#include "math_util.h"
#include "mat44.h"
#include "vec4.h"
#include "kasa.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_fp_t acc;
fpv4_t acc_w;
struct kasa_fit kasa_fit;
fp_t radius;
intv3_t bias;
/* number of samples needed to calibrate */
uint16_t batch_size;
uint16_t nsamples;
};
void init_mag_cal(struct mag_cal_t *moc);

2
test/build.mk
View File

@ -45,6 +45,7 @@ test-list-host += kb_mkbp
#test-list-host += kb_scan # crbug.com/976974
test-list-host += lid_sw
test-list-host += lightbar
test-list-host += mag_cal
test-list-host += math_util
test-list-host += motion_angle
test-list-host += motion_angle_tablet
@ -123,6 +124,7 @@ kb_mkbp-y=kb_mkbp.o
kb_scan-y=kb_scan.o
lid_sw-y=lid_sw.o
lightbar-y=lightbar.o
mag_cal-y=mag_cal.o
math_util-y=math_util.o
motion_angle-y=motion_angle.o motion_angle_data_literals.o motion_common.o
motion_angle_tablet-y=motion_angle_tablet.o motion_angle_data_literals_tablet.o motion_common.o

91
test/mag_cal.c Normal file
View File

@ -0,0 +1,91 @@
/* Copyright 2020 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 "mag_cal.h"
#include "test_util.h"
#include <stdio.h>
/**
* Various samples that might be seen in the wild. Normal range for magnetic
* fields is around 80 uT. This translates to roughly +/-525 units for the
* lis2mdl sensor.
*
* Random numbers were generated using the range of [518,532] (+- 2.14 uT) for
* the high values and [-5,5] (+- 1.53 uT) for the low values.
*/
static intv3_t samples[] = {
{ -522, 5, -5 },
{ -528, -3, 1 },
{ -531, -2, 0 },
{ -525, -1, 3 },
{ 527, 3, -2 },
{ 523, -5, 1 },
{ 520, -3, 2 },
{ 522, 0, -4 },
{ -3, -519, -2 },
{ 1, -521, 5 },
{ 2, -526, 4 },
{ 0, -532, -5 },
{ -5, 528, 4 },
{ -2, 531, -4 },
{ 1, 522, 2 },
{ 5, 532, 3 },
{ -5, 0, -524 },
{ -1, -2, -527 },
{ -3, 4, -532 },
{ 5, 3, -531 },
{ 4, -2, 524 },
{ 1, 3, 520 },
{ 5, -5, 528 },
{ 0, 2, 521 },
};
static int test_mag_cal_computes_bias(void)
{
struct mag_cal_t cal;
int i;
init_mag_cal(&cal);
cal.batch_size = ARRAY_SIZE(samples);
/* Test that we don't calibrate until we added the final sample. */
for (i = 0; i < cal.batch_size - 1; ++i)
TEST_EQ(0, mag_cal_update(&cal, samples[i]), "%d");
/* Add the final sample and check calibration. */
TEST_EQ(1, mag_cal_update(&cal, samples[cal.batch_size - 1]), "%d");
TEST_EQ(525, FP_TO_INT(cal.radius), "%d");
TEST_EQ(-1, cal.bias[0], "%d");
TEST_EQ(1, cal.bias[1], "%d");
TEST_EQ(-2, cal.bias[2], "%d");
/*
* State should have reset, run the same code again to verify that
* we get the same calibration.
*/
for (i = 0; i < cal.batch_size - 1; ++i)
TEST_EQ(0, mag_cal_update(&cal, samples[i]), "%d");
TEST_EQ(1, mag_cal_update(&cal, samples[cal.batch_size - 1]), "%d");
TEST_EQ(525, FP_TO_INT(cal.radius), "%d");
TEST_EQ(-1, cal.bias[0], "%d");
TEST_EQ(1, cal.bias[1], "%d");
TEST_EQ(-2, cal.bias[2], "%d");
return EC_SUCCESS;
}
void run_test(void)
{
test_reset();
RUN_TEST(test_mag_cal_computes_bias);
test_print_result();
}

10
test/mag_cal.tasklist Normal file
View File

@ -0,0 +1,10 @@
/* Copyright 2020 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.
*/
/**
* See CONFIG_TASK_LIST in config.h for details.
*/
#define CONFIG_TEST_TASK_LIST /* No test task */

4
test/test_config.h
View File

@ -65,6 +65,10 @@
#define CONFIG_MATH_UTIL
#endif
#ifdef TEST_MAG_CAL
#define CONFIG_MAG_CALIBRATE
#endif
#ifdef TEST_STILLNESS_DETECTOR
#define CONFIG_FPU
#define CONFIG_ONLINE_CALIB