rsgltzyd/HeavenlyHeroStar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 增强运动手势的校准与检测逻辑

Browse Source
This commit is contained in:
雨霖铃
2026-07-08 22:18:42 +08:00
parent efb3c8f304
commit b154c3fa20
2 changed files with 150 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main/app/motion_gesture.c
+147 -43
View File
@@ -7,20 +7,38 @@
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"
#include "lvgl.h"
#include <assert.h>
#include <math.h>
static const char *TAG = "motion";
#define SAMPLE_PERIOD_MS 10
#define SAMPLE_RATE_HZ 100
/* Calibration: keep a sliding window of the last 500 ms and finish only when
* the window is stable (low variance). This avoids calibrating while the user
* is picking up or moving the device. */
#define CALIBRATION_MS 500
#define CALIBRATION_WINDOW_SAMPLES (CALIBRATION_MS / SAMPLE_PERIOD_MS)
#define CALIBRATION_MAX_VAR_G2 0.010f
/* Shake is detected on the horizontal plane (X/Y) so that picking the device
* up or putting it down (Z-axis jolt) does not trigger navigation. */
#define THRESHOLD_LOW_G 0.6f
#define THRESHOLD_MED_G 0.8f
#define THRESHOLD_HIGH_G 1.0f
/* A valid shake must produce enough strong samples within a short window. */
#define WINDOW_MS 300
#define MIN_SAMPLES 5
#define COOLDOWN_MS 800
#define CALIBRATION_MS 500
/* A real shake oscillates; require at least two zero-crossings on the
* dominant horizontal axis to distinguish it from a single jolt. */
#define MIN_ZERO_CROSSINGS 2
/* Cooldown prevents one physical shake from generating multiple events. */
#define COOLDOWN_MS 350
typedef enum {
GESTURE_IDLE = 0,
@@ -32,20 +50,29 @@ static bool s_enabled = true;
static float s_threshold = THRESHOLD_MED_G;
static gesture_state_t s_state = GESTURE_IDLE;
/* Baseline calibration */
static float s_ref_x;
static float s_ref_y;
static float s_ref_z;
static bool s_calibrated;
static int64_t s_cal_start_us;
static int s_cal_count;
static float s_cal_sum_x;
static float s_cal_sum_y;
static float s_cal_sum_z;
static float s_cal_buf_x[CALIBRATION_WINDOW_SAMPLES];
static float s_cal_buf_y[CALIBRATION_WINDOW_SAMPLES];
static float s_cal_buf_z[CALIBRATION_WINDOW_SAMPLES];
static int s_cal_head;
static int s_cal_count;
/* Detection window state */
static int s_over_count;
static int64_t s_window_start_us;
static int s_zero_crossings;
static int s_last_x_sign;
static int s_last_y_sign;
static int64_t s_cooldown_until_us;
/* Last horizontal magnitude, exposed for UI/debug. */
static float s_last_horizontal_mag;
static void nav_handler_task(void *arg)
{
(void)arg;
@@ -58,11 +85,6 @@ static void nav_handler_task(void *arg)
}
}
static float magnitude(float ax, float ay, float az)
{
return sqrtf(ax * ax + ay * ay + az * az);
}
static void post_nav_next(void)
{
motion_nav_event_t ev = MOTION_NAV_NEXT;
@@ -73,30 +95,78 @@ static void post_nav_next(void)
}
}
static void run_calibration(float ax, float ay, float az)
{
s_cal_sum_x += ax;
s_cal_sum_y += ay;
s_cal_sum_z += az;
s_cal_count++;
int64_t elapsed_us = esp_timer_get_time() - s_cal_start_us;
if (elapsed_us >= (int64_t)CALIBRATION_MS * 1000) {
if (s_cal_count > 0) {
s_ref_x = s_cal_sum_x / (float)s_cal_count;
s_ref_y = s_cal_sum_y / (float)s_cal_count;
s_ref_z = s_cal_sum_z / (float)s_cal_count;
}
s_calibrated = true;
ESP_LOGI(TAG, "Baseline calibrated (ref=%.3f,%.3f,%.3f g)", s_ref_x, s_ref_y,
s_ref_z);
}
}
static void reset_window(void)
{
s_over_count = 0;
s_window_start_us = 0;
s_zero_crossings = 0;
s_last_x_sign = 0;
s_last_y_sign = 0;
}
static void run_calibration(float ax, float ay, float az)
{
s_cal_buf_x[s_cal_head] = ax;
s_cal_buf_y[s_cal_head] = ay;
s_cal_buf_z[s_cal_head] = az;
s_cal_head = (s_cal_head + 1) % CALIBRATION_WINDOW_SAMPLES;
if (s_cal_count < CALIBRATION_WINDOW_SAMPLES) {
s_cal_count++;
}
if (s_cal_count < CALIBRATION_WINDOW_SAMPLES) {
return;
}
float sum_x = 0.0f;
float sum_y = 0.0f;
float sum_z = 0.0f;
float sum_sq_x = 0.0f;
float sum_sq_y = 0.0f;
float sum_sq_z = 0.0f;
for (int i = 0; i < CALIBRATION_WINDOW_SAMPLES; i++) {
sum_x += s_cal_buf_x[i];
sum_y += s_cal_buf_y[i];
sum_z += s_cal_buf_z[i];
sum_sq_x += s_cal_buf_x[i] * s_cal_buf_x[i];
sum_sq_y += s_cal_buf_y[i] * s_cal_buf_y[i];
sum_sq_z += s_cal_buf_z[i] * s_cal_buf_z[i];
}
const int n = CALIBRATION_WINDOW_SAMPLES;
float var_x = (sum_sq_x / n) - (sum_x / n) * (sum_x / n);
float var_y = (sum_sq_y / n) - (sum_y / n) * (sum_y / n);
float var_z = (sum_sq_z / n) - (sum_z / n) * (sum_z / n);
float max_var = fmaxf(var_x, fmaxf(var_y, var_z));
if (max_var > CALIBRATION_MAX_VAR_G2) {
static int64_t s_last_cal_warn_us;
int64_t now_us = esp_timer_get_time();
if ((now_us - s_last_cal_warn_us) >= 1000 * 1000) {
s_last_cal_warn_us = now_us;
ESP_LOGW(TAG, "Calibration: motion detected (var=%.3f), waiting", max_var);
}
return;
}
s_ref_x = sum_x / n;
s_ref_y = sum_y / n;
s_ref_z = sum_z / n;
s_calibrated = true;
ESP_LOGI(TAG, "Baseline calibrated (ref=%.3f,%.3f,%.3f g)", s_ref_x, s_ref_y, s_ref_z);
}
static int axis_sign(float value)
{
if (value > 0.0f) {
return 1;
}
if (value < 0.0f) {
return -1;
}
return 0;
}
static void process_sample(float ax, float ay, float az)
@@ -108,6 +178,15 @@ static void process_sample(float ax, float ay, float az)
return;
}
float dx = ax - s_ref_x;
float dy = ay - s_ref_y;
float dz = az - s_ref_z;
/* Use only horizontal magnitude for shake detection. Z-axis is dominated by
* gravity and is the main axis for pick-up/put-down false triggers. */
float horizontal_mag = sqrtf(dx * dx + dy * dy);
s_last_horizontal_mag = horizontal_mag;
if (s_state == GESTURE_COOLDOWN) {
if (now_us >= s_cooldown_until_us) {
s_state = GESTURE_IDLE;
@@ -116,24 +195,34 @@ static void process_sample(float ax, float ay, float az)
return;
}
float dx = ax - s_ref_x;
float dy = ay - s_ref_y;
float dz = az - s_ref_z;
float mag = magnitude(dx, dy, dz);
if (mag > s_threshold) {
if (horizontal_mag > s_threshold) {
if (s_over_count == 0) {
s_window_start_us = now_us;
}
s_over_count++;
int x_sign = axis_sign(dx);
int y_sign = axis_sign(dy);
if (s_last_x_sign != 0 && x_sign != s_last_x_sign) {
s_zero_crossings++;
}
if (s_last_y_sign != 0 && y_sign != s_last_y_sign) {
s_zero_crossings++;
}
s_last_x_sign = x_sign;
s_last_y_sign = y_sign;
int64_t window_us = (int64_t)WINDOW_MS * 1000;
if ((now_us - s_window_start_us) > window_us) {
s_over_count = 1;
/* Window expired: start fresh with the current sample. */
reset_window();
s_window_start_us = now_us;
s_over_count = 1;
s_last_x_sign = axis_sign(dx);
s_last_y_sign = axis_sign(dy);
}
if (s_over_count >= MIN_SAMPLES) {
if (s_over_count >= MIN_SAMPLES && s_zero_crossings >= MIN_ZERO_CROSSINGS) {
if (s_enabled) {
post_nav_next();
}
@@ -147,6 +236,8 @@ static void process_sample(float ax, float ay, float az)
reset_window();
}
}
(void)dz;
}
void motion_gesture_init(motion_nav_cb_t cb)
@@ -156,7 +247,10 @@ void motion_gesture_init(motion_nav_cb_t cb)
s_nav_queue = xQueueCreate(4, sizeof(motion_nav_event_t));
xTaskCreate(nav_handler_task, "nav_handler", 2048, NULL, 5, NULL);
s_cal_start_us = esp_timer_get_time();
s_calibrated = false;
s_cal_head = 0;
s_cal_count = 0;
ESP_LOGI(TAG, "Motion gesture ready (calibrating %d ms)", CALIBRATION_MS);
}
@@ -179,7 +273,7 @@ void motion_gesture_set_sensitivity(shake_sensitivity_t sens)
s_threshold = THRESHOLD_HIGH_G;
break;
default:
LV_ASSERT_MSG(false, "unexpected shake_sensitivity_t");
assert(false && "unexpected shake_sensitivity_t");
break;
}
ESP_LOGI(TAG, "Sensitivity set (threshold=%.2f g)", s_threshold);
@@ -189,3 +283,13 @@ void motion_gesture_on_sample(float ax_g, float ay_g, float az_g)
{
process_sample(ax_g, ay_g, az_g);
}
float motion_gesture_get_last_magnitude(void)
{
return s_last_horizontal_mag;
}
bool motion_gesture_is_calibrated(void)
{
return s_calibrated;
}