/** * @file shake_detector.cpp * @brief Shake detection implementation using QMI8658 IMU sensor * * This module implements shake detection using the SensorLib QMI8658 driver. * It performs baseline calibration at startup and detects acceleration * deviations that indicate device movement or shaking. */ #include "sensors/shake_detector.h" #include "SensorQMI8658.hpp" #include "config/hardware_config.h" #include "esp_log.h" #include "esp_timer.h" #include #include static const char *TAG = "shake_detector"; // ---------------------------------------------------------------------------- // Private State // ---------------------------------------------------------------------------- static SensorQMI8658 qmiSensor; static uint32_t lastTriggerTime = 0; static float lastAccelX = 0, lastAccelY = 0, lastAccelZ = 0; static float baselineX = 0, baselineY = 0, baselineZ = 0; static bool baselineReady = false; // ---------------------------------------------------------------------------- // Helper Functions // ---------------------------------------------------------------------------- static uint32_t getMillis() { return (uint32_t)(esp_timer_get_time() / 1000); } // ---------------------------------------------------------------------------- // Public API Implementation // ---------------------------------------------------------------------------- extern "C" bool shakeDetectorInit(void) { ESP_LOGI(TAG, "Initializing shake detector (QMI8658)"); // Initialize sensor (I2C driver should already be installed by display // driver) if (!qmiSensor.begin(QMI8658_I2C_PORT, QMI8658_I2C_ADDR, QMI8658_PIN_SDA, QMI8658_PIN_SCL)) { ESP_LOGE(TAG, "Failed to initialize QMI8658"); return false; } uint8_t chipId = qmiSensor.getChipID(); ESP_LOGI(TAG, "Sensor initialized. Chip ID: 0x%02x", chipId); // Configure accelerometer (4G range, 1000Hz ODR) ESP_LOGI(TAG, "Configuring accelerometer..."); int retAccel = qmiSensor.configAccelerometer(SensorQMI8658::ACC_RANGE_4G, SensorQMI8658::ACC_ODR_1000Hz, SensorQMI8658::LPF_MODE_0, true); if (retAccel != 0) { ESP_LOGE(TAG, "Accelerometer config failed: %d", retAccel); return false; } ESP_LOGI(TAG, "Accelerometer configured"); // Configure gyroscope (64DPS range, 896.8Hz ODR) ESP_LOGI(TAG, "Configuring gyroscope..."); int retGyro = qmiSensor.configGyroscope(SensorQMI8658::GYR_RANGE_64DPS, SensorQMI8658::GYR_ODR_896_8Hz, SensorQMI8658::LPF_MODE_3, true); if (retGyro != 0) { ESP_LOGW(TAG, "Gyroscope config failed: %d (continuing)", retGyro); } else { ESP_LOGI(TAG, "Gyroscope configured"); } // Enable sensors qmiSensor.enableGyroscope(); qmiSensor.enableAccelerometer(); ESP_LOGI(TAG, "Sensors enabled"); // Verify sensor is responding vTaskDelay(pdMS_TO_TICKS(50)); if (qmiSensor.getDataReady()) { float testX, testY, testZ; if (qmiSensor.getAccelerometer(testX, testY, testZ)) { ESP_LOGI(TAG, "First reading: x=%.3f y=%.3f z=%.3f", testX, testY, testZ); } } baselineReady = false; ESP_LOGI(TAG, "Shake detector ready. Calibrating baseline..."); return true; } extern "C" ShakeEvent shakeDetectorUpdate(void) { uint32_t now = getMillis(); if (!qmiSensor.getDataReady()) { static uint32_t lastNotReadyLog = 0; if (now - lastNotReadyLog > 3000) { ESP_LOGW(TAG, "Sensor data not ready"); lastNotReadyLog = now; } return SHAKE_NONE; } float x, y, z; if (!qmiSensor.getAccelerometer(x, y, z)) { static uint32_t lastReadFailLog = 0; if (now - lastReadFailLog > 3000) { ESP_LOGW(TAG, "Failed to read accelerometer"); lastReadFailLog = now; } return SHAKE_NONE; } lastAccelX = x; lastAccelY = y; lastAccelZ = z; // Baseline calibration phase if (!baselineReady) { static uint32_t baselineCount = 0; static float baselineSumX = 0, baselineSumY = 0, baselineSumZ = 0; baselineSumX += x; baselineSumY += y; baselineSumZ += z; baselineCount++; ESP_LOGI(TAG, "[%2lu/%d] x=%+.3f y=%+.3f z=%+.3f", (unsigned long)baselineCount, SHAKE_BASELINE_SAMPLES, x, y, z); if (baselineCount >= SHAKE_BASELINE_SAMPLES) { baselineX = baselineSumX / SHAKE_BASELINE_SAMPLES; baselineY = baselineSumY / SHAKE_BASELINE_SAMPLES; baselineZ = baselineSumZ / SHAKE_BASELINE_SAMPLES; baselineReady = true; // Reset for potential future recalibration baselineCount = 0; baselineSumX = baselineSumY = baselineSumZ = 0; float magnitude = sqrtf(baselineX * baselineX + baselineY * baselineY + baselineZ * baselineZ); ESP_LOGI(TAG, "Baseline calibrated: x=%+.3f y=%+.3f z=%+.3f (mag: %.3f)", baselineX, baselineY, baselineZ, magnitude); } return SHAKE_NONE; } // Calculate deviation from baseline float deltaX = x - baselineX; float deltaY = y - baselineY; float deltaZ = z - baselineZ; float deviation = sqrtf(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ); // Periodic logging for monitoring static uint32_t lastLogTime = 0; if (now - lastLogTime > 1000) { ESP_LOGI(TAG, "ACCEL: x=%+.3f y=%+.3f z=%+.3f | deviation=%.3f", x, y, z, deviation); lastLogTime = now; } // Debounce check if (now - lastTriggerTime < SHAKE_DEBOUNCE_MS) { return SHAKE_NONE; } // Shake detection if (deviation > SHAKE_ACCEL_THRESHOLD) { ESP_LOGI(TAG, "SHAKE DETECTED! deviation=%.3f > threshold=%.3f", deviation, SHAKE_ACCEL_THRESHOLD); lastTriggerTime = now; // Random emotion selection 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 shakeDetectorGetRawAccel(float *x, float *y, float *z) { if (!qmiSensor.getDataReady()) { return false; } if (!qmiSensor.getAccelerometer(*x, *y, *z)) { return false; } lastAccelX = *x; lastAccelY = *y; lastAccelZ = *z; return true; }