55
main/imu/imu_manager.c
Normal file
55
main/imu/imu_manager.c
Normal file
@@ -0,0 +1,55 @@
|
||||
#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;
|
||||
}
|
||||
Reference in New Issue
Block a user