214 lines
6.7 KiB
C++
214 lines
6.7 KiB
C++
/**
|
|
* @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 <math.h>
|
|
|
|
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;
|
|
}
|