Introduces a new shake emotion animation with a shake-stop transition, including embedded Lottie animation data and a state machine for emotion transitions. Adds a non-volatile storage (NVS) driver for persistent state management, including first-boot detection, boot count, and IMU calibration storage. Updates emotion manager and related headers to support new shake emotion states, and integrates new UI components for calibration and loading animations.
291 lines
8.5 KiB
C++
291 lines
8.5 KiB
C++
/**
|
|
* @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 <cmath>
|
|
#include <cstdlib>
|
|
|
|
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;
|
|
}
|