215 lines
6.9 KiB
C++
215 lines
6.9 KiB
C++
#ifndef GYROSCOPE_H
|
||
#define GYROSCOPE_H
|
||
|
||
#include <esp_log.h>
|
||
#include <esp_check.h>
|
||
#include <esp_timer.h>
|
||
#include <driver/gpio.h>
|
||
#include "i2c_bus.h"
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/event_groups.h"
|
||
#include <string.h>
|
||
#include <stdio.h>
|
||
#include <math.h>
|
||
#include <freertos/FreeRTOS.h>
|
||
#include <freertos/task.h>
|
||
#include <freertos/queue.h>
|
||
#include <freertos/semphr.h>
|
||
#include "esp_err.h"
|
||
#include "config.h"
|
||
#include "io_expansion_board.h"
|
||
#include <functional>
|
||
#include "esp_timer.h"
|
||
|
||
// 加速度数据结构体
|
||
typedef struct {
|
||
float x; // X轴加速度 (g)
|
||
float y; // Y轴加速度 (g)
|
||
float z; // Z轴加速度 (g)
|
||
} AccelerationData;
|
||
|
||
// 一阶滤波系数
|
||
#define WEIGHTFACTOR 0.93f
|
||
|
||
/* 陀螺仪寄存器定义 */
|
||
#define CTRL1_XL 0x10 // 加速度计控制寄存器1
|
||
#define CTRL2_G 0x11 // 陀螺仪控制寄存器2
|
||
#define CTRL3_C 0x12 // 控制寄存器3
|
||
#define CTRL4_C 0x13 // 控制寄存器4
|
||
#define CTRL5_C 0x14 // 控制寄存器5
|
||
#define CTRL6_C 0x15 // 控制寄存器6
|
||
#define CTRL7_G 0x16 // 陀螺仪控制寄存器7
|
||
#define CTRL8_XL 0x17 // 加速度计控制寄存器8
|
||
#define CTRL9_XL 0x18 // 加速度计控制寄存器9
|
||
#define CTRL10_C 0x19 // 控制寄存器10
|
||
#define INT1_CTRL 0x0D // 中断1控制寄存器
|
||
#define WAKE_UP_DUR 0x5B // 唤醒持续时间寄存器
|
||
#define WAKE_UP_THS 0x5C // 唤醒阈值寄存器
|
||
#define TAP_CFG 0x58 // 敲击配置寄存器
|
||
#define TAP_THS_6D 0x59 // 6D方向阈值寄存器
|
||
#define INT_DUR2 0x5A // 中断持续时间寄存器
|
||
#define MD1_CFG 0x5E // 中断1配置寄存器
|
||
#define MD2_CFG 0x5F // 中断2配置寄存器
|
||
#define FUNC_SRC1 0x53 // 功能源寄存器1
|
||
#define WAKE_UP_SRC 0x1B // 唤醒源寄存器
|
||
|
||
#define ACCEL_X_L 0x28 // OUTX_L_XL
|
||
#define ACCEL_X_H 0x29 // OUTX_H_XL
|
||
#define ACCEL_Y_L 0x2A // OUTY_L_XL
|
||
#define ACCEL_Y_H 0x2B // OUTY_H_XL
|
||
#define ACCEL_Z_L 0x2C // OUTZ_L_XL
|
||
#define ACCEL_Z_H 0x2D // OUTZ_H_XL
|
||
|
||
#define ACCEL_SENSITIVITY_2G 0.061f // 2g 灵敏度 (g/LSB)
|
||
|
||
#define I2C_MASTER_TIMEOUT_MS 1000 // I2C主设备超时时间(毫秒)
|
||
|
||
/**
|
||
* @class Gyroscope
|
||
* @brief 陀螺仪驱动类,封装芯片操作
|
||
*/
|
||
#define TAG_GYROSCOPE "Gyroscope"
|
||
class Gyroscope
|
||
{
|
||
public:
|
||
/**
|
||
* @brief 构造函数
|
||
*/
|
||
Gyroscope();
|
||
|
||
/**
|
||
* @brief 析构函数
|
||
*/
|
||
~Gyroscope();
|
||
|
||
/**
|
||
* @brief 初始化中断引脚
|
||
* @param gpio_num 中断引脚编号
|
||
* @return esp_err_t 初始化结果
|
||
*/
|
||
esp_err_t InterruptGpioInit(gpio_num_t gpio_num);
|
||
|
||
/**
|
||
* @brief 创建陀螺仪读取任务
|
||
*/
|
||
void CreateGyroReadTask();
|
||
|
||
/**
|
||
* @brief 初始化陀螺仪
|
||
* @return esp_err_t 初始化结果
|
||
*/
|
||
esp_err_t Init(i2c_bus_handle_t bus_handle);
|
||
|
||
/**
|
||
* @brief 清除中断标志
|
||
* @return esp_err_t 清除结果
|
||
*/
|
||
esp_err_t ClearInterruptFlags();
|
||
|
||
|
||
/**
|
||
* @brief 获取当前加速度数据
|
||
* @param data 指向AccelerationData结构体的指针,用于存储读取到的加速度数据
|
||
* @return esp_err_t 获取结果
|
||
*/
|
||
esp_err_t GetAcceleration(AccelerationData* data);
|
||
|
||
/**
|
||
* @brief 创建500ms周期定时器
|
||
* @return esp_err_t 创建结果
|
||
*/
|
||
esp_err_t CreateTimer();
|
||
|
||
/**
|
||
* @brief 停止并删除定时器
|
||
*/
|
||
void DeleteTimer();
|
||
|
||
/**
|
||
* @brief 注册摇晃检测回调函数
|
||
* @param callback 当检测到摇晃时调用的回调函数
|
||
*/
|
||
void OnShake(std::function<void()> callback);
|
||
|
||
/**
|
||
* @brief 注册拿起检测回调函数
|
||
* @param callback 当检测到设备被拿起时调用的回调函数
|
||
*/
|
||
void OnPickUp(std::function<void()> callback);
|
||
|
||
/**
|
||
* @brief 注册放置检测回调函数
|
||
* @param callback 当检测到设备被放置时调用的回调函数
|
||
*/
|
||
void OnPutDown(std::function<void()> callback);
|
||
|
||
/**
|
||
* @brief 注册倾斜检测回调函数
|
||
* @param callback 当检测到设备倾斜时调用的回调函数
|
||
*/
|
||
void OnTilt(std::function<void()> callback);
|
||
|
||
i2c_bus_handle_t bus_handle_ = nullptr; // I2C总线句柄
|
||
volatile uint8_t interrupt_count_ = 0; // 中断计数
|
||
volatile bool interrupt_flag_ = false; // 中断标志位)(判断是否需要处理)
|
||
|
||
private:
|
||
static void GpioIsrHandler(void* arg); // 中断处理函数
|
||
static void GyroReadTask(void* pvParameters);
|
||
static void TimerCallback(void* arg); // 定时器回调函数
|
||
|
||
void TimerInterruptHandler(); // 定时器中断处理函数
|
||
|
||
esp_err_t WriteRegister(uint8_t reg, uint8_t data);
|
||
esp_err_t ReadRegister(uint8_t reg, uint8_t* data);
|
||
esp_err_t ReadRegisters(uint8_t reg, uint8_t* data, size_t length);
|
||
|
||
/**
|
||
* @brief 读取原始加速度数据
|
||
* @param x 指向X轴加速度存储的指针
|
||
* @param y 指向Y轴加速度存储的指针
|
||
* @param z 指向Z轴加速度存储的指针
|
||
* @return esp_err_t 读取结果
|
||
*/
|
||
esp_err_t ReadRawAcceleration(int16_t* x, int16_t* y, int16_t* z);
|
||
|
||
std::function<void()> on_shake_callback_; // 摇晃回调函数
|
||
std::function<void()> on_pickup_callback_; // 拿起回调函数
|
||
std::function<void()> on_putdown_callback_; // 放置回调函数
|
||
std::function<void()> on_tilt_callback_; // 倾斜回调函数
|
||
|
||
esp_timer_handle_t timer_handle_ = nullptr; // 定时器句柄
|
||
|
||
/**
|
||
* @brief 分配静态任务所需的内存
|
||
* @return bool 分配是否成功
|
||
*/
|
||
bool AllocateStaticTaskMemory();
|
||
|
||
/**
|
||
* @brief 释放静态任务分配的内存
|
||
*/
|
||
void FreeStaticTaskMemory();
|
||
|
||
TaskHandle_t xGyroscopeTaskHandle_ = nullptr;
|
||
StackType_t* pGyroscopeTaskStackBuffer_ = nullptr; // 任务栈指针（PSRAM）
|
||
StaticTask_t* pxGyroscopeTaskBuffer_ = nullptr; // 任务控制块指针（内部RAM）
|
||
|
||
// 滤波相关成员变量
|
||
int16_t last_filtered_x_ = 0; // 上一次滤波后的X轴值
|
||
int16_t last_filtered_y_ = 0; // 上一次滤波后的Y轴值
|
||
int16_t last_filtered_z_ = 0; // 上一次滤波后的Z轴值
|
||
bool is_first_sample_ = true; // 是否为第一次采样标志
|
||
|
||
// 状态检测相关成员变量
|
||
float accel_data_y_last_ = 0.0f; // 保存上一次Y轴数据
|
||
float total_accel_last_ = 1.0f; // 保存上一次合成加速度值，初始值为1g（平放时的典型值）
|
||
uint8_t stable_count_ = 0; // 稳定状态计数器
|
||
uint8_t lift_count_ = 0; // 拿起动作计数器
|
||
uint8_t putdown_count_ = 0; // 放置动作计数器
|
||
bool is_stable_ = false; // 是否处于稳定平放状态
|
||
bool is_lifted_ = false; // 是否处于被拿起状态
|
||
};
|
||
|
||
#endif // GYROSCOPE_H
|