/** * @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 float lastAccelX = 0, lastAccelY = 0, lastAccelZ = 0; static float baselineX = 0, baselineY = 0, baselineZ = 0; static bool baselineReady = false; // Continuous shake detection state static uint32_t shakeCounter = 0; static bool isShaking = 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: dev=%.3f counter=%lu shaking=%d", deviation, (unsigned long)shakeCounter, isShaking); lastLogTime = now; } // Continuous shake detection - track consecutive readings above threshold bool currentlyAboveThreshold = (deviation > SHAKE_ACCEL_THRESHOLD); if (currentlyAboveThreshold) { shakeCounter++; // If we've hit the threshold for continuous shake, start shaking state if (shakeCounter >= SHAKE_CONTINUOUS_THRESHOLD && !isShaking) { isShaking = true; ESP_LOGI(TAG, "SHAKE STARTED! (counter=%lu)", (unsigned long)shakeCounter); return SHAKE_ACTIVE; } // If already shaking, continue reporting active if (isShaking) { return SHAKE_ACTIVE; } } else { // Below threshold - reset counter shakeCounter = 0; // If we were shaking, now we've stopped if (isShaking) { isShaking = false; ESP_LOGI(TAG, "SHAKE STOPPED!"); return SHAKE_STOPPED; } } 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; } extern "C" bool shakeDetectorCalibrateBaseline(void) { ESP_LOGI(TAG, "Starting explicit baseline calibration..."); float sumX = 0, sumY = 0, sumZ = 0; int validSamples = 0; // Collect samples for calibration for (int i = 0; i < SHAKE_BASELINE_SAMPLES; i++) { // Wait for data to be ready int retries = 0; while (!qmiSensor.getDataReady() && retries < 50) { vTaskDelay(pdMS_TO_TICKS(10)); retries++; } if (retries >= 50) { ESP_LOGW(TAG, "Timeout waiting for sensor data"); continue; } float x, y, z; if (qmiSensor.getAccelerometer(x, y, z)) { sumX += x; sumY += y; sumZ += z; validSamples++; ESP_LOGI(TAG, "[%d/%d] x=%+.3f y=%+.3f z=%+.3f", validSamples, SHAKE_BASELINE_SAMPLES, x, y, z); } vTaskDelay(pdMS_TO_TICKS(50)); // 20Hz sampling } if (validSamples < SHAKE_BASELINE_SAMPLES / 2) { ESP_LOGE(TAG, "Calibration failed: only %d valid samples", validSamples); return false; } // Compute and set baseline baselineX = sumX / validSamples; baselineY = sumY / validSamples; baselineZ = sumZ / validSamples; baselineReady = true; float magnitude = sqrtf(baselineX * baselineX + baselineY * baselineY + baselineZ * baselineZ); ESP_LOGI(TAG, "Calibration complete: x=%+.3f y=%+.3f z=%+.3f (mag: %.3f)", baselineX, baselineY, baselineZ, magnitude); return true; } extern "C" void shakeDetectorSetBaseline(float x, float y, float z) { baselineX = x; baselineY = y; baselineZ = z; baselineReady = true; float magnitude = sqrtf(x * x + y * y + z * z); ESP_LOGI( TAG, "Baseline set from stored values: x=%+.3f y=%+.3f z=%+.3f (mag: %.3f)", x, y, z, magnitude); } extern "C" void shakeDetectorGetBaseline(float *x, float *y, float *z) { if (x) *x = baselineX; if (y) *y = baselineY; if (z) *z = baselineZ; }