diff options
| -rw-r--r-- | common/body_detection.c | 226 | ||||
| -rw-r--r-- | common/build.mk | 1 | ||||
| -rw-r--r-- | common/math_util.c | 3 | ||||
| -rw-r--r-- | common/motion_sense.c | 75 | ||||
| -rw-r--r-- | include/body_detection.h | 33 | ||||
| -rw-r--r-- | include/math_util.h | 3 | ||||
| -rw-r--r-- | util/ectool.c | 13 |
7 files changed, 342 insertions, 12 deletions
diff --git a/common/body_detection.c b/common/body_detection.c new file mode 100644 index 0000000000..020314e71e --- /dev/null +++ b/common/body_detection.c @@ -0,0 +1,226 @@ +/* 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 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) + (sum(x') - sum(x))^2 + + * (n * x_n - sum(x'))^2 / n - (n * x_0 - sum(x'))^2 / n + */ +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 new_sum = x->sum + (x_n - x->history[history_idx]); + + x->n2_variance = x->n2_variance + POW2((int64_t)new_sum - x->sum) + + (POW2((int64_t)x_n * n - new_sum) - + POW2((int64_t)x_0 * n - new_sum)) / n; + 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) +{ +#ifdef CONFIG_GESTURE_HOST_DETECTION + struct ec_response_motion_sensor_data vector = { + .flags = MOTIONSENSE_SENSOR_FLAG_WAKEUP, + .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(); +#endif + /* 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); + const int var_noise = POW2((int64_t)rms_noise) * POW2(98) / 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 range = body_sensor->drv->get_range(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); + /* + * 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(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); + 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); + break; + } +} + +void body_detect_set_enable(int enable) +{ + body_detect_enable = enable; + body_detect_change_state(BODY_DETECTION_ON_BODY); +} + +int body_detect_get_enable(void) +{ + return body_detect_enable; +} diff --git a/common/build.mk b/common/build.mk index f54f9ad5a8..4ef78d57f5 100644 --- a/common/build.mk +++ b/common/build.mk @@ -44,6 +44,7 @@ common-$(CONFIG_BATTERY)+=battery.o common-$(CONFIG_BATTERY_FUEL_GAUGE)+=battery_fuel_gauge.o common-$(CONFIG_BLUETOOTH_LE)+=bluetooth_le.o common-$(CONFIG_BLUETOOTH_LE_STACK)+=btle_hci_controller.o btle_ll.o +common-$(CONFIG_BODY_DETECTION)+=body_detection.o common-$(CONFIG_CAPSENSE)+=capsense.o common-$(CONFIG_CEC)+=cec.o common-$(CONFIG_CROS_BOARD_INFO)+=cbi.o diff --git a/common/math_util.c b/common/math_util.c index 450dfbe5e7..dc52729c65 100644 --- a/common/math_util.c +++ b/common/math_util.c @@ -10,9 +10,6 @@ #include "math_util.h" #include "util.h" -/* Some useful math functions. Use with integers only! */ -#define SQ(x) ((x) * (x)) - /* For cosine lookup table, define the increment and the size of the table. */ #define COSINE_LUT_INCR_DEG 5 #define COSINE_LUT_SIZE ((180 / COSINE_LUT_INCR_DEG) + 1) diff --git a/common/motion_sense.c b/common/motion_sense.c index 85cce294c9..c1bf6b8930 100644 --- a/common/motion_sense.c +++ b/common/motion_sense.c @@ -7,6 +7,7 @@ #include "accelgyro.h" #include "atomic.h" +#include "body_detection.h" #include "chipset.h" #include "common.h" #include "console.h" @@ -187,6 +188,10 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor) sensor->next_collection = ts.le.lo + sensor->collection_rate; sensor->oversampling = 0; mutex_unlock(&g_sensor_mutex); +#ifdef CONFIG_BODY_DETECTION + if (sensor - motion_sensors == CONFIG_BODY_DETECTION_SENSOR) + body_detect_reset(); +#endif return 0; } @@ -444,6 +449,10 @@ static void motion_sense_shutdown(void) MOTIONSENSE_ACTIVITY_DOUBLE_TAP, 1, NULL); } #endif +#ifdef CONFIG_BODY_DETECTION + /* disable the body detection since motion sensor is down */ + body_detect_set_enable(false); +#endif } DECLARE_HOOK(HOOK_CHIPSET_SHUTDOWN, motion_sense_shutdown, MOTION_SENSE_HOOK_PRIO); @@ -459,6 +468,10 @@ static void motion_sense_suspend(void) sensor_active = SENSOR_ACTIVE_S3; +#ifdef CONFIG_BODY_DETECTION + /* disable the body detection since motion sensor is suspended */ + body_detect_set_enable(false); +#endif /* * During shutdown sequence sensor rails can be powered down * asynchronously to the EC hence EC cannot interlock the sensor @@ -659,9 +672,11 @@ static int motion_sense_process(struct motion_sensor_t *sensor, { int ret = EC_SUCCESS; int is_odr_pending = 0; + int has_data_read = 0; + int sensor_num = sensor - motion_sensors; if (*event & TASK_EVENT_MOTION_ODR_CHANGE) { - const int sensor_bit = 1 << (sensor - motion_sensors); + const int sensor_bit = 1 << sensor_num; int odr_pending = atomic_read_clear(&odr_event_required); is_odr_pending = odr_pending & sensor_bit; @@ -673,8 +688,10 @@ static int motion_sense_process(struct motion_sensor_t *sensor, if ((*event & TASK_EVENT_MOTION_INTERRUPT_MASK || is_odr_pending) && (sensor->drv->irq_handler != NULL)) { ret = sensor->drv->irq_handler(sensor, event); + if (ret == EC_SUCCESS) + has_data_read = 1; } -#endif +#endif /* CONFIG_ACCEL_INTERRUPTS */ if (motion_sensor_in_forced_mode(sensor)) { if (motion_sensor_time_to_read(ts, sensor)) { ret = motion_sense_read(sensor); @@ -683,8 +700,10 @@ static int motion_sense_process(struct motion_sensor_t *sensor, ret = EC_ERROR_BUSY; } - if (ret == EC_SUCCESS) + if (ret == EC_SUCCESS) { motion_sense_push_raw_xyz(sensor); + has_data_read = 1; + } } if (IS_ENABLED(CONFIG_ACCEL_FIFO) && *event & TASK_EVENT_MOTION_FLUSH_PENDING) { @@ -704,6 +723,17 @@ static int motion_sense_process(struct motion_sensor_t *sensor, motion_sense_fifo_insert_async_event( sensor, ASYNC_EVENT_ODR); } + if (has_data_read) { +#ifdef CONFIG_GESTURE_SW_DETECTION + /* Run gesture recognition engine */ + if (sensor_num == CONFIG_GESTURE_SENSOR_DOUBLE_TAP) + gesture_calc(event); +#endif +#ifdef CONFIG_BODY_DETECTION + if (sensor_num == CONFIG_BODY_DETECTION_SENSOR) + body_detect(); +#endif + } return ret; } @@ -734,10 +764,6 @@ static void check_and_queue_gestures(uint32_t *event) const struct motion_sensor_t *sensor; #endif -#ifdef CONFIG_GESTURE_SW_DETECTION - /* Run gesture recognition engine */ - gesture_calc(event); -#endif #ifdef CONFIG_GESTURE_SENSOR_DOUBLE_TAP if (*event & TASK_EVENT_MOTION_ACTIVITY_INTERRUPT( MOTIONSENSE_ACTIVITY_DOUBLE_TAP)) { @@ -1168,8 +1194,8 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args) return EC_RES_INVALID_COMMAND; if (sensor->drv->set_range(sensor, - in->sensor_range.data, - in->sensor_range.roundup) + in->sensor_range.data, + in->sensor_range.roundup) != EC_SUCCESS) { return EC_RES_INVALID_PARAM; } @@ -1350,6 +1376,15 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args) out->list_activities.disabled |= disabled; } } +#ifdef CONFIG_BODY_DETECTION + if (body_detect_get_enable()) { + out->list_activities.enabled |= + BIT(MOTIONSENSE_ACTIVITY_BODY_DETECTION); + } else { + out->list_activities.disabled |= + BIT(MOTIONSENSE_ACTIVITY_BODY_DETECTION); + } +#endif if (ret != EC_RES_SUCCESS) return ret; args->response_size = sizeof(out->list_activities); @@ -1372,11 +1407,33 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args) in->set_activity.enable, &in->set_activity); } +#ifdef CONFIG_BODY_DETECTION + if (in->set_activity.activity == + MOTIONSENSE_ACTIVITY_BODY_DETECTION) + body_detect_set_enable(in->set_activity.enable); +#endif if (ret != EC_RES_SUCCESS) return ret; args->response_size = 0; break; } + case MOTIONSENSE_CMD_GET_ACTIVITY: { + switch (in->get_activity.activity) { +#ifdef CONFIG_BODY_DETECTION + case MOTIONSENSE_ACTIVITY_BODY_DETECTION: + out->get_activity.state = (uint8_t) + body_detect_get_state(); + ret = EC_RES_SUCCESS; + break; +#endif + default: + ret = EC_RES_INVALID_PARAM; + } + if (ret != EC_RES_SUCCESS) + return ret; + args->response_size = sizeof(out->get_activity); + break; + } #endif /* defined(CONFIG_GESTURE_HOST_DETECTION) */ #ifdef CONFIG_ACCEL_SPOOF_MODE diff --git a/include/body_detection.h b/include/body_detection.h new file mode 100644 index 0000000000..d3c1865e36 --- /dev/null +++ b/include/body_detection.h @@ -0,0 +1,33 @@ +/* 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. + */ + +#ifndef __CROS_EC_BODY_DETECTION_H +#define __CROS_EC_BODY_DETECTION_H + +#include <stdint.h> + +enum body_detect_states { + BODY_DETECTION_OFF_BODY, + BODY_DETECTION_ON_BODY +}; + +/* get/set the state of body detection */ +enum body_detect_states body_detect_get_state(void); +void body_detect_change_state(enum body_detect_states state); + +/* Reset the data. This should be called when ODR is changed*/ +void body_detect_reset(void); + +/* Body detect main function. This should be called when new sensor data come */ +void body_detect(void); + +/* enable/disable body detection */ +void body_detect_set_enable(int enable); + +/* get enable state of body detection */ +int body_detect_get_enable(void); + + +#endif /* __CROS_EC_BODY_DETECTION_H */ diff --git a/include/math_util.h b/include/math_util.h index 6b60d4a1d6..266bde6743 100644 --- a/include/math_util.h +++ b/include/math_util.h @@ -48,6 +48,9 @@ typedef int64_t fp_inter_t; #endif +/* Some useful math functions. Use with integers only! */ +#define POW2(x) ((x) * (x)) + /* * Fixed-point addition and subtraction can be done directly, because they * work identically. diff --git a/util/ectool.c b/util/ectool.c index be8ceae1db..963a04f2d6 100644 --- a/util/ectool.c +++ b/util/ectool.c @@ -5541,6 +5541,19 @@ static int cmd_motionsense(int argc, char **argv) return rv; return 0; } + if (argc == 4 && !strcasecmp(argv[1], "get_activity")) { + param.cmd = MOTIONSENSE_CMD_GET_ACTIVITY; + param.get_activity.sensor_num = strtol(argv[2], &e, 0); + param.get_activity.activity = strtol(argv[3], &e, 0); + + rv = ec_command(EC_CMD_MOTION_SENSE_CMD, 2, + ¶m, ms_command_sizes[param.cmd].outsize, + resp, ms_command_sizes[param.cmd].insize); + if (rv < 0) + return rv; + printf("State: %d\n", resp->get_activity.state); + return 0; + } if (argc == 2 && !strcasecmp(argv[1], "lid_angle")) { param.cmd = MOTIONSENSE_CMD_LID_ANGLE; |