1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
|
/* 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
|