diff --git a/main/app/motion_gesture.c b/main/app/motion_gesture.c index 3fa49b0..ca063c0 100644 --- a/main/app/motion_gesture.c +++ b/main/app/motion_gesture.c @@ -7,20 +7,38 @@ #include "freertos/FreeRTOS.h" #include "freertos/queue.h" #include "freertos/task.h" -#include "lvgl.h" +#include #include 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; +} diff --git a/main/app/motion_gesture.h b/main/app/motion_gesture.h index 64bc12f..c3b460e 100644 --- a/main/app/motion_gesture.h +++ b/main/app/motion_gesture.h @@ -14,3 +14,6 @@ void motion_gesture_init(motion_nav_cb_t cb); void motion_gesture_set_enabled(bool enabled); void motion_gesture_set_sensitivity(shake_sensitivity_t sens); void motion_gesture_on_sample(float ax_g, float ay_g, float az_g); + +float motion_gesture_get_last_magnitude(void); +bool motion_gesture_is_calibrated(void);