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

common: Add sensor stillness detector 

This change adds a stillness detector for 3d sensors.
This will be needed to filter sensor readings when
calibrating later.

BUG=b:138303429,chromium:1023858
BRANCH=None
TEST=buildall with new unit tests
Change-Id: I919ae7533fd42b0394de66aa0585e58343a662cc
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1833157
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
This commit is contained in:
Yuval Peress 2019-10-01 00:51:23 -06:00 committed by Commit Bot
parent 093b5d0f97
commit 61ef5715f6
9 changed files with 355 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
common/build.mk
View File

@ -119,6 +119,7 @@ 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_SHA1)+= sha1.o
common-$(CONFIG_SHA256)+=sha256.o
common-$(CONFIG_SOFTWARE_CLZ)+=clz.o

110
common/stillness_detector.c Normal file
View File

@ -0,0 +1,110 @@
/* 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.
*/
#include "common.h"
#include "stillness_detector.h"
#include "timer.h"
#include <string.h>
static void still_det_reset(struct still_det *still_det)
{
still_det->num_samples = 0;
still_det->acc_x = FLOAT_TO_FP(0.0f);
still_det->acc_y = FLOAT_TO_FP(0.0f);
still_det->acc_z = FLOAT_TO_FP(0.0f);
still_det->acc_xx = FLOAT_TO_FP(0.0f);
still_det->acc_yy = FLOAT_TO_FP(0.0f);
still_det->acc_zz = FLOAT_TO_FP(0.0f);
}
static bool stillness_batch_complete(struct still_det *still_det,
uint32_t sample_time)
{
bool complete = false;
uint32_t batch_window = time_until(still_det->window_start_time,
sample_time);
/* Checking if enough data is accumulated */
if (batch_window >= still_det->min_batch_window &&
still_det->num_samples > still_det->min_batch_size) {
if (batch_window <= still_det->max_batch_window) {
complete = true;
} else {
/* Checking for too long batch window, reset and start
* over
*/
still_det_reset(still_det);
}
} else if (batch_window > still_det->min_batch_window &&
still_det->num_samples < still_det->min_batch_size) {
/* Not enough samples collected, reset and start over */
still_det_reset(still_det);
}
return complete;
}
static inline fp_t compute_variance(fp_t acc_squared, fp_t acc, fp_t inv)
{
/* (acc^2 - (acc * acc * inv)) * inv */
return fp_mul((acc_squared - fp_mul(fp_sq(acc), inv)), inv);
}
bool still_det_update(struct still_det *still_det, uint32_t sample_time,
fp_t x, fp_t y, fp_t z)
{
fp_t inv = FLOAT_TO_FP(0.0f), var_x, var_y, var_z;
bool complete = false;
/* Accumulate for mean and VAR */
still_det->acc_x += x;
still_det->acc_y += y;
still_det->acc_z += z;
still_det->acc_xx += fp_mul(x, x);
still_det->acc_yy += fp_mul(y, y);
still_det->acc_zz += fp_mul(z, z);
switch (++still_det->num_samples) {
case 0:
/* If we rolled over, go back. */
still_det->num_samples--;
break;
case 1:
/* Set a new start time if new batch. */
still_det->window_start_time = sample_time;
break;
}
if (stillness_batch_complete(still_det, sample_time)) {
/*
* Compute 1/num_samples and check for num_samples == 0 (should
* never happen, but just in case)
*/
if (still_det->num_samples) {
inv = fp_div(1.0f, INT_TO_FP(still_det->num_samples));
} else {
still_det_reset(still_det);
return complete;
}
/* Calculating the VAR = sum(x^2)/n - sum(x)^2/n^2 */
var_x = compute_variance(
still_det->acc_xx, still_det->acc_x, inv);
var_y = compute_variance(
still_det->acc_yy, still_det->acc_y, inv);
var_z = compute_variance(
still_det->acc_zz, still_det->acc_z, inv);
/* Checking if sensor is still */
if (var_x < still_det->var_threshold &&
var_y < still_det->var_threshold &&
var_z < still_det->var_threshold) {
still_det->mean_x = fp_mul(still_det->acc_x, inv);
still_det->mean_y = fp_mul(still_det->acc_y, inv);
still_det->mean_z = fp_mul(still_det->acc_z, inv);
complete = true;
}
/* Reset and start over */
still_det_reset(still_det);
}
return complete;
}

7
include/config.h
View File

@ -2763,6 +2763,9 @@
/* Need for a math library */
#undef CONFIG_MATH_UTIL
/* Include sensor online calibration (requires CONFIG_FPU) */
#undef CONFIG_ONLINE_CALIB
/* Include code to do online compass calibration */
#undef CONFIG_MAG_CALIBRATE
@ -5153,4 +5156,8 @@
#define CONFIG_CRC8
#endif
#if defined(CONFIG_ONLINE_CALIB) && !defined(CONFIG_FPU)
#error "Online calibration requires CONFIG_FPU"
#endif
#endif /* __CROS_EC_CONFIG_H */

71
include/stillness_detector.h Normal file
View File

@ -0,0 +1,71 @@
/* 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.
*/
#ifndef __CROS_EC_STILLNESS_DETECTOR_H
#define __CROS_EC_STILLNESS_DETECTOR_H
#include "common.h"
#include "math_util.h"
#include "stdbool.h"
#include <stdint.h>
struct still_det {
/** Variance threshold for the stillness confidence score. [units]^2 */
fp_t var_threshold;
/** The minimum window duration to consider a still sample. */
uint32_t min_batch_window;
/** The maximum window duration to consider a still sample. */
uint32_t max_batch_window;
/**
* The minimum number of samples in a window to consider a still sample.
*/
uint16_t min_batch_size;
/** The timestamp of the first sample in the current batch. */
uint32_t window_start_time;
/** The number of samples in the current batch. */
uint16_t num_samples;
/** Accumulators used for calculating stillness. */
fp_t acc_x, acc_y, acc_z, acc_xx, acc_yy, acc_zz, mean_x, mean_y,
mean_z;
};
#define STILL_DET(VAR_THRES, MIN_BATCH_WIN, MAX_BATCH_WIN, MIN_BATCH_SIZE) \
((struct still_det){ \
.var_threshold = VAR_THRES, \
.min_batch_window = MIN_BATCH_WIN, \
.max_batch_window = MAX_BATCH_WIN, \
.min_batch_size = MIN_BATCH_SIZE, \
.window_start_time = 0, \
.acc_x = 0.0f, \
.acc_y = 0.0f, \
.acc_z = 0.0f, \
.acc_xx = 0.0f, \
.acc_yy = 0.0f, \
.acc_zz = 0.0f, \
.mean_x = 0.0f, \
.mean_y = 0.0f, \
.mean_z = 0.0f, \
})
/**
* Update a stillness detector with a new sample.
*
* @param sample_time The timestamp of the sample to add.
* @param x The x component of the sample to add.
* @param y The y component of the sample to add.
* @param z The z component of the sample to add.
* @return True if the sample triggered a complete batch and mean_* are now
* valid.
*/
bool still_det_update(struct still_det *still_det, uint32_t sample_time, fp_t x,
fp_t y, fp_t z);
#endif /* __CROS_EC_STILLNESS_DETECTOR_H */

16
include/test_util.h
View File

@ -38,16 +38,22 @@
} \
} while (0)
#if defined(__cplusplus) && !defined(__auto_type)
#define __auto_type auto
#endif
#define TEST_OPERATOR(a, b, op, fmt) \
do { \
if (!((a) op (b))) { \
__auto_type _a = (a); \
__auto_type _b = (b); \
if (!(_a op _b)) { \
ccprintf("%d: ASSERSION failed: %s " #op " %s\n", \
__LINE__, #a, #b); \
ccprintf("\t\tEVAL: " fmt " " #op " " fmt "\n", \
(a), (b)); \
task_dump_trace(); \
return EC_ERROR_UNKNOWN; \
} \
_a, _b); \
task_dump_trace(); \
return EC_ERROR_UNKNOWN; \
} \
} while (0)
#define TEST_EQ(a, b, fmt) TEST_OPERATOR(a, b, ==, fmt)

2
test/build.mk
View File

@ -88,6 +88,7 @@ test-list-host += utils
test-list-host += utils_str
test-list-host += vboot
test-list-host += x25519
test-list-host += stillness_detector
endif
aes-y=aes.o
@ -171,6 +172,7 @@ vboot-y=vboot.o
float-y=fp.o
fp-y=fp.o
x25519-y=x25519.o
stillness_detector-y=stillness_detector.o
TPM2_ROOT := $(CROS_WORKON_SRCROOT)/src/third_party/tpm2
$(out)/RO/common/new_nvmem.o: CFLAGS += -I$(TPM2_ROOT) -I chip/g

139
test/stillness_detector.c Normal file
View File

@ -0,0 +1,139 @@
/* 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.
*/
#include "stillness_detector.h"
#include "motion_sense.h"
#include "test_util.h"
#include "timer.h"
#include <stdio.h>
/*****************************************************************************/
/*
* Need to define motion sensor globals just to compile.
* We include motion task to force the inclusion of math_util.c
*/
struct motion_sensor_t motion_sensors[] = {};
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
static int test_build_still_det_struct(void)
{
struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
TEST_NEAR(det.var_threshold, 0.00025f, 0.000001f, "%f");
TEST_EQ(det.min_batch_window, 800 * MSEC, "%u");
TEST_EQ(det.max_batch_window, 1200 * MSEC, "%u");
TEST_EQ(det.min_batch_size, 5, "%u");
return EC_SUCCESS;
}
static int test_not_still_short_window(void)
{
struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
int i;
for (i = 0; i < 6; ++i)
TEST_ASSERT(!still_det_update(&det, i * 100 * MSEC,
0.0f, 0.0f, 0.0f));
return EC_SUCCESS;
}
static int test_not_still_long_window(void)
{
struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
int i;
for (i = 0; i < 5; ++i)
TEST_ASSERT(!still_det_update(&det, i * 300 * MSEC,
0.0f, 0.0f, 0.0f));
return EC_SUCCESS;
}
static int test_not_still_not_enough_samples(void)
{
struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
int i;
for (i = 0; i < 4; ++i)
TEST_ASSERT(!still_det_update(&det, i * 200 * MSEC,
0.0f, 0.0f, 0.0f));
return EC_SUCCESS;
}
static int test_is_still_all_axes(void)
{
struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
int i;
for (i = 0; i < 9; ++i) {
int result = still_det_update(&det, i * 100 * MSEC,
i * 0.001f, i * 0.001f,
i * 0.001f);
TEST_EQ(result, i == 8 ? 1 : 0, "%d");
}
TEST_NEAR(det.mean_x, 0.004f, 0.0001f, "%f");
TEST_NEAR(det.mean_y, 0.004f, 0.0001f, "%f");
TEST_NEAR(det.mean_z, 0.004f, 0.0001f, "%f");
return EC_SUCCESS;
}
static int test_not_still_one_axis(void)
{
struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
int i;
for (i = 0; i < 9; ++i) {
TEST_ASSERT(!still_det_update(&det, i * 100 * MSEC,
i * 0.001f, i * 0.001f,
i * 0.01f));
}
return EC_SUCCESS;
}
static int test_resets(void)
{
struct still_det det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
int i;
for (i = 0; i < 9; ++i) {
TEST_ASSERT(!still_det_update(&det, i * 100 * MSEC,
i * 0.001f, i * 0.001f,
i * 0.01f));
}
for (i = 0; i < 9; ++i) {
int result = still_det_update(&det, i * 100 * MSEC,
i * 0.001f, i * 0.001f,
i * 0.001f);
TEST_EQ(result, i == 8 ? 1 : 0, "%d");
}
TEST_NEAR(det.mean_x, 0.004f, 0.0001f, "%f");
TEST_NEAR(det.mean_y, 0.004f, 0.0001f, "%f");
TEST_NEAR(det.mean_z, 0.004f, 0.0001f, "%f");
return EC_SUCCESS;
}
void run_test(void)
{
test_reset();
RUN_TEST(test_build_still_det_struct);
RUN_TEST(test_not_still_short_window);
RUN_TEST(test_not_still_long_window);
RUN_TEST(test_not_still_not_enough_samples);
RUN_TEST(test_is_still_all_axes);
RUN_TEST(test_not_still_one_axis);
RUN_TEST(test_resets);
test_print_result();
}

9
test/stillness_detector.tasklist Normal file
View File

@ -0,0 +1,9 @@
/* 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 /* No test task */

5
test/test_config.h
View File

@ -65,6 +65,11 @@
#define CONFIG_MATH_UTIL
#endif
#ifdef TEST_STILLNESS_DETECTOR
#define CONFIG_FPU
#define CONFIG_ONLINE_CALIB
#endif
#ifdef TEST_FLOAT
#define CONFIG_FPU
#define CONFIG_MAG_CALIBRATE