summaryrefslogtreecommitdiff
path: root/common/body_detection.c
diff options
context:
space:
mode:
authorJack Rosenthal <jrosenth@chromium.org>2021-11-04 12:11:58 -0600
committerCommit Bot <commit-bot@chromium.org>2021-11-05 04:22:34 +0000
commit252457d4b21f46889eebad61d4c0a65331919cec (patch)
tree01856c4d31d710b20e85a74c8d7b5836e35c3b98 /common/body_detection.c
parent08f5a1e6fc2c9467230444ac9b582dcf4d9f0068 (diff)
downloadchrome-ec-stabilize-14695.107.B-ish.tar.gz
In the interest of making long-term branch maintenance incur as little technical debt on us as possible, we should not maintain any files on the branch we are not actually using. This has the added effect of making it extremely clear when merging CLs from the main branch when changes have the possibility to affect us. The follow-on CL adds a convenience script to actually pull updates from the main branch and generate a CL for the update. BUG=b:204206272 BRANCH=ish TEST=make BOARD=arcada_ish && make BOARD=drallion_ish Signed-off-by: Jack Rosenthal <jrosenth@chromium.org> Change-Id: I17e4694c38219b5a0823e0a3e55a28d1348f4b18 Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3262038 Reviewed-by: Jett Rink <jettrink@chromium.org> Reviewed-by: Tom Hughes <tomhughes@chromium.org>
Diffstat (limited to 'common/body_detection.c')
-rw-r--r--common/body_detection.c256
1 files changed, 0 insertions, 256 deletions
diff --git a/common/body_detection.c b/common/body_detection.c
deleted file mode 100644
index 4fbc88e852..0000000000
--- a/common/body_detection.c
+++ /dev/null
@@ -1,256 +0,0 @@
-/* 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 "accelgyro.h"
-#include "body_detection.h"
-#include "console.h"
-#include "hwtimer.h"
-#include "lid_switch.h"
-#include "math_util.h"
-#include "motion_sense_fifo.h"
-#include "timer.h"
-
-/* Console output macros */
-#define CPUTS(outstr) cputs(CC_ACCEL, outstr)
-#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
-#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
-
-static struct motion_sensor_t *body_sensor =
- &motion_sensors[CONFIG_BODY_DETECTION_SENSOR];
-
-static int window_size = CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE;
-static uint64_t var_threshold_scaled, confidence_delta_scaled;
-static int stationary_timeframe;
-
-static int history_idx;
-static enum body_detect_states motion_state = BODY_DETECTION_OFF_BODY;
-
-static bool history_initialized;
-static bool body_detect_enable;
-STATIC_IF(CONFIG_ACCEL_SPOOF_MODE) bool spoof_enable;
-
-static struct body_detect_motion_data
-{
- int history[CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE]; /* acceleration */
- int sum; /* sum(history) */
- uint64_t n2_variance; /* n^2 * var(history) */
-} data[2]; /* motion data for X-axis and Y-axis */
-
-/*
- * This function will update new variance and new sum according to incoming
- * value, previous value, previous sum and previous variance.
- * In order to prevent inaccuracy, we use integer to calculate instead of float
- *
- * n: window size
- * x: data in the old window
- * x': data in the new window
- * x_0: oldest value in the window, will be replaced by x_n
- * x_n: new coming value
- *
- * n^2 * var(x') = n^2 * var(x) + (x_n - x_0) *
- * (n * (x_n + x_0) - sum(x') - sum(x))
- */
-static void update_motion_data(struct body_detect_motion_data *x, int x_n)
-{
- const int n = window_size;
- const int x_0 = x->history[history_idx];
- const int sum_diff = x_n - x_0;
- const int new_sum = x->sum + sum_diff;
-
- x->n2_variance += sum_diff *
- ((int64_t)n * (x_n + x_0) - new_sum - x->sum);
- x->sum = new_sum;
- x->history[history_idx] = x_n;
-}
-
-/* Update motion data of X, Y with new sensor data. */
-static void update_motion_variance(void)
-{
- update_motion_data(&data[X], body_sensor->xyz[X]);
- update_motion_data(&data[Y], body_sensor->xyz[Y]);
- history_idx = (history_idx + 1 >= window_size) ? 0 : history_idx + 1;
-}
-
-/* return Var(X) + Var(Y) */
-static uint64_t get_motion_variance(void)
-{
- return (data[X].n2_variance + data[Y].n2_variance)
- / window_size / window_size;
-}
-
-static int calculate_motion_confidence(uint64_t var)
-{
- if (var < var_threshold_scaled - confidence_delta_scaled)
- return 0;
- if (var > var_threshold_scaled + confidence_delta_scaled)
- return 100;
- return 100 * (var - var_threshold_scaled + confidence_delta_scaled) /
- (2 * confidence_delta_scaled);
-}
-
-/* Change the motion state and commit the change to AP. */
-void body_detect_change_state(enum body_detect_states state, bool spoof)
-{
- if (IS_ENABLED(CONFIG_ACCEL_SPOOF_MODE) && spoof_enable && !spoof)
- return;
- if (IS_ENABLED(CONFIG_GESTURE_HOST_DETECTION)) {
- struct ec_response_motion_sensor_data vector = {
- .flags = MOTIONSENSE_SENSOR_FLAG_BYPASS_FIFO,
- .activity_data = {
- .activity = MOTIONSENSE_ACTIVITY_BODY_DETECTION,
- .state = state,
- },
- .sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID,
- };
- motion_sense_fifo_stage_data(&vector, NULL, 0,
- __hw_clock_source_read());
- motion_sense_fifo_commit_data();
- }
- /* change the motion state */
- motion_state = state;
- if (state == BODY_DETECTION_ON_BODY) {
- /* reset time counting of stationary */
- stationary_timeframe = 0;
- }
- /* state changing log */
- CPRINTS("body_detect changed state to: %s body",
- motion_state ? "on" : "off");
-}
-
-enum body_detect_states body_detect_get_state(void)
-{
- return motion_state;
-}
-
-/* Determine window size for 1 second by sensor data rate. */
-static void determine_window_size(int odr)
-{
- window_size = odr / 1000;
- /* Normally, window_size should not exceed MAX_WINDOW_SIZE. */
- if (window_size > CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE) {
- /* This will cause window size not enough for 1 second */
- CPRINTS("ODR exceeds CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE");
- window_size = CONFIG_BODY_DETECTION_MAX_WINDOW_SIZE;
- }
-}
-
-/* Determine variance threshold scale by range and resolution. */
-static void determine_threshold_scale(int range, int resolution, int rms_noise)
-{
- /*
- * range: g
- * resolution: bits
- * data_1g: LSB/g
- * data_1g / 9800: LSB/(mm/s^2)
- * (data_1g / 9800)^2: (LSB^2)/(mm^2/s^4), which number of
- * var(sensor data) will represents 1 (mm^2/s^4)
- * rms_noise: ug
- * var_noise: mm^2/s^4
- */
- const int data_1g = BIT(resolution - 1) / range;
- const int multiplier = POW2(data_1g);
- const int divisor = POW2(9800);
- /*
- * We are measuring the var(X) + var(Y), so theoretically, the
- * var(noise) should be 2 * rms_noise^2. However, in most case, on a
- * very stationary plane, the average of var(noise) are less than 2 *
- * rms_noise^2. We can multiply the rms_noise^2 with the
- * CONFIG_BODY_DETECTION_VAR_NOISE_FACTOR / 100.
- */
- const int var_noise = POW2((uint64_t)rms_noise) *
- CONFIG_BODY_DETECTION_VAR_NOISE_FACTOR * POW2(98)
- / 100 / POW2(10000);
-
- var_threshold_scaled = (uint64_t)
- (CONFIG_BODY_DETECTION_VAR_THRESHOLD + var_noise) *
- multiplier / divisor;
- confidence_delta_scaled = (uint64_t)
- CONFIG_BODY_DETECTION_CONFIDENCE_DELTA *
- multiplier / divisor;
-}
-
-void body_detect_reset(void)
-{
- int odr = body_sensor->drv->get_data_rate(body_sensor);
- int resolution = body_sensor->drv->get_resolution(body_sensor);
- int rms_noise = body_sensor->drv->get_rms_noise(body_sensor);
-
- body_detect_change_state(BODY_DETECTION_ON_BODY, false);
- /*
- * The sensor is suspended since its ODR is 0,
- * there is no need to reset until sensor is up again
- */
- if (odr == 0)
- return;
- determine_window_size(odr);
- determine_threshold_scale(body_sensor->current_range,
- resolution, rms_noise);
- /* initialize motion data and state */
- memset(data, 0, sizeof(data));
- history_idx = 0;
- history_initialized = 0;
-}
-
-void body_detect(void)
-{
- uint64_t motion_var;
- int motion_confidence;
-
- if (!body_detect_enable)
- return;
-
- update_motion_variance();
- if (!history_initialized) {
- if (history_idx == window_size - 1)
- history_initialized = 1;
- return;
- }
-
- motion_var = get_motion_variance();
- motion_confidence = calculate_motion_confidence(motion_var);
- switch (motion_state) {
- case BODY_DETECTION_OFF_BODY:
- if (motion_confidence > CONFIG_BODY_DETECTION_ON_BODY_CON)
- body_detect_change_state(BODY_DETECTION_ON_BODY, false);
- break;
- case BODY_DETECTION_ON_BODY:
- stationary_timeframe += 1;
- /* confidence exceeds the limit, reset time counting */
- if (motion_confidence >= CONFIG_BODY_DETECTION_OFF_BODY_CON)
- stationary_timeframe = 0;
- /* if no motion for enough time, change state to off_body */
- if (stationary_timeframe >=
- CONFIG_BODY_DETECTION_STATIONARY_DURATION * window_size)
- body_detect_change_state(BODY_DETECTION_OFF_BODY,
- false);
- break;
- }
-}
-
-void body_detect_set_enable(int enable)
-{
- body_detect_enable = enable;
- body_detect_change_state(BODY_DETECTION_ON_BODY, false);
-}
-
-int body_detect_get_enable(void)
-{
- return body_detect_enable;
-}
-
-#ifdef CONFIG_ACCEL_SPOOF_MODE
-void body_detect_set_spoof(int enable)
-{
- spoof_enable = enable;
- /* After disabling spoof mode, commit current state. */
- if (!enable)
- body_detect_change_state(motion_state, false);
-}
-
-bool body_detect_get_spoof(void)
-{
- return spoof_enable;
-}
-#endif