/** * @file shake_detector.cpp * @brief Shake detection implementation using SensorLib QMI8658 */ #include "shake_detector.h" #include "SensorQMI8658.hpp" #include "esp_log.h" #include "esp_timer.h" #include static const char *TAG = "shake_detector"; // Sensor instance static SensorQMI8658 qmi; // I2C Pins (Matches example_qspi_with_ram.c) #define I2C_MASTER_SCL 14 #define I2C_MASTER_SDA 15 #define I2C_MASTER_NUM I2C_NUM_0 #define QMI8658_ADDR 0x6B // Shake detection thresholds #define SHAKE_ACCEL_THRESHOLD 0.5f // 0.5G deviation from baseline #define SHAKE_DEBOUNCE_MS 2000 // Debounce time between detections #define BASELINE_SAMPLES 10 // Number of samples to average for baseline static uint32_t last_trigger_time = 0; // Store latest readings and baseline static float last_acc_x = 0, last_acc_y = 0, last_acc_z = 0; static float baseline_x = 0, baseline_y = 0, baseline_z = 0; static bool baseline_ready = false; static uint32_t get_millis() { return (uint32_t)(esp_timer_get_time() / 1000); } extern "C" bool shake_detector_init(void) { ESP_LOGI(TAG, "Initializing Shake Detector (QMI8658)"); // Initialize sensor (I2C driver already installed by main app) if (!qmi.begin(I2C_MASTER_NUM, QMI8658_ADDR, I2C_MASTER_SDA, I2C_MASTER_SCL)) { ESP_LOGE(TAG, "Failed to initialize QMI8658!"); return false; } uint8_t chip_id = qmi.getChipID(); ESP_LOGI(TAG, "✓ Sensor initialized. Chip ID: 0x%02x", chip_id); // Configure accelerometer (match vendor demo settings exactly) ESP_LOGI(TAG, "Configuring accelerometer (4G, 1000Hz, LPF_MODE_0)..."); int ret_accel = qmi.configAccelerometer(SensorQMI8658::ACC_RANGE_4G, SensorQMI8658::ACC_ODR_1000Hz, SensorQMI8658::LPF_MODE_0, true); if (ret_accel != 0) { ESP_LOGE(TAG, "Failed to configure accelerometer: %d", ret_accel); return false; } ESP_LOGI(TAG, "✓ Accelerometer configured"); // Configure gyroscope (match vendor demo settings) ESP_LOGI(TAG, "Configuring gyroscope (64DPS, 896.8Hz, LPF_MODE_3)..."); int ret_gyro = qmi.configGyroscope(SensorQMI8658::GYR_RANGE_64DPS, SensorQMI8658::GYR_ODR_896_8Hz, SensorQMI8658::LPF_MODE_3, true); if (ret_gyro != 0) { ESP_LOGW(TAG, "Gyroscope config failed: %d (will continue)", ret_gyro); } else { ESP_LOGI(TAG, "✓ Gyroscope configured"); } // Enable sensors (gyro first, then accel - as in vendor demo) // Enable sensors (vendor demo doesn't check return values - they're buggy in SensorLib) // The SensorLib enable functions have backwards logic: they return bool but compare to DEV_WIRE_NONE (0) // This causes them to always appear to fail even when they succeed qmi.enableGyroscope(); qmi.enableAccelerometer(); ESP_LOGI(TAG, "✓ Gyroscope enabled"); ESP_LOGI(TAG, "✓ Accelerometer enabled"); // Test reading vTaskDelay(pdMS_TO_TICKS(50)); if (qmi.getDataReady()) { float test_x, test_y, test_z; if (qmi.getAccelerometer(test_x, test_y, test_z)) { ESP_LOGI(TAG, "✓ First reading OK: x=%.3f y=%.3f z=%.3f", test_x, test_y, test_z); } else { ESP_LOGW(TAG, "Data ready but read failed"); } } else { ESP_LOGW(TAG, "Sensor not ready for first read"); } baseline_ready = false; ESP_LOGI(TAG, "Shake Detector ready. Calibrating baseline..."); return true; } extern "C" shake_event_t shake_detector_update(void) { uint32_t now = get_millis(); if (!qmi.getDataReady()) { // Log occasionally if data is not ready, to see if sensor is stuck static uint32_t last_not_ready_time = 0; if (now - last_not_ready_time > 3000) { ESP_LOGW(TAG, "QMI8658 Data Not Ready - sensor may be unresponsive"); last_not_ready_time = now; } return SHAKE_NONE; } float x, y, z; if (!qmi.getAccelerometer(x, y, z)) { static uint32_t last_read_fail_time = 0; if (now - last_read_fail_time > 3000) { ESP_LOGW(TAG, "Failed to read accelerometer data"); last_read_fail_time = now; } return SHAKE_NONE; } last_acc_x = x; last_acc_y = y; last_acc_z = z; // Baseline calibration: collect first N samples to establish device orientation if (!baseline_ready) { static uint32_t baseline_count = 0; static float baseline_sum_x = 0, baseline_sum_y = 0, baseline_sum_z = 0; baseline_sum_x += x; baseline_sum_y += y; baseline_sum_z += z; baseline_count++; ESP_LOGI(TAG, "[%2d/10] x=%+.3f y=%+.3f z=%+.3f", baseline_count, x, y, z); if (baseline_count >= BASELINE_SAMPLES) { baseline_x = baseline_sum_x / BASELINE_SAMPLES; baseline_y = baseline_sum_y / BASELINE_SAMPLES; baseline_z = baseline_sum_z / BASELINE_SAMPLES; baseline_ready = true; baseline_count = 0; baseline_sum_x = baseline_sum_y = baseline_sum_z = 0; ESP_LOGI(TAG, "✓ Baseline: x=%+.3f y=%+.3f z=%+.3f (magnitude: %.3f)", baseline_x, baseline_y, baseline_z, sqrtf(baseline_x*baseline_x + baseline_y*baseline_y + baseline_z*baseline_z)); } return SHAKE_NONE; } // Calculate deviation from baseline float delta_x = x - baseline_x; float delta_y = y - baseline_y; float delta_z = z - baseline_z; float deviation = sqrtf(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z); // Log raw values every 1 second for monitoring static uint32_t last_log_time = 0; if (now - last_log_time > 1000) { ESP_LOGI(TAG, "ACCEL: x=%+.3f y=%+.3f z=%+.3f | Δ=%+.3f", x, y, z, deviation); last_log_time = now; } // Debounce: ignore if we just triggered a shake if (now - last_trigger_time < SHAKE_DEBOUNCE_MS) { return SHAKE_NONE; } // Detect shake: acceleration deviation exceeds threshold if (deviation > SHAKE_ACCEL_THRESHOLD) { ESP_LOGI(TAG, "SHAKE! Δ=%.3f > Thr=%.3f | accel:[%+.2f,%+.2f,%+.2f]", deviation, SHAKE_ACCEL_THRESHOLD, x, y, z); last_trigger_time = now; // Pick a random emotion for the shake int r = rand() % 3; switch (r) { case 0: ESP_LOGI(TAG, "→ Emotion: SURPRISED"); return SHAKE_SURPRISED; case 1: ESP_LOGI(TAG, "→ Emotion: SAD"); return SHAKE_SAD; case 2: ESP_LOGI(TAG, "→ Emotion: ANGRY"); return SHAKE_ANGRY; default: return SHAKE_SURPRISED; } } return SHAKE_NONE; } extern "C" bool shake_detector_get_raw_accel(float *x, float *y, float *z) { if (!qmi.getDataReady()) { return false; } if (!qmi.getAccelerometer(*x, *y, *z)) { return false; } last_acc_x = *x; last_acc_y = *y; last_acc_z = *z; return true; }