feat: 实现摇一摇导航功能

2026-07-05 21:28:54 +08:00
parent e5e1d5b85e
commit ff7d82286a
16 changed files with 727 additions and 0 deletions
@@ -0,0 +1,191 @@
+#include "motion_gesture.h"
+
+#include "ui_nav.h"
+
+#include "esp_log.h"
+#include "esp_timer.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+#include "freertos/task.h"
+#include "lvgl.h"
+
+#include <math.h>
+
+static const char *TAG = "motion";
+
+#define SAMPLE_RATE_HZ           100
+#define THRESHOLD_LOW_G          0.6f
+#define THRESHOLD_MED_G          0.8f
+#define THRESHOLD_HIGH_G         1.0f
+#define WINDOW_MS                300
+#define MIN_SAMPLES              5
+#define COOLDOWN_MS              800
+#define CALIBRATION_MS           500
+
+typedef enum {
+    GESTURE_IDLE = 0,
+    GESTURE_COOLDOWN,
+} gesture_state_t;
+
+static QueueHandle_t s_nav_queue;
+static bool s_enabled = true;
+static float s_threshold = THRESHOLD_MED_G;
+static gesture_state_t s_state = GESTURE_IDLE;
+
+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 int s_over_count;
+static int64_t s_window_start_us;
+static int64_t s_cooldown_until_us;
+
+static void nav_handler_task(void *arg)
+{
+    (void)arg;
+    motion_nav_event_t ev;
+
+    while (xQueueReceive(s_nav_queue, &ev, portMAX_DELAY) == pdTRUE) {
+        if (ev == MOTION_NAV_NEXT) {
+            ui_nav_next();
+        }
+    }
+}
+
+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;
+    if (xQueueSend(s_nav_queue, &ev, 0) != pdTRUE) {
+        ESP_LOGW(TAG, "nav queue full");
+    } else {
+        ESP_LOGI(TAG, "Shake detected -> next screen");
+    }
+}
+
+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;
+}
+
+static void process_sample(float ax, float ay, float az)
+{
+    int64_t now_us = esp_timer_get_time();
+
+    if (!s_calibrated) {
+        run_calibration(ax, ay, az);
+        return;
+    }
+
+    if (s_state == GESTURE_COOLDOWN) {
+        if (now_us >= s_cooldown_until_us) {
+            s_state = GESTURE_IDLE;
+            reset_window();
+        }
+        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 (s_over_count == 0) {
+            s_window_start_us = now_us;
+        }
+        s_over_count++;
+
+        int64_t window_us = (int64_t)WINDOW_MS * 1000;
+        if ((now_us - s_window_start_us) > window_us) {
+            s_over_count = 1;
+            s_window_start_us = now_us;
+        }
+
+        if (s_over_count >= MIN_SAMPLES) {
+            if (s_enabled) {
+                post_nav_next();
+            }
+            s_state = GESTURE_COOLDOWN;
+            s_cooldown_until_us = now_us + (int64_t)COOLDOWN_MS * 1000;
+            reset_window();
+        }
+    } else if (s_over_count > 0) {
+        int64_t window_us = (int64_t)WINDOW_MS * 1000;
+        if ((now_us - s_window_start_us) > window_us) {
+            reset_window();
+        }
+    }
+}
+
+void motion_gesture_init(motion_nav_cb_t cb)
+{
+    (void)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();
+    ESP_LOGI(TAG, "Motion gesture ready (calibrating %d ms)", CALIBRATION_MS);
+}
+
+void motion_gesture_set_enabled(bool enabled)
+{
+    s_enabled = enabled;
+    ESP_LOGI(TAG, "Shake navigation %s", enabled ? "enabled" : "disabled");
+}
+
+void motion_gesture_set_sensitivity(shake_sensitivity_t sens)
+{
+    switch (sens) {
+    case SHAKE_SENS_LOW:
+        s_threshold = THRESHOLD_LOW_G;
+        break;
+    case SHAKE_SENS_MED:
+        s_threshold = THRESHOLD_MED_G;
+        break;
+    case SHAKE_SENS_HIGH:
+        s_threshold = THRESHOLD_HIGH_G;
+        break;
+    default:
+        LV_ASSERT_MSG(false, "unexpected shake_sensitivity_t");
+        break;
+    }
+    ESP_LOGI(TAG, "Sensitivity set (threshold=%.2f g)", s_threshold);
+}
+
+void motion_gesture_on_sample(float ax_g, float ay_g, float az_g)
+{
+    process_sample(ax_g, ay_g, az_g);
+}