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>
This commit is contained in:
parent
e9c55a5830
commit
62df6c8c83
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
}
|
|
@ -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 */
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
}
|
|
@ -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)
|
|
@ -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 {
|
||||
|
|
Loading…
Reference in New Issue