626 lines
21 KiB
C++
626 lines
21 KiB
C++
#include "gyroscope.h"
|
||
#include "freertos/idf_additions.h"
|
||
|
||
Gyroscope::Gyroscope()
|
||
{
|
||
bus_handle_ = nullptr;
|
||
on_shake_callback_ = nullptr; // 初始化回调函数
|
||
on_pickup_callback_ = nullptr; // 初始化拿起回调函数
|
||
on_putdown_callback_ = nullptr; // 初始化放置回调函数
|
||
on_tilt_callback_ = nullptr; // 初始化倾斜回调函数
|
||
timer_handle_ = nullptr; // 初始化定时器句柄
|
||
pGyroscopeTaskStackBuffer_ = nullptr;
|
||
pxGyroscopeTaskBuffer_ = nullptr;
|
||
|
||
// 初始化滤波相关变量
|
||
last_filtered_x_ = 0;
|
||
last_filtered_y_ = 0;
|
||
last_filtered_z_ = 0;
|
||
is_first_sample_ = true;
|
||
}
|
||
|
||
Gyroscope::~Gyroscope()
|
||
{
|
||
// 删除定时器
|
||
DeleteTimer();
|
||
|
||
if (pGyroscopeTaskStackBuffer_) {
|
||
free(pGyroscopeTaskStackBuffer_);
|
||
pGyroscopeTaskStackBuffer_ = nullptr;
|
||
}
|
||
if (pxGyroscopeTaskBuffer_) {
|
||
vPortFree(pxGyroscopeTaskBuffer_);
|
||
pxGyroscopeTaskBuffer_ = nullptr;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 创建500ms周期定时器
|
||
* @return esp_err_t 创建结果
|
||
*/
|
||
esp_err_t Gyroscope::CreateTimer()
|
||
{
|
||
if (timer_handle_ != nullptr) {
|
||
ESP_LOGW(TAG_GYROSCOPE, "定时器已存在");
|
||
return ESP_OK;
|
||
}
|
||
|
||
// 配置定时器参数
|
||
esp_timer_create_args_t timer_args = {};
|
||
timer_args.callback = &TimerCallback; // 静态回调函数
|
||
timer_args.arg = this; // 传入实例指针
|
||
timer_args.name = "gyro_timer"; // 定时器名称
|
||
timer_args.dispatch_method = ESP_TIMER_TASK; // 使用任务调度
|
||
timer_args.skip_unhandled_events = false; // 不跳过未处理事件
|
||
|
||
// 创建定时器
|
||
esp_err_t err = esp_timer_create(&timer_args, &timer_handle_);
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "创建定时器失败: 0x%x", err);
|
||
return err;
|
||
}
|
||
|
||
// 启动定时器，500ms周期重复
|
||
err = esp_timer_start_periodic(timer_handle_, 50000); // 50000微秒 = 50ms 20hz
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "启动定时器失败: 0x%x", err);
|
||
esp_timer_delete(timer_handle_);
|
||
timer_handle_ = nullptr;
|
||
return err;
|
||
}
|
||
|
||
ESP_LOGI(TAG_GYROSCOPE, "50ms周期定时器创建并启动成功");
|
||
return ESP_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 停止并删除定时器
|
||
*/
|
||
void Gyroscope::DeleteTimer()
|
||
{
|
||
if (timer_handle_ == nullptr) {
|
||
return;
|
||
}
|
||
|
||
// 停止定时器
|
||
esp_err_t err = esp_timer_stop(timer_handle_);
|
||
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "停止定时器失败: 0x%x", err);
|
||
}
|
||
|
||
// 删除定时器
|
||
err = esp_timer_delete(timer_handle_);
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "删除定时器失败: 0x%x", err);
|
||
}
|
||
|
||
timer_handle_ = nullptr;
|
||
ESP_LOGI(TAG_GYROSCOPE, "定时器已删除");
|
||
}
|
||
|
||
/**
|
||
* @brief 静态定时器回调函数
|
||
* @param arg 传入的Gyroscope实例指针
|
||
*/
|
||
void Gyroscope::TimerCallback(void* arg)
|
||
{
|
||
Gyroscope* gyroscope = static_cast<Gyroscope*>(arg);
|
||
if (gyroscope) {
|
||
gyroscope->TimerInterruptHandler();
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 定时器中断处理函数
|
||
* 每50ms调用一次，可以在这里执行周期性任务
|
||
*/
|
||
void Gyroscope::TimerInterruptHandler()
|
||
{
|
||
// 在这里添加定时器中断处理逻辑
|
||
// 例如：读取传感器数据、检测动作、发送状态等
|
||
AccelerationData accel_data_ = {0};
|
||
|
||
esp_err_t err = GetAcceleration(&accel_data_);
|
||
if (err == ESP_OK) {
|
||
// 根据实际安装方式重新定义姿态判断
|
||
// 平放状态：Y轴接近±1g（因为Y轴垂直于桌面），X和Z轴接近0
|
||
bool is_flat = (fabs(accel_data_.y) > 0.85 && fabs(accel_data_.y) < 1.15) &&
|
||
(fabs(accel_data_.x) < 0.15 && fabs(accel_data_.z) < 0.15);
|
||
|
||
// 手持状态：Y轴接近0，X和Z轴中至少有一个有明显值
|
||
bool is_held = (fabs(accel_data_.y) < 0.7) &&
|
||
(fabs(accel_data_.x) > 0.15 || fabs(accel_data_.z) > 0.15);
|
||
|
||
// 计算合成加速度（总加速度大小）
|
||
float total_accel = sqrt(accel_data_.x * accel_data_.x +
|
||
accel_data_.y * accel_data_.y +
|
||
accel_data_.z * accel_data_.z);
|
||
|
||
// 计算与上次合成加速度的差值（加速度变化量）
|
||
float total_accel_diff = fabs(total_accel - total_accel_last_);
|
||
|
||
// 计算Y轴加速度变化
|
||
float y_diff = accel_data_.y - accel_data_y_last_;
|
||
|
||
// 检测是否处于平放稳定状态
|
||
if(is_flat) {
|
||
stable_count_++;
|
||
if(stable_count_ >= 8) { // 连续8次检测到平放状态（400ms），减少误判
|
||
stable_count_ = 8; // 防止溢出
|
||
if(!is_stable_) {
|
||
ESP_LOGI(TAG_GYROSCOPE, "定时器: 检测到稳定平放状态 (x:%.2f, y:%.2f, z:%.2f, total:%.2f)",
|
||
accel_data_.x, accel_data_.y, accel_data_.z, total_accel);
|
||
is_stable_ = true;
|
||
is_lifted_ = false; // 重置拿起状态
|
||
putdown_count_ = 0; // 重置放置计数器
|
||
lift_count_ = 0; // 重置拿起计数器
|
||
|
||
// 调用放置回调
|
||
if (on_putdown_callback_) {
|
||
on_putdown_callback_();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else {
|
||
stable_count_ = 0; // 不在范围内则重置稳定计数器
|
||
|
||
// 如果之前是稳定状态，检测是否有拿起动作
|
||
if(is_stable_) {
|
||
// 检测拿起动作的多种特征：
|
||
// 1. Y轴从接近1g变为接近0g（位置变化）
|
||
// 2. 合成加速度有显著变化（表示有加速度）
|
||
bool y_change_indicates_lift = fabs(y_diff) > 0.25;
|
||
bool total_accel_change_indicates_lift = total_accel_diff > 0.3; // 检测加速度突变
|
||
|
||
if(y_change_indicates_lift || total_accel_change_indicates_lift) {
|
||
lift_count_++;
|
||
if(lift_count_ >= 1) { // 降低拿起检测阈值，提高灵敏度
|
||
ESP_LOGI(TAG_GYROSCOPE, "定时器: 检测到拿起动作 (x:%.2f, y:%.2f, z:%.2f, total:%.2f, y_diff:%.2f, accel_diff:%.2f)",
|
||
accel_data_.x, accel_data_.y, accel_data_.z, total_accel, y_diff, total_accel_diff);
|
||
is_stable_ = false; // 重置状态
|
||
is_lifted_ = true; // 设置拿起状态
|
||
lift_count_ = 0;
|
||
stable_count_ = 0;
|
||
|
||
// 调用拿起回调
|
||
if (on_pickup_callback_) {
|
||
on_pickup_callback_();
|
||
}
|
||
}
|
||
} else {
|
||
lift_count_ = 0; // 变化不符合预期，重置拿起计数器
|
||
}
|
||
}
|
||
// 如果之前是被拿起状态，检测是否有放置动作
|
||
else if(is_lifted_) {
|
||
// 检测从手持到平放的变化 - Y轴从接近0g变为接近1g
|
||
bool y_change_indicates_putdown = (y_diff > 0.25 && y_diff < 0.80);
|
||
|
||
if(y_change_indicates_putdown) {
|
||
putdown_count_++;
|
||
if(putdown_count_ >= 3) { // 连续3次检测到缓慢变化
|
||
ESP_LOGI(TAG_GYROSCOPE, "定时器: 检测到缓慢放置动作 (x:%.2f, y:%.2f, z:%.2f, total:%.2f, y_diff:%.2f)",
|
||
accel_data_.x, accel_data_.y, accel_data_.z, total_accel, y_diff);
|
||
is_lifted_ = false; // 重置拿起状态
|
||
putdown_count_ = 0;
|
||
|
||
// 调用放置回调
|
||
if (on_putdown_callback_) {
|
||
on_putdown_callback_();
|
||
}
|
||
}
|
||
} else {
|
||
putdown_count_ = 0; // 变化不符合预期，重置放置计数器
|
||
}
|
||
}
|
||
|
||
// 检测倾斜动作 - 仅当手持时才触发倾斜
|
||
if (is_held && (fabs(accel_data_.x) > 0.3f || fabs(accel_data_.z) > 0.3f)) {
|
||
// 检测到倾斜，调用倾斜回调
|
||
if (on_tilt_callback_) {
|
||
on_tilt_callback_();
|
||
}
|
||
}
|
||
}
|
||
// 保存当前值作为下一次的上一次值
|
||
accel_data_y_last_ = accel_data_.y;
|
||
total_accel_last_ = total_accel;
|
||
}
|
||
}
|
||
/**
|
||
* @brief 注册摇晃检测回调函数
|
||
*
|
||
* @param callback 当检测到摇晃时调用的回调函数
|
||
*/
|
||
void Gyroscope::OnShake(std::function<void()> callback)
|
||
{
|
||
on_shake_callback_ = callback;
|
||
}
|
||
|
||
/**
|
||
* @brief 注册拿起检测回调函数
|
||
*
|
||
* @param callback 当检测到设备被拿起时调用的回调函数
|
||
*/
|
||
void Gyroscope::OnPickUp(std::function<void()> callback)
|
||
{
|
||
on_pickup_callback_ = callback;
|
||
}
|
||
|
||
/**
|
||
* @brief 注册放置检测回调函数
|
||
*
|
||
* @param callback 当检测到设备被放置时调用的回调函数
|
||
*/
|
||
void Gyroscope::OnPutDown(std::function<void()> callback)
|
||
{
|
||
on_putdown_callback_ = callback;
|
||
}
|
||
|
||
/**
|
||
* @brief 注册倾斜检测回调函数
|
||
*
|
||
* @param callback 当检测到设备倾斜时调用的回调函数
|
||
*/
|
||
void Gyroscope::OnTilt(std::function<void()> callback)
|
||
{
|
||
on_tilt_callback_ = callback;
|
||
}
|
||
|
||
/**
|
||
* @brief 静态中断处理函数
|
||
*
|
||
* @param arg 传入的Gyroscope实例指针
|
||
*/
|
||
void IRAM_ATTR Gyroscope::GpioIsrHandler(void* arg)
|
||
{
|
||
Gyroscope* gyroscope = static_cast<Gyroscope*>(arg);
|
||
if (gyroscope) {
|
||
gyroscope->interrupt_flag_ = true;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 向陀螺仪寄存器写入数据
|
||
*
|
||
* @param reg 寄存器地址
|
||
* @param data 要写入的数据
|
||
* @return esp_err_t ESP_OK表示成功
|
||
*/
|
||
esp_err_t Gyroscope::WriteRegister(uint8_t reg, uint8_t data)
|
||
{
|
||
if (bus_handle_ == nullptr) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "I2C总线未初始化");
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
return i2c_bus_write_bytes(bus_handle_, GYROSCOPE_I2C_SLAVE_ADDR << 1, ®, 1, &data, 1);
|
||
}
|
||
|
||
/**
|
||
* @brief 从陀螺仪寄存器读取数据
|
||
*
|
||
* @param reg 寄存器地址
|
||
* @param data 读取到的数据指针
|
||
* @return esp_err_t ESP_OK表示成功
|
||
*/
|
||
esp_err_t Gyroscope::ReadRegister(uint8_t reg, uint8_t* data)
|
||
{
|
||
if (bus_handle_ == nullptr || data == nullptr) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "参数错误或I2C总线未初始化");
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
return i2c_bus_read_bytes(bus_handle_, GYROSCOPE_I2C_SLAVE_ADDR << 1, ®, 1, data, 1);
|
||
}
|
||
|
||
/**
|
||
* @brief 批量读取陀螺仪寄存器数据
|
||
*
|
||
* @param reg 起始寄存器地址
|
||
* @param data 读取到的数据指针
|
||
* @param length 要读取的字节数
|
||
* @return esp_err_t ESP_OK表示成功
|
||
*/
|
||
esp_err_t Gyroscope::ReadRegisters(uint8_t reg, uint8_t* data, size_t length)
|
||
{
|
||
if (bus_handle_ == nullptr || data == nullptr) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "参数错误或I2C总线未初始化");
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
return i2c_bus_read_bytes(bus_handle_, GYROSCOPE_I2C_SLAVE_ADDR << 1, ®, 1, data, length);
|
||
}
|
||
|
||
/**
|
||
* @brief 初始化陀螺仪中断GPIO
|
||
*
|
||
* @param gpio_num 要初始化的GPIO引脚号
|
||
* @return esp_err_t ESP_OK表示成功
|
||
*/
|
||
esp_err_t Gyroscope::InterruptGpioInit(gpio_num_t gpio_num)
|
||
{
|
||
gpio_config_t io_conf = {};
|
||
io_conf.intr_type = GPIO_INTR_POSEDGE; // 上升沿触发
|
||
io_conf.pin_bit_mask = (1ULL<<gpio_num);
|
||
io_conf.mode = GPIO_MODE_INPUT;
|
||
io_conf.pull_up_en = GPIO_PULLUP_DISABLE; // 禁用上拉,因为INT1已经是推挽输出
|
||
io_conf.pull_down_en = GPIO_PULLDOWN_ENABLE; // 启用下拉
|
||
|
||
esp_err_t err = gpio_config(&io_conf);
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "GPIO配置失败,错误码: 0x%x", err);
|
||
return err;
|
||
}
|
||
|
||
// 配置中断服务
|
||
err = gpio_install_isr_service(ESP_INTR_FLAG_LEVEL3 | ESP_INTR_FLAG_IRAM);
|
||
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "GPIO中断服务安装失败,错误码: 0x%x", err);
|
||
return err;
|
||
}
|
||
|
||
// 注册中断处理函数
|
||
err = gpio_isr_handler_add(gpio_num, GpioIsrHandler, this);
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "GPIO中断处理函数添加失败,错误码: 0x%x", err);
|
||
return err;
|
||
}
|
||
|
||
// 清除任何待处理的中断
|
||
gpio_intr_disable(gpio_num);
|
||
gpio_intr_enable(gpio_num);
|
||
|
||
ESP_LOGI(TAG_GYROSCOPE, "GPIO%d中断初始化完成", gpio_num);
|
||
return ESP_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 清除中断标志
|
||
*
|
||
* 该函数会读取唤醒源寄存器和功能源寄存器来清除中断标志
|
||
*
|
||
* @return esp_err_t ESP_OK表示成功
|
||
*/
|
||
esp_err_t Gyroscope::ClearInterruptFlags()
|
||
{
|
||
esp_err_t err;
|
||
uint8_t wake_up_src = 0;
|
||
uint8_t func_src1 = 0;
|
||
// 读取唤醒源寄存器和功能源寄存器来清除中断
|
||
err = ReadRegister(WAKE_UP_SRC, &wake_up_src);
|
||
if (err != ESP_OK) return err;
|
||
err = ReadRegister(FUNC_SRC1, &func_src1);
|
||
if (err != ESP_OK) return err;
|
||
return ESP_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 初始化陀螺仪
|
||
*
|
||
* @param bus_handle I2C总线句柄
|
||
* @return esp_err_t ESP_OK表示成功
|
||
*/
|
||
esp_err_t Gyroscope::Init(i2c_bus_handle_t bus_handle)
|
||
{
|
||
bus_handle_ = bus_handle;
|
||
if (bus_handle_ == nullptr) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "I2C总线句柄为空");
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
ESP_LOGI(TAG_GYROSCOPE, "开始初始化陀螺仪...");
|
||
|
||
// 1. 验证设备连接和通信
|
||
uint8_t who_am_i = 0;
|
||
esp_err_t err = ReadRegister(0x0F, &who_am_i); // WHO_AM_I寄存器
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "无法读取陀螺仪ID: 0x%x", err);
|
||
return err;
|
||
}
|
||
|
||
if (who_am_i != 0x6A) { // LSM6DS3TR-C的WHO_AM_I值
|
||
ESP_LOGE(TAG_GYROSCOPE, "设备ID不匹配,期望0x6A,实际0x%02X", who_am_i);
|
||
return ESP_FAIL;
|
||
}
|
||
ESP_LOGI(TAG_GYROSCOPE, "陀螺仪设备ID验证成功: 0x%02X", who_am_i);
|
||
|
||
// 2. 初始化中断引脚
|
||
err = InterruptGpioInit(GYROSCOPE_INPUT_PIN_SEL);
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "中断引脚初始化失败: 0x%x", err);
|
||
return err;
|
||
}
|
||
ESP_LOGI(TAG_GYROSCOPE, "开始配置唤醒中断...");
|
||
|
||
// 1. 软件复位陀螺仪
|
||
err = WriteRegister(CTRL3_C, 0x01); // SW_RESET
|
||
if (err != ESP_OK) return err;
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(100)); // 等待复位完成
|
||
|
||
// 3. 配置加速度计: 104Hz ODR, ±2g满量程
|
||
err = WriteRegister(CTRL1_XL, 0x50); // 0b01010000 - 104Hz ODR, ±2g
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 4. 配置陀螺仪: 104Hz ODR, ±125dps满量程
|
||
err = WriteRegister(CTRL2_G, 0x50); // 0b01010000
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 5. 配置控制寄存器3: 高电平有效,推挽输出
|
||
err = WriteRegister(CTRL3_C, 0x44); // 0b01000100
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 6. 禁用陀螺仪低功耗模式
|
||
err = WriteRegister(CTRL7_G, 0x00); // 禁用低功耗模式
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 7. 配置唤醒功能: 启用X轴的唤醒检测
|
||
err = WriteRegister(TAP_CFG, 0x81); // 0b10000000 - 仅启用高通滤波器
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 8. 配置唤醒阈值: 非常灵敏的设置
|
||
err = WriteRegister(WAKE_UP_THS, 0x3F); // 31.25mg - 最灵敏的阈值
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 9. 配置唤醒持续时间: 6个ODR周期
|
||
err = WriteRegister(WAKE_UP_DUR, 0x06);
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 10. 配置中断1路由: 将唤醒中断路由到INT1
|
||
err = WriteRegister(MD1_CFG, 0x20); // 0b00100000 - 仅唤醒中断到INT1
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 11. 配置中断2路由: 禁用所有中断
|
||
err = WriteRegister(MD2_CFG, 0x00);
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 12. 配置INT1_CTRL: 启用唤醒中断到INT1
|
||
err = WriteRegister(INT1_CTRL, 0x20); // 0b00100000
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 13. 启用嵌入式功能
|
||
err = WriteRegister(CTRL10_C, 0x3C); // 0b00111000
|
||
if (err != ESP_OK) return err;
|
||
|
||
// 14. 等待配置生效
|
||
vTaskDelay(pdMS_TO_TICKS(100));
|
||
|
||
CreateTimer();
|
||
|
||
ESP_LOGI(TAG_GYROSCOPE, "陀螺仪唤醒中断配置完成");
|
||
|
||
ESP_LOGI(TAG_GYROSCOPE, "陀螺仪初始化完成");
|
||
return ESP_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 创建陀螺仪读取任务
|
||
*
|
||
* 该任务会周期性读取陀螺仪数据并处理中断
|
||
*/
|
||
void Gyroscope::CreateGyroReadTask()
|
||
{
|
||
if (bus_handle_ == nullptr) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "I2C总线未初始化,无法启动陀螺仪任务");
|
||
return;
|
||
}
|
||
|
||
xTaskCreateWithCaps(
|
||
GyroReadTask,
|
||
"gyro_read_task",
|
||
4096,
|
||
this,
|
||
5, // Reduced priority to allow other tasks to run
|
||
&xGyroscopeTaskHandle_,
|
||
MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT
|
||
);
|
||
|
||
if (xGyroscopeTaskHandle_ == NULL) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "Failed to create static gyro read task");
|
||
} else {
|
||
ESP_LOGI(TAG_GYROSCOPE, "Static gyro read task created successfully");
|
||
}
|
||
}
|
||
|
||
esp_err_t Gyroscope::ReadRawAcceleration(int16_t* x, int16_t* y, int16_t* z) {
|
||
// 1. 参数合法性检查
|
||
if (x == nullptr || y == nullptr || z == nullptr) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "参数为空指针");
|
||
return ESP_ERR_INVALID_ARG;
|
||
}
|
||
|
||
// 2. 读取原始加速度数据（6字节：X_L/X_H/Y_L/Y_H/Z_L/Z_H）
|
||
uint8_t accel_data_[6] = {0};
|
||
esp_err_t err = ReadRegisters(ACCEL_X_L, accel_data_, 6);
|
||
if (err != ESP_OK) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "读取加速度寄存器失败: 0x%x", err);
|
||
return err;
|
||
}
|
||
|
||
// 3. 组合高低字节为int16_t（二进制补码，适配传感器输出）
|
||
int16_t current_raw_x = (int16_t)((accel_data_[1] << 8) | accel_data_[0]);
|
||
int16_t current_raw_y = (int16_t)((accel_data_[3] << 8) | accel_data_[2]);
|
||
int16_t current_raw_z = (int16_t)((accel_data_[5] << 8) | accel_data_[4]);
|
||
|
||
// 4. 一阶α滤波(整数版，无浮点运算，避免精度丢失)
|
||
int16_t filtered_x, filtered_y, filtered_z;
|
||
if (is_first_sample_) {
|
||
// 首次采样：直接初始化滤波值为原始值
|
||
filtered_x = current_raw_x;
|
||
filtered_y = current_raw_y;
|
||
filtered_z = current_raw_z;
|
||
is_first_sample_ = false; // 标记首次采样完成
|
||
} else {
|
||
// 核心滤波公式（整数版）：α*原始值 + (1-α)*上一次滤波值
|
||
// WEIGHTFACTOR=0.9; (1-WEIGHTFACTOR)=0.1
|
||
const int32_t weight_scaled = (int32_t)(WEIGHTFACTOR * 10); // 9
|
||
const int32_t inv_weight_scaled = 10 - weight_scaled; // 1
|
||
|
||
filtered_x = (weight_scaled * (int32_t)current_raw_x +
|
||
inv_weight_scaled * last_filtered_x_) / 10;
|
||
filtered_y = (weight_scaled * (int32_t)current_raw_y +
|
||
inv_weight_scaled * last_filtered_y_) / 10;
|
||
filtered_z = (weight_scaled * (int32_t)current_raw_z +
|
||
inv_weight_scaled * last_filtered_z_) / 10;
|
||
}
|
||
|
||
// 5. 转换为int16_t输出（filtered_x/y/z是int32_t，范围仍在int16_t内）
|
||
*x = (int16_t)filtered_x;
|
||
*y = (int16_t)filtered_y;
|
||
*z = (int16_t)filtered_z;
|
||
|
||
return ESP_OK;
|
||
}
|
||
|
||
esp_err_t Gyroscope::GetAcceleration(AccelerationData* data) {
|
||
if (data == nullptr) {
|
||
ESP_LOGE(TAG_GYROSCOPE, "数据指针为空");
|
||
return ESP_ERR_INVALID_ARG;
|
||
}
|
||
|
||
int16_t raw_x, raw_y, raw_z;
|
||
esp_err_t err = ReadRawAcceleration(&raw_x, &raw_y, &raw_z);
|
||
if (err != ESP_OK) {
|
||
return err;
|
||
}
|
||
|
||
// 转换公式：加速度(g) = 原始值 × 灵敏度(mg/LSB) × 10^-3
|
||
data->x = raw_x * ACCEL_SENSITIVITY_2G / 1000.0f;
|
||
data->y = raw_y * ACCEL_SENSITIVITY_2G / 1000.0f;
|
||
data->z = raw_z * ACCEL_SENSITIVITY_2G / 1000.0f;
|
||
|
||
return ESP_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 陀螺仪读取任务
|
||
* Y轴立起 Z轴躺下
|
||
* 该任务会周期性读取陀螺仪数据并处理中断
|
||
*/
|
||
void IRAM_ATTR Gyroscope::GyroReadTask(void* pvParameters)
|
||
{
|
||
Gyroscope* gyroscope = static_cast<Gyroscope*>(pvParameters);
|
||
uint32_t interrupt_count = 0;
|
||
while (1) {
|
||
if (gyroscope->interrupt_flag_) {
|
||
interrupt_count++;
|
||
gyroscope->ClearInterruptFlags();
|
||
gyroscope->interrupt_flag_ = false;
|
||
|
||
// 调用用户注册的回调函数
|
||
if (gyroscope->on_shake_callback_) {
|
||
gyroscope->on_shake_callback_();
|
||
}
|
||
|
||
ESP_LOGI(TAG_GYROSCOPE, "累计中断次数: %lu", interrupt_count);
|
||
if(interrupt_count >= 2){
|
||
interrupt_count = 0;
|
||
ESP_LOGI(TAG_GYROSCOPE, "检测到摇晃>>>>>>>>>>");
|
||
}
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(50)); // 50ms检查一次,更快的响应
|
||
}
|
||
}
|