diff options
-rw-r--r-- | common/build.mk | 1 | ||||
-rw-r--r-- | common/stillness_detector.c | 110 | ||||
-rw-r--r-- | include/config.h | 7 | ||||
-rw-r--r-- | include/stillness_detector.h | 71 | ||||
-rw-r--r-- | include/test_util.h | 16 | ||||
-rw-r--r-- | test/build.mk | 2 | ||||
-rw-r--r-- | test/stillness_detector.c | 139 | ||||
-rw-r--r-- | test/stillness_detector.tasklist | 9 | ||||
-rw-r--r-- | test/test_config.h | 5 |
9 files changed, 355 insertions, 5 deletions
diff --git a/common/build.mk b/common/build.mk index 2a976936d2..51ced0ae42 100644 --- a/common/build.mk +++ b/common/build.mk @@ -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 diff --git a/common/stillness_detector.c b/common/stillness_detector.c new file mode 100644 index 0000000000..c33472aa22 --- /dev/null +++ b/common/stillness_detector.c @@ -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; +} diff --git a/include/config.h b/include/config.h index a8e70d0ccf..6d34b5e5aa 100644 --- a/include/config.h +++ b/include/config.h @@ -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 */ diff --git a/include/stillness_detector.h b/include/stillness_detector.h new file mode 100644 index 0000000000..65598d4d5c --- /dev/null +++ b/include/stillness_detector.h @@ -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 */ diff --git a/include/test_util.h b/include/test_util.h index 23034f918e..e10dcbe259 100644 --- a/include/test_util.h +++ b/include/test_util.h @@ -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) diff --git a/test/build.mk b/test/build.mk index b9e54574e4..4011fa455b 100644 --- a/test/build.mk +++ b/test/build.mk @@ -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 diff --git a/test/stillness_detector.c b/test/stillness_detector.c new file mode 100644 index 0000000000..04e2e55d7e --- /dev/null +++ b/test/stillness_detector.c @@ -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(); +} diff --git a/test/stillness_detector.tasklist b/test/stillness_detector.tasklist new file mode 100644 index 0000000000..5ffe662d01 --- /dev/null +++ b/test/stillness_detector.tasklist @@ -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 */ diff --git a/test/test_config.h b/test/test_config.h index fc5731bac8..c757c233d3 100644 --- a/test/test_config.h +++ b/test/test_config.h @@ -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 |