56 lines
1.3 KiB
C
56 lines
1.3 KiB
C
|
#include "imu/imu_manager.h"
|
||
|
|
|
||
|
|
#include <math.h>
|
||
|
|
#include "esp_log.h"
|
||
|
|
#include "esp_timer.h"
|
||
|
|
#include "freertos/FreeRTOS.h"
|
||
|
|
#include "freertos/task.h"
|
||
|
|
#include "imu/I2C_Driver.h"
|
||
|
|
#include "imu/QMI8658.h"
|
||
|
|
|
||
|
|
static const char *TAG = "imu";
|
||
|
|
|
||
|
|
static imu_shake_cb_t s_shake_cb = NULL;
|
||
|
|
static int64_t s_last_shake_us = 0;
|
||
|
|
|
||
|
|
static void imu_task(void *arg)
|
||
|
|
{
|
||
|
|
(void)arg;
|
||
|
|
const float threshold_g = 1.6f;
|
||
|
|
const int64_t min_interval_us = 800000;
|
||
|
|
|
||
|
|
while (1) {
|
||
|
|
QMI8658_Loop();
|
||
|
|
float ax = Accel.x;
|
||
|
|
float ay = Accel.y;
|
||
|
|
float az = Accel.z;
|
||
|
|
float mag = sqrtf(ax * ax + ay * ay + az * az);
|
||
|
|
float delta = fabsf(mag - 1.0f);
|
||
|
|
|
||
|
|
if (delta > threshold_g) {
|
||
|
|
int64_t now = esp_timer_get_time();
|
||
|
|
if (now - s_last_shake_us > min_interval_us) {
|
||
|
|
s_last_shake_us = now;
|
||
|
|
ESP_LOGI(TAG, "Shake detected (delta=%.2f)", delta);
|
||
|
|
if (s_shake_cb) {
|
||
|
|
s_shake_cb();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(50));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void imu_manager_init(void)
|
||
|
|
{
|
||
|
|
I2C_Init();
|
||
|
|
QMI8658_Init();
|
||
|
|
xTaskCreatePinnedToCore(imu_task, "imu_task", 4096, NULL, 4, NULL, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void imu_manager_set_shake_callback(imu_shake_cb_t cb)
|
||
|
|
{
|
||
|
|
s_shake_cb = cb;
|
||
|
|
}
|