From 994af4a65fa7ece2f11f45038c75408d8166784a Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Fri, 22 Nov 2019 15:22:51 -0700 Subject: 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 Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1931464 Reviewed-by: Jack Rosenthal Reviewed-by: Gwendal Grignou --- common/build.mk | 3 +- common/mag_cal.c | 175 +++++++++++++++----------------------------------- include/mag_cal.h | 17 +---- test/build.mk | 2 + test/mag_cal.c | 91 ++++++++++++++++++++++++++ test/mag_cal.tasklist | 10 +++ test/test_config.h | 4 ++ 7 files changed, 161 insertions(+), 141 deletions(-) create mode 100644 test/mag_cal.c create mode 100644 test/mag_cal.tasklist diff --git a/common/build.mk b/common/build.mk index 0fc5a7d393..04ec98bcd0 100644 --- a/common/build.mk +++ b/common/build.mk @@ -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 diff --git a/common/mag_cal.c b/common/mag_cal.c index db09050007..1dc40e34db 100644 --- a/common/mag_cal.c +++ b/common/mag_cal.c @@ -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; diff --git a/include/mag_cal.h b/include/mag_cal.h index b0b35ded6f..5c12e00be7 100644 --- a/include/mag_cal.h +++ b/include/mag_cal.h @@ -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); diff --git a/test/build.mk b/test/build.mk index 97997ed8b3..c577226fdb 100644 --- a/test/build.mk +++ b/test/build.mk @@ -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 diff --git a/test/mag_cal.c b/test/mag_cal.c new file mode 100644 index 0000000000..e1931c352a --- /dev/null +++ b/test/mag_cal.c @@ -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 + +/** + * 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(); +} diff --git a/test/mag_cal.tasklist b/test/mag_cal.tasklist new file mode 100644 index 0000000000..ff715f69cd --- /dev/null +++ b/test/mag_cal.tasklist @@ -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 */ + diff --git a/test/test_config.h b/test/test_config.h index 9dbbd4e1d0..3c269c4946 100644 --- a/test/test_config.h +++ b/test/test_config.h @@ -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 -- cgit v1.2.1