diff options
author | Yuval Peress <peress@chromium.org> | 2019-10-17 09:55:09 -0600 |
---|---|---|
committer | Commit Bot <commit-bot@chromium.org> | 2020-01-21 23:07:33 +0000 |
commit | 62df6c8c83814e3ade12afd346afb11d7a6150c8 (patch) | |
tree | 38e94228f99223405b94579aa82a46f310a2b16a | |
parent | e9c55a5830e558fbf6158b879da0a93904ef1e6f (diff) | |
download | chrome-ec-62df6c8c83814e3ade12afd346afb11d7a6150c8.tar.gz |
common: Implement kasa sphere fit algorithm
Add an implementation of the kasa sphere fit algorithm
adapted from AOSP.
BUG=b:138303429,chromium:1023858
TEST=Added unit tests
BRANCH=None
Change-Id: I8194bfaddbb7c57a2b20a1917c91f7c78707e685
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1867226
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
-rw-r--r-- | common/build.mk | 3 | ||||
-rw-r--r-- | common/kasa.c | 74 | ||||
-rw-r--r-- | include/kasa.h | 46 | ||||
-rw-r--r-- | test/build.mk | 2 | ||||
-rw-r--r-- | test/kasa.c | 73 | ||||
-rw-r--r-- | test/kasa.tasklist | 10 | ||||
-rw-r--r-- | test/test_config.h | 5 |
7 files changed, 212 insertions, 1 deletions
diff --git a/common/build.mk b/common/build.mk index 372cf98eb2..0fc5a7d393 100644 --- a/common/build.mk +++ b/common/build.mk @@ -119,7 +119,8 @@ common-$(CONFIG_ROLLBACK)+=rollback.o common-$(CONFIG_RWSIG)+=rwsig.o vboot/common.o common-$(CONFIG_RWSIG_TYPE_RWSIG)+=vboot/vb21_lib.o common-$(CONFIG_MATH_UTIL)+=math_util.o -common-$(CONFIG_ONLINE_CALIB)+=stillness_detector.o +common-$(CONFIG_ONLINE_CALIB)+=stillness_detector.o kasa.o math_util.o \ + mat44.o vec3.o common-$(CONFIG_SHA1)+= sha1.o common-$(CONFIG_SHA256)+=sha256.o common-$(CONFIG_SOFTWARE_CLZ)+=clz.o diff --git a/common/kasa.c b/common/kasa.c new file mode 100644 index 0000000000..79c75ad55b --- /dev/null +++ b/common/kasa.c @@ -0,0 +1,74 @@ +/* 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 "kasa.h" +#include "mat44.h" +#include <string.h> + +void kasa_reset(struct kasa_fit *kasa) +{ + memset(kasa, 0, sizeof(struct kasa_fit)); +} + +void kasa_accumulate(struct kasa_fit *kasa, fp_t x, fp_t y, fp_t z) +{ + fp_t w = fp_sq(x) + fp_sq(y) + fp_sq(z); + + kasa->acc_x += x; + kasa->acc_y += y; + kasa->acc_z += z; + kasa->acc_w += w; + + kasa->acc_xx += fp_sq(x); + kasa->acc_xy += fp_mul(x, y); + kasa->acc_xz += fp_mul(x, z); + kasa->acc_xw += fp_mul(x, w); + + kasa->acc_yy += fp_sq(y); + kasa->acc_yz += fp_mul(y, z); + kasa->acc_yw += fp_mul(y, w); + + kasa->acc_zz += fp_sq(z); + kasa->acc_zw += fp_mul(z, w); + + kasa->nsamples += 1; +} + +void kasa_compute(struct kasa_fit *kasa, fpv3_t bias, fp_t *radius) +{ + /* A * out = b + * (4 x 4) (4 x 1) (4 x 1) + */ + mat44_fp_t A; + fpv4_t b, out; + sizev4_t pivot; + + A[0][0] = kasa->nsamples; + A[0][1] = A[1][0] = kasa->acc_x; + A[0][2] = A[2][0] = kasa->acc_y; + A[0][3] = A[3][0] = kasa->acc_z; + A[1][1] = kasa->acc_xx; + A[1][2] = A[2][1] = kasa->acc_xy; + A[1][3] = A[3][1] = kasa->acc_xz; + A[2][2] = kasa->acc_yy; + A[2][3] = A[3][2] = kasa->acc_yz; + A[3][3] = kasa->acc_zz; + + b[0] = -kasa->acc_w; + b[1] = -kasa->acc_xw; + b[2] = -kasa->acc_yw; + b[3] = -kasa->acc_zw; + + mat44_fp_decompose_lup(A, pivot); + mat44_fp_solve(A, out, b, pivot); + + bias[0] = fp_mul(out[1], FLOAT_TO_FP(-0.5f)); + bias[1] = fp_mul(out[2], FLOAT_TO_FP(-0.5f)); + bias[2] = fp_mul(out[3], FLOAT_TO_FP(-0.5f)); + + *radius = fpv3_dot(bias, bias) - out[0]; + *radius = (*radius > 0) ? fp_sqrtf(*radius) : FLOAT_TO_FP(0.0f); +} diff --git a/include/kasa.h b/include/kasa.h new file mode 100644 index 0000000000..6157b5632d --- /dev/null +++ b/include/kasa.h @@ -0,0 +1,46 @@ +/* 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. + */ + +/* Kasa sphere fit algorithm */ + +#ifndef __CROS_EC_KASA_H +#define __CROS_EC_KASA_H + +#include "vec3.h" + +struct kasa_fit { + fp_t acc_x, acc_y, acc_z, acc_w; + fp_t acc_xx, acc_xy, acc_xz, acc_xw; + fp_t acc_yy, acc_yz, acc_yw; + fp_t acc_zz, acc_zw; + uint32_t nsamples; +}; + +/** + * Resets the kasa_fit data structure (sets all variables to zero). + * + * @param kasa Pointer to the struct that should be reset. + */ +void kasa_reset(struct kasa_fit *kasa); + +/** + * Add a new sample to the kasa_fit structure. + * + * @param x The X component of the new sample. + * @param y The Y component of the new sample. + * @param z the Z component of the new sample. + */ +void kasa_accumulate(struct kasa_fit *kasa, fp_t x, fp_t y, fp_t z); + +/** + * Compute the current center/radius from the kasa_fit structure. + * + * @param kasa Pointer to the struct that should be used for the calculation. + * @param bias Pointer to the start of a fp_t[3] to save the computed center. + * @param radius Pointer to a fp_t that will hold the computed radius. + */ +void kasa_compute(struct kasa_fit *kasa, fpv3_t bias, fp_t *radius); + +#endif /* __CROS_EC_KASA_H */ diff --git a/test/build.mk b/test/build.mk index 319bfc038c..97997ed8b3 100644 --- a/test/build.mk +++ b/test/build.mk @@ -39,6 +39,7 @@ test-list-host += inductive_charging test-list-host += interrupt test-list-host += is_enabled test-list-host += is_enabled_error +test-list-host += kasa test-list-host += kb_8042 test-list-host += kb_mkbp #test-list-host += kb_scan # crbug.com/976974 @@ -127,6 +128,7 @@ 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 motion_lid-y=motion_lid.o motion_sense_fifo-y=motion_sense_fifo.o +kasa-y=kasa.o mutex-y=mutex.o nvmem-y=nvmem.o nvmem_tpm2_mock.o pingpong-y=pingpong.o diff --git a/test/kasa.c b/test/kasa.c new file mode 100644 index 0000000000..8221db2ae5 --- /dev/null +++ b/test/kasa.c @@ -0,0 +1,73 @@ +/* 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 "kasa.h" +#include "test_util.h" +#include "motion_sense.h" +#include <stdio.h> + +struct motion_sensor_t motion_sensors[] = {}; +const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); + +static int test_kasa_reset(void) +{ + struct kasa_fit kasa; + + kasa_reset(&kasa); + + TEST_EQ(kasa.nsamples, 0, "%u"); + TEST_NEAR(kasa.acc_x, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_y, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_z, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_w, 0.0f, 0.000001f, "%f"); + + TEST_NEAR(kasa.acc_xx, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_xy, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_xz, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_xw, 0.0f, 0.000001f, "%f"); + + TEST_NEAR(kasa.acc_yy, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_yz, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_yw, 0.0f, 0.000001f, "%f"); + + TEST_NEAR(kasa.acc_zz, 0.0f, 0.000001f, "%f"); + TEST_NEAR(kasa.acc_zw, 0.0f, 0.000001f, "%f"); + + return EC_SUCCESS; +} + +static int test_kasa_calculate(void) +{ + struct kasa_fit kasa; + fpv3_t bias; + float radius; + + kasa_reset(&kasa); + kasa_accumulate(&kasa, 1.01f, 0.01f, 0.01f); + kasa_accumulate(&kasa, -0.99f, 0.01f, 0.01f); + kasa_accumulate(&kasa, 0.01f, 1.01f, 0.01f); + kasa_accumulate(&kasa, 0.01f, -0.99f, 0.01f); + kasa_accumulate(&kasa, 0.01f, 0.01f, 1.01f); + kasa_accumulate(&kasa, 0.01f, 0.01f, -0.99f); + kasa_compute(&kasa, bias, &radius); + + TEST_NEAR(bias[0], 0.01f, 0.0001f, "%f"); + TEST_NEAR(bias[1], 0.01f, 0.0001f, "%f"); + TEST_NEAR(bias[2], 0.01f, 0.0001f, "%f"); + TEST_NEAR(radius, 1.0f, 0.0001f, "%f"); + + return EC_SUCCESS; +} + +void run_test(void) +{ + test_reset(); + + RUN_TEST(test_kasa_reset); + RUN_TEST(test_kasa_calculate); + + test_print_result(); +} diff --git a/test/kasa.tasklist b/test/kasa.tasklist new file mode 100644 index 0000000000..0e3696c3f0 --- /dev/null +++ b/test/kasa.tasklist @@ -0,0 +1,10 @@ +/* Copyright 2019 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 \ + TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE) diff --git a/test/test_config.h b/test/test_config.h index 9ab25f4e7e..9dbbd4e1d0 100644 --- a/test/test_config.h +++ b/test/test_config.h @@ -95,6 +95,11 @@ #define CONFIG_TEMP_CACHE_STALE_THRES (1 * SECOND) #endif +#ifdef TEST_KASA +#define CONFIG_FPU +#define CONFIG_ONLINE_CALIB +#endif + #if defined(TEST_MOTION_LID) || defined(TEST_MOTION_ANGLE) || \ defined(TEST_MOTION_ANGLE_TABLET) || defined(TEST_MOTION_SENSE_FIFO) enum sensor_id { |