1504 lines
50 KiB
C++
1504 lines
50 KiB
C++
#include "qmi8658a.h"
|
||
#include <esp_log.h>
|
||
#include <cmath>
|
||
#include <cstring>
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/task.h"
|
||
|
||
#define TAG "QMI8658A"
|
||
|
||
QMI8658A::QMI8658A(i2c_master_bus_handle_t i2c_bus, uint8_t addr)
|
||
: I2cDevice(i2c_bus, addr),
|
||
state_(QMI8658A_STATE_UNINITIALIZED),
|
||
last_error_(QMI8658A_OK),
|
||
acc_scale_(1.0f),
|
||
gyro_scale_(1.0f),
|
||
is_calibrating_(false),
|
||
calibration_start_time_(0),
|
||
calibration_duration_(0),
|
||
calibration_sample_count_(0),
|
||
buffer_task_handle_(nullptr),
|
||
buffer_enabled_(false),
|
||
buffer_interval_ms_(0),
|
||
interrupt_pin_(GPIO_NUM_NC),
|
||
interrupt_type_(QMI8658A_INT_DISABLE),
|
||
interrupt_enabled_(false),
|
||
fifo_enabled_(false) {
|
||
// 默认配置 - 修正ODR设置以匹配实际寄存器值
|
||
config_.mode = QMI8658A_MODE_DUAL;
|
||
config_.acc_range = QMI8658A_ACC_RANGE_4G; // 匹配CTRL2寄存器值0x16
|
||
config_.gyro_range = QMI8658A_GYRO_RANGE_512DPS; // 匹配CTRL3寄存器值0x56
|
||
config_.acc_odr = QMI8658A_ODR_125HZ; // 匹配实际寄存器值0x06
|
||
config_.gyro_odr = QMI8658A_ODR_125HZ; // 匹配实际寄存器值0x06
|
||
|
||
// 初始化缓冲区结构
|
||
memset(&data_buffer_, 0, sizeof(data_buffer_));
|
||
data_buffer_.mutex = nullptr;
|
||
|
||
// 初始化校准数据
|
||
memset(&calibration_, 0, sizeof(calibration_));
|
||
memset(calibration_acc_sum_, 0, sizeof(calibration_acc_sum_));
|
||
memset(calibration_gyro_sum_, 0, sizeof(calibration_gyro_sum_));
|
||
|
||
// 初始化FIFO配置
|
||
memset(&fifo_config_, 0, sizeof(fifo_config_));
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::Initialize(const qmi8658a_config_t* config) {
|
||
ESP_LOGI(TAG, "Initializing QMI8658A sensor...");
|
||
|
||
state_ = QMI8658A_STATE_INITIALIZING;
|
||
|
||
// 执行自检
|
||
qmi8658a_error_t result = PerformSelfTest();
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Self-test failed during initialization");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 软件复位
|
||
result = SoftReset();
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Software reset failed");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 保存配置
|
||
if (config) {
|
||
config_ = *config;
|
||
}
|
||
|
||
// 计算比例因子
|
||
CalculateScaleFactors();
|
||
|
||
// 配置加速度计 - 使用更保守的设置
|
||
result = SetAccelConfig(QMI8658A_ACC_RANGE_4G, QMI8658A_ODR_125HZ);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to configure accelerometer");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 配置陀螺仪 - 使用更保守的设置
|
||
result = SetGyroConfig(QMI8658A_GYRO_RANGE_256DPS, QMI8658A_ODR_125HZ);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to configure gyroscope");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 配置CTRL1 - 使能数据就绪/中断相关功能以确保STATUS0有效
|
||
result = WriteRegWithVerification(QMI8658A_CTRL1, 0x60);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGW(TAG, "Failed to configure CTRL1 register");
|
||
} else {
|
||
VerifyRegisterValue(QMI8658A_CTRL1, 0x60, "CTRL1");
|
||
}
|
||
|
||
uint8_t mode_val = config_.mode;
|
||
result = SetMode(static_cast<qmi8658a_mode_t>(mode_val));
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to set mode");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 添加额外的寄存器配置
|
||
// 配置CTRL5寄存器 - 设置加速度计和陀螺仪的带宽滤波
|
||
result = WriteRegWithVerification(QMI8658A_CTRL5, 0x35);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGW(TAG, "Failed to configure CTRL5 register");
|
||
}
|
||
else {
|
||
VerifyRegisterValue(QMI8658A_CTRL5, 0x35, "CTRL5");
|
||
}
|
||
|
||
// 配置CTRL6寄存器 - 设置陀螺仪的LPF
|
||
result = WriteRegWithVerification(QMI8658A_CTRL6, 0x00);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGW(TAG, "Failed to configure CTRL6 register");
|
||
}
|
||
|
||
// 等待传感器稳定
|
||
vTaskDelay(pdMS_TO_TICKS(100)); // 增加等待时间到100ms
|
||
|
||
// 验证关键寄存器配置
|
||
uint8_t expected_ctrl2 = (config_.acc_range << 4) | config_.acc_odr;
|
||
result = VerifyRegisterValue(QMI8658A_CTRL2, expected_ctrl2, "CTRL2");
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "CTRL2 verification failed");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
uint8_t expected_ctrl3 = (config_.gyro_range << 4) | config_.gyro_odr;
|
||
result = VerifyRegisterValue(QMI8658A_CTRL3, expected_ctrl3, "CTRL3 (Gyro Config)");
|
||
if (result != QMI8658A_OK) {
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
uint8_t expected_ctrl7 = (mode_val == QMI8658A_MODE_DISABLE) ? 0x00 : (uint8_t)(0x80 | mode_val);
|
||
result = VerifyRegisterValue(QMI8658A_CTRL7, expected_ctrl7, "CTRL7 (Mode)");
|
||
if (result != QMI8658A_OK) {
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 诊断寄存器状态
|
||
ESP_LOGI(TAG, "=== QMI8658A Register Diagnostics ===");
|
||
uint8_t ctrl1 = ReadReg(QMI8658A_CTRL1);
|
||
uint8_t ctrl2 = ReadReg(QMI8658A_CTRL2);
|
||
uint8_t ctrl3 = ReadReg(QMI8658A_CTRL3);
|
||
uint8_t ctrl5 = ReadReg(QMI8658A_CTRL5);
|
||
uint8_t ctrl6 = ReadReg(QMI8658A_CTRL6);
|
||
uint8_t ctrl7 = ReadReg(QMI8658A_CTRL7);
|
||
uint8_t status0 = ReadReg(QMI8658A_STATUS0);
|
||
uint8_t status1 = ReadReg(QMI8658A_STATUS1);
|
||
|
||
ESP_LOGI(TAG, "CTRL1: 0x%02X, CTRL2: 0x%02X, CTRL3: 0x%02X, CTRL5: 0x%02X, CTRL6: 0x%02X, CTRL7: 0x%02X",
|
||
ctrl1, ctrl2, ctrl3, ctrl5, ctrl6, ctrl7);
|
||
ESP_LOGI(TAG, "STATUS0: 0x%02X, STATUS1: 0x%02X", status0, status1);
|
||
ESP_LOGI(TAG, "=====================================");
|
||
|
||
// 等待数据就绪 - 只进行一次等待
|
||
if (config_.mode != QMI8658A_MODE_DISABLE) {
|
||
ESP_LOGI(TAG, "Waiting for sensor data to be ready...");
|
||
uint32_t wait_count = 0;
|
||
const uint32_t max_wait = 100; // 最多等待1秒
|
||
|
||
while (wait_count < max_wait) {
|
||
uint8_t status = ReadReg(QMI8658A_STATUS0);
|
||
if (status & 0x03) { // 检查加速度计或陀螺仪数据就绪
|
||
ESP_LOGI(TAG, "Data ready after %lu ms", wait_count * 10);
|
||
break;
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(10));
|
||
wait_count++;
|
||
}
|
||
|
||
if (wait_count >= max_wait) {
|
||
ESP_LOGW(TAG, "Data ready timeout, but initialization completed");
|
||
}
|
||
}
|
||
|
||
state_ = QMI8658A_STATE_READY;
|
||
UpdateScaleFactors();
|
||
ESP_LOGI(TAG, "QMI8658A initialization completed successfully");
|
||
DumpRegisters();
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
uint8_t QMI8658A::GetChipId() {
|
||
uint8_t chip_id = ReadReg(QMI8658A_WHO_AM_I);
|
||
if (chip_id == 0xFF) {
|
||
ESP_LOGE(TAG, "Failed to read chip ID register, I2C communication error");
|
||
return 0xFF;
|
||
}
|
||
return chip_id;
|
||
}
|
||
|
||
uint8_t QMI8658A::GetRevisionId() {
|
||
return ReadReg(QMI8658A_REVISION_ID);
|
||
}
|
||
|
||
// 静态连接检测方法（用于生产测试）
|
||
bool QMI8658A::CheckConnection(i2c_master_bus_handle_t i2c_bus, uint8_t* detected_address) {
|
||
// 可能的QMI8658A I2C地址
|
||
uint8_t possible_addresses[] = {0x6A, 0x6B};
|
||
uint8_t num_addresses = sizeof(possible_addresses) / sizeof(possible_addresses[0]);
|
||
|
||
for (uint8_t i = 0; i < num_addresses; i++) {
|
||
uint8_t addr = possible_addresses[i];
|
||
|
||
// 创建临时I2C设备句柄进行测试
|
||
i2c_master_dev_handle_t dev_handle;
|
||
i2c_device_config_t dev_cfg = {
|
||
.dev_addr_length = I2C_ADDR_BIT_LEN_7,
|
||
.device_address = addr,
|
||
.scl_speed_hz = 400000, // 400kHz
|
||
};
|
||
|
||
esp_err_t ret = i2c_master_bus_add_device(i2c_bus, &dev_cfg, &dev_handle);
|
||
if (ret != ESP_OK) {
|
||
continue; // 尝试下一个地址
|
||
}
|
||
|
||
// 尝试读取WHO_AM_I寄存器
|
||
uint8_t reg_addr = QMI8658A_WHO_AM_I;
|
||
uint8_t chip_id = 0;
|
||
|
||
ret = i2c_master_transmit_receive(dev_handle, ®_addr, 1, &chip_id, 1, 1000);
|
||
|
||
// 清理设备句柄
|
||
i2c_master_bus_rm_device(dev_handle);
|
||
|
||
if (ret == ESP_OK && chip_id == QMI8658A_CHIP_ID) {
|
||
// 找到有效的QMI8658A设备
|
||
if (detected_address != nullptr) {
|
||
*detected_address = addr;
|
||
}
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false; // 未找到有效设备
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SoftReset() {
|
||
ESP_LOGI(TAG, "Performing soft reset...");
|
||
|
||
qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_RESET, 0xB0);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to perform soft reset");
|
||
return result;
|
||
}
|
||
|
||
// 等待复位完成 - 大幅增加等待时间以确保传感器完全稳定
|
||
vTaskDelay(pdMS_TO_TICKS(200)); // 增加到200ms等待时间
|
||
|
||
// 验证复位是否成功 - 检查芯片ID
|
||
uint8_t chip_id = GetChipId();
|
||
if (chip_id != QMI8658A_CHIP_ID) {
|
||
ESP_LOGE(TAG, "Soft reset verification failed: chip ID = 0x%02X", chip_id);
|
||
return SetError(QMI8658A_ERROR_INIT_FAILED);
|
||
}
|
||
|
||
ESP_LOGI(TAG, "Soft reset completed and verified successfully");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetMode(qmi8658a_mode_t mode) {
|
||
ESP_LOGI(TAG, "Setting mode: %d", mode);
|
||
|
||
uint8_t value = 0x00;
|
||
switch (mode) {
|
||
case QMI8658A_MODE_DISABLE: value = 0x00; break;
|
||
case QMI8658A_MODE_ACC_ONLY: value = 0x81; break;
|
||
case QMI8658A_MODE_GYRO_ONLY: value = 0x82; break;
|
||
case QMI8658A_MODE_DUAL: value = 0x83; break;
|
||
}
|
||
qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL7, value, 3);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to set mode: %d", mode);
|
||
return result;
|
||
}
|
||
|
||
// 更新配置
|
||
config_.mode = mode;
|
||
|
||
ESP_LOGI(TAG, "Mode set successfully: CTRL7=0x%02X", value);
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetAccelConfig(qmi8658a_acc_range_t range, qmi8658a_odr_t odr) {
|
||
ESP_LOGI(TAG, "Setting accelerometer config: range=%d, odr=%d", range, odr);
|
||
|
||
uint8_t ctrl2_val = (range << 4) | odr;
|
||
|
||
// 使用带验证的写入函数，最多重试3次
|
||
qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL2, ctrl2_val, 3);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to set accelerometer config: range=%d, odr=%d", range, odr);
|
||
return result;
|
||
}
|
||
|
||
// 更新配置
|
||
config_.acc_range = range;
|
||
config_.acc_odr = odr;
|
||
UpdateScaleFactors();
|
||
|
||
ESP_LOGI(TAG, "Accelerometer config set successfully: CTRL2=0x%02X", ctrl2_val);
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetGyroConfig(qmi8658a_gyro_range_t range, qmi8658a_odr_t odr) {
|
||
ESP_LOGI(TAG, "Setting gyroscope config: range=%d, odr=%d", range, odr);
|
||
|
||
uint8_t ctrl3_val = (range << 4) | odr;
|
||
|
||
// 使用带验证的写入函数，最多重试3次
|
||
qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL3, ctrl3_val, 3);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to set gyroscope config: range=%d, odr=%d", range, odr);
|
||
return result;
|
||
}
|
||
|
||
// 更新配置
|
||
config_.gyro_range = range;
|
||
config_.gyro_odr = odr;
|
||
UpdateScaleFactors();
|
||
|
||
ESP_LOGI(TAG, "Gyroscope config set successfully: CTRL3=0x%02X", ctrl3_val);
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
void QMI8658A::CalculateScaleFactors() {
|
||
// 计算加速度计比例因子 (g)
|
||
switch (config_.acc_range) {
|
||
case QMI8658A_ACC_RANGE_2G:
|
||
acc_scale_ = 2.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_ACC_RANGE_4G:
|
||
acc_scale_ = 4.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_ACC_RANGE_8G:
|
||
acc_scale_ = 8.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_ACC_RANGE_16G:
|
||
acc_scale_ = 16.0f / 32768.0f;
|
||
break;
|
||
}
|
||
|
||
// 计算陀螺仪比例因子 (dps)
|
||
switch (config_.gyro_range) {
|
||
case QMI8658A_GYRO_RANGE_16DPS:
|
||
gyro_scale_ = 16.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_32DPS:
|
||
gyro_scale_ = 32.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_64DPS:
|
||
gyro_scale_ = 64.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_128DPS:
|
||
gyro_scale_ = 128.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_256DPS:
|
||
gyro_scale_ = 256.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_512DPS:
|
||
gyro_scale_ = 512.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_1024DPS:
|
||
gyro_scale_ = 1024.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_2048DPS:
|
||
gyro_scale_ = 2048.0f / 32768.0f;
|
||
break;
|
||
}
|
||
|
||
ESP_LOGI(TAG, "Scale factors - Acc: %.6f, Gyro: %.6f", acc_scale_, gyro_scale_);
|
||
}
|
||
|
||
int16_t QMI8658A::ReadInt16(uint8_t reg_low) {
|
||
uint8_t data[2];
|
||
ReadRegs(reg_low, data, 2);
|
||
return (int16_t)((data[1] << 8) | data[0]);
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ReadAccelData(float* acc_x, float* acc_y, float* acc_z) {
|
||
if (!acc_x || !acc_y || !acc_z) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
uint8_t buf[6];
|
||
ReadRegs(QMI8658A_AX_L, buf, 6);
|
||
int16_t raw_x = (int16_t)((buf[1] << 8) | buf[0]);
|
||
int16_t raw_y = (int16_t)((buf[3] << 8) | buf[2]);
|
||
int16_t raw_z = (int16_t)((buf[5] << 8) | buf[4]);
|
||
|
||
// 应用缩放和校准
|
||
float ax = raw_x * acc_scale_;
|
||
float ay = raw_y * acc_scale_;
|
||
float az = raw_z * acc_scale_;
|
||
if (calibration_.is_calibrated) {
|
||
float bx = calibration_.acc_bias[0];
|
||
float by = calibration_.acc_bias[1];
|
||
float bz = calibration_.acc_bias[2];
|
||
bool valid_bias = fabs(bx) < 0.5f && fabs(by) < 0.5f && fabs(bz) < 0.5f;
|
||
if (valid_bias) {
|
||
ax -= bx;
|
||
ay -= by;
|
||
az -= bz;
|
||
}
|
||
}
|
||
*acc_x = ax;
|
||
*acc_y = ay;
|
||
*acc_z = az;
|
||
|
||
// 数据合理性检查 - 加速度值不应超过设置的量程
|
||
float max_acc = 4.0f; // 假设使用4G量程
|
||
if (fabs(*acc_x) > max_acc * 1.1f || fabs(*acc_y) > max_acc * 1.1f || fabs(*acc_z) > max_acc * 1.1f) {
|
||
ESP_LOGE(TAG, "Invalid accelerometer readings: [%.3f, %.3f, %.3f] g", *acc_x, *acc_y, *acc_z);
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ReadGyroData(float* gyro_x, float* gyro_y, float* gyro_z) {
|
||
if (!gyro_x || !gyro_y || !gyro_z) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
uint8_t buf[6];
|
||
ReadRegs(QMI8658A_GX_L, buf, 6);
|
||
int16_t raw_x = (int16_t)((buf[1] << 8) | buf[0]);
|
||
int16_t raw_y = (int16_t)((buf[3] << 8) | buf[2]);
|
||
int16_t raw_z = (int16_t)((buf[5] << 8) | buf[4]);
|
||
|
||
float gx = raw_x * gyro_scale_;
|
||
float gy = raw_y * gyro_scale_;
|
||
float gz = raw_z * gyro_scale_;
|
||
if (calibration_.is_calibrated) {
|
||
float bx = calibration_.gyro_bias[0];
|
||
float by = calibration_.gyro_bias[1];
|
||
float bz = calibration_.gyro_bias[2];
|
||
bool valid_bias = fabs(bx) < 50.0f && fabs(by) < 50.0f && fabs(bz) < 50.0f;
|
||
if (valid_bias) {
|
||
gx -= bx;
|
||
gy -= by;
|
||
gz -= bz;
|
||
}
|
||
}
|
||
*gyro_x = gx;
|
||
*gyro_y = gy;
|
||
*gyro_z = gz;
|
||
|
||
float max_gyro = 400.0f;
|
||
if (fabs(*gyro_x) > max_gyro || fabs(*gyro_y) > max_gyro || fabs(*gyro_z) > max_gyro) {
|
||
ESP_LOGW(TAG, "Gyro readings out of expected range: [%.3f, %.3f, %.3f] dps", *gyro_x, *gyro_y, *gyro_z);
|
||
// 不再直接返回错误，而是继续处理但记录警告
|
||
}
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ReadTemperature(float* temperature) {
|
||
if (!temperature) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
uint8_t tl = ReadReg(QMI8658A_TEMP_L);
|
||
uint8_t th = ReadReg(QMI8658A_TEMP_H);
|
||
*temperature = (float)th + ((float)tl / 256.0f);
|
||
|
||
if (*temperature < -40.0f || *temperature > 125.0f) {
|
||
ESP_LOGW(TAG, "Temperature reading out of expected range: %.2f°C", *temperature);
|
||
*temperature = 25.0f;
|
||
}
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::InitializeBuffer() {
|
||
// 初始化缓冲区
|
||
memset(&data_buffer_, 0, sizeof(data_buffer_));
|
||
data_buffer_.mutex = xSemaphoreCreateMutex();
|
||
if (data_buffer_.mutex == NULL) {
|
||
return SetError(QMI8658A_ERROR_INIT_FAILED);
|
||
}
|
||
|
||
buffer_enabled_ = false;
|
||
buffer_task_handle_ = NULL;
|
||
|
||
ESP_LOGI(TAG, "Data buffer initialized");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::StartBufferedReading(uint32_t interval_ms) {
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
if (data_buffer_.mutex == NULL) {
|
||
qmi8658a_error_t r = InitializeBuffer();
|
||
if (r != QMI8658A_OK) {
|
||
return r;
|
||
}
|
||
}
|
||
|
||
if (buffer_enabled_) {
|
||
ESP_LOGW(TAG, "Buffered reading already started");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
buffer_interval_ms_ = interval_ms;
|
||
buffer_enabled_ = true;
|
||
|
||
// 创建缓冲任务
|
||
BaseType_t result = xTaskCreate(
|
||
BufferTask,
|
||
"qmi8658a_buffer",
|
||
4096,
|
||
this,
|
||
5,
|
||
&buffer_task_handle_
|
||
);
|
||
|
||
if (result != pdPASS) {
|
||
buffer_enabled_ = false;
|
||
return SetError(QMI8658A_ERROR_INIT_FAILED);
|
||
}
|
||
|
||
ESP_LOGI(TAG, "Started buffered reading with %" PRIu32 " ms interval", interval_ms);
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::StopBufferedReading() {
|
||
if (!buffer_enabled_) {
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
buffer_enabled_ = false;
|
||
|
||
if (buffer_task_handle_ != NULL) {
|
||
vTaskDelete(buffer_task_handle_);
|
||
buffer_task_handle_ = NULL;
|
||
}
|
||
|
||
ESP_LOGI(TAG, "Stopped buffered reading");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::GetBufferedData(qmi8658a_data_t* data, uint32_t max_count, uint32_t* actual_count) {
|
||
if (!data || !actual_count) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
*actual_count = 0;
|
||
|
||
if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(100)) != pdTRUE) {
|
||
return SetError(QMI8658A_ERROR_TIMEOUT);
|
||
}
|
||
|
||
uint32_t count = 0;
|
||
while (count < max_count && data_buffer_.count > 0) {
|
||
data[count] = data_buffer_.data[data_buffer_.tail];
|
||
data_buffer_.tail = (data_buffer_.tail + 1) % QMI8658A_BUFFER_SIZE;
|
||
data_buffer_.count--;
|
||
count++;
|
||
}
|
||
|
||
*actual_count = count;
|
||
xSemaphoreGive(data_buffer_.mutex);
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ClearBuffer() {
|
||
if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(100)) != pdTRUE) {
|
||
return SetError(QMI8658A_ERROR_TIMEOUT);
|
||
}
|
||
|
||
data_buffer_.head = 0;
|
||
data_buffer_.tail = 0;
|
||
data_buffer_.count = 0;
|
||
data_buffer_.overflow = false;
|
||
|
||
xSemaphoreGive(data_buffer_.mutex);
|
||
|
||
ESP_LOGI(TAG, "Buffer cleared");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
uint32_t QMI8658A::GetBufferCount() {
|
||
if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
|
||
return 0;
|
||
}
|
||
|
||
uint32_t count = data_buffer_.count;
|
||
xSemaphoreGive(data_buffer_.mutex);
|
||
|
||
return count;
|
||
}
|
||
|
||
bool QMI8658A::IsBufferOverflow() {
|
||
if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
|
||
return false;
|
||
}
|
||
|
||
bool overflow = data_buffer_.overflow;
|
||
xSemaphoreGive(data_buffer_.mutex);
|
||
|
||
return overflow;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ConfigureInterrupt(qmi8658a_interrupt_t int_type, gpio_num_t pin) {
|
||
interrupt_type_ = int_type;
|
||
interrupt_pin_ = pin;
|
||
|
||
if (int_type == QMI8658A_INT_DISABLE) {
|
||
interrupt_enabled_ = false;
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
// 配置GPIO中断
|
||
gpio_config_t io_conf = {};
|
||
io_conf.intr_type = GPIO_INTR_POSEDGE;
|
||
io_conf.mode = GPIO_MODE_INPUT;
|
||
io_conf.pin_bit_mask = (1ULL << pin);
|
||
io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
|
||
io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
|
||
|
||
esp_err_t ret = gpio_config(&io_conf);
|
||
if (ret != ESP_OK) {
|
||
return SetError(QMI8658A_ERROR_INIT_FAILED);
|
||
}
|
||
|
||
// 安装中断服务
|
||
ret = gpio_install_isr_service(0);
|
||
if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) {
|
||
return SetError(QMI8658A_ERROR_INIT_FAILED);
|
||
}
|
||
|
||
// 添加中断处理程序
|
||
ret = gpio_isr_handler_add(pin, InterruptHandler, this);
|
||
if (ret != ESP_OK) {
|
||
return SetError(QMI8658A_ERROR_INIT_FAILED);
|
||
}
|
||
|
||
// 配置传感器中断
|
||
uint8_t int_config = 0;
|
||
switch (int_type) {
|
||
case QMI8658A_INT_DATA_READY:
|
||
int_config = 0x01;
|
||
break;
|
||
case QMI8658A_INT_FIFO_WATERMARK:
|
||
int_config = 0x02;
|
||
break;
|
||
case QMI8658A_INT_FIFO_FULL:
|
||
int_config = 0x04;
|
||
break;
|
||
case QMI8658A_INT_MOTION_DETECT:
|
||
int_config = 0x08;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
WriteReg(0x56, int_config); // INT_EN寄存器
|
||
|
||
interrupt_enabled_ = true;
|
||
ESP_LOGI(TAG, "Interrupt configured: type=%d, pin=%d", int_type, pin);
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::EnableFIFO(const qmi8658a_fifo_config_t* fifo_config) {
|
||
if (!fifo_config) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
fifo_config_ = *fifo_config;
|
||
|
||
// 配置FIFO
|
||
uint8_t fifo_ctrl = 0x40; // 启用FIFO
|
||
if (fifo_config->watermark > 0 && fifo_config->watermark <= QMI8658A_FIFO_SIZE) {
|
||
fifo_ctrl |= (fifo_config->watermark & 0x1F);
|
||
}
|
||
|
||
WriteReg(0x13, fifo_ctrl); // FIFO_CTRL寄存器
|
||
|
||
// 如果配置了中断，设置中断
|
||
if (fifo_config->interrupt_type != QMI8658A_INT_DISABLE) {
|
||
qmi8658a_error_t result = ConfigureInterrupt(fifo_config->interrupt_type, fifo_config->interrupt_pin);
|
||
if (result != QMI8658A_OK) {
|
||
return result;
|
||
}
|
||
}
|
||
|
||
fifo_enabled_ = true;
|
||
ESP_LOGI(TAG, "FIFO enabled with watermark: %d", fifo_config->watermark);
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::DisableFIFO() {
|
||
WriteReg(0x13, 0x00); // 禁用FIFO
|
||
fifo_enabled_ = false;
|
||
ESP_LOGI(TAG, "FIFO disabled");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ReadFIFO(qmi8658a_data_t* data_array, uint8_t max_count, uint8_t* actual_count) {
|
||
if (!data_array || !actual_count) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
*actual_count = 0;
|
||
|
||
if (!fifo_enabled_) {
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
// 读取FIFO状态
|
||
uint8_t fifo_status = ReadReg(0x14); // FIFO_STATUS寄存器
|
||
uint8_t fifo_count = fifo_status & 0x1F;
|
||
|
||
if (fifo_count == 0) {
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
uint8_t read_count = (fifo_count < max_count) ? fifo_count : max_count;
|
||
|
||
for (uint8_t i = 0; i < read_count; i++) {
|
||
qmi8658a_error_t result = ReadSensorData(&data_array[i]);
|
||
if (result != QMI8658A_OK) {
|
||
return result;
|
||
}
|
||
}
|
||
|
||
*actual_count = read_count;
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::AddToBuffer(const qmi8658a_data_t* data) {
|
||
if (!data) {
|
||
return QMI8658A_ERROR_INVALID_PARAM;
|
||
}
|
||
|
||
if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
|
||
return QMI8658A_ERROR_TIMEOUT;
|
||
}
|
||
|
||
if (data_buffer_.count >= QMI8658A_BUFFER_SIZE) {
|
||
// 缓冲区满，覆盖最旧的数据
|
||
data_buffer_.tail = (data_buffer_.tail + 1) % QMI8658A_BUFFER_SIZE;
|
||
data_buffer_.overflow = true;
|
||
} else {
|
||
data_buffer_.count++;
|
||
}
|
||
|
||
data_buffer_.data[data_buffer_.head] = *data;
|
||
data_buffer_.head = (data_buffer_.head + 1) % QMI8658A_BUFFER_SIZE;
|
||
|
||
xSemaphoreGive(data_buffer_.mutex);
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
void QMI8658A::BufferTask(void* parameter) {
|
||
QMI8658A* sensor = static_cast<QMI8658A*>(parameter);
|
||
qmi8658a_data_t data;
|
||
|
||
while (sensor->buffer_enabled_) {
|
||
if (sensor->ReadSensorData(&data) == QMI8658A_OK) {
|
||
sensor->AddToBuffer(&data);
|
||
|
||
// 如果正在校准，添加到校准数据
|
||
if (sensor->is_calibrating_) {
|
||
sensor->calibration_acc_sum_[0] += data.acc_x;
|
||
sensor->calibration_acc_sum_[1] += data.acc_y;
|
||
sensor->calibration_acc_sum_[2] += data.acc_z;
|
||
sensor->calibration_gyro_sum_[0] += data.gyro_x;
|
||
sensor->calibration_gyro_sum_[1] += data.gyro_y;
|
||
sensor->calibration_gyro_sum_[2] += data.gyro_z;
|
||
sensor->calibration_sample_count_++;
|
||
}
|
||
}
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(sensor->buffer_interval_ms_));
|
||
}
|
||
|
||
vTaskDelete(NULL);
|
||
}
|
||
|
||
void IRAM_ATTR QMI8658A::InterruptHandler(void* arg) {
|
||
QMI8658A* sensor = static_cast<QMI8658A*>(arg);
|
||
|
||
// 在中断中只做最小的处理
|
||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||
|
||
// 可以在这里设置一个标志或发送通知
|
||
// 实际的数据读取应该在任务中进行
|
||
|
||
// 使用sensor参数避免未使用变量警告
|
||
(void)sensor;
|
||
(void)xHigherPriorityTaskWoken;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ReadSensorData(qmi8658a_data_t* data) {
|
||
if (!data) {
|
||
ESP_LOGE(TAG, "Data pointer is null");
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
ESP_LOGE(TAG, "Sensor not ready, current state: %d", state_);
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
// 增加重试机制
|
||
const int max_retries = 3;
|
||
for (int retry = 0; retry < max_retries; retry++) {
|
||
// 检查数据是否就绪
|
||
if (!IsDataReady()) {
|
||
// 优化警告日志输出：降低级别并减少频率
|
||
static int data_not_ready_count = 0;
|
||
static int last_log_time = 0;
|
||
int current_time = esp_timer_get_time();
|
||
|
||
// 每10次警告或每1秒才输出一次日志，降低日志级别为DEBUG
|
||
if (data_not_ready_count % 10 == 0 || (current_time - last_log_time) > 1000000) {
|
||
ESP_LOGD(TAG, "Sensor data not ready (计数: %d), STATUS0: 0x%02X",
|
||
data_not_ready_count, ReadReg(QMI8658A_STATUS0));
|
||
last_log_time = current_time;
|
||
}
|
||
data_not_ready_count++;
|
||
|
||
// 添加小延迟后重试
|
||
if (retry < max_retries - 1) {
|
||
vTaskDelay(pdMS_TO_TICKS(5));
|
||
continue;
|
||
}
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
// 初始化数据结构
|
||
memset(data, 0, sizeof(qmi8658a_data_t));
|
||
data->timestamp = esp_timer_get_time();
|
||
data->valid = false;
|
||
|
||
qmi8658a_error_t result;
|
||
bool data_valid = true;
|
||
|
||
// 读取加速度数据
|
||
if (config_.mode == QMI8658A_MODE_ACC_ONLY || config_.mode == QMI8658A_MODE_DUAL) {
|
||
result = ReadAccelData(&data->acc_x, &data->acc_y, &data->acc_z);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to read accelerometer data, error: %d", result);
|
||
data_valid = false;
|
||
} else {
|
||
static float prev_acc_x = 0, prev_acc_y = 0, prev_acc_z = 0;
|
||
static bool acc_initialized = false;
|
||
if (!acc_initialized) {
|
||
prev_acc_x = data->acc_x;
|
||
prev_acc_y = data->acc_y;
|
||
prev_acc_z = data->acc_z;
|
||
data->acc_x = prev_acc_x; data->acc_y = prev_acc_y; data->acc_z = prev_acc_z;
|
||
acc_initialized = true;
|
||
} else {
|
||
const float alpha = 0.6f;
|
||
float filtered_x = alpha * prev_acc_x + (1 - alpha) * data->acc_x;
|
||
float filtered_y = alpha * prev_acc_y + (1 - alpha) * data->acc_y;
|
||
float filtered_z = alpha * prev_acc_z + (1 - alpha) * data->acc_z;
|
||
prev_acc_x = filtered_x; prev_acc_y = filtered_y; prev_acc_z = filtered_z;
|
||
data->acc_x = filtered_x; data->acc_y = filtered_y; data->acc_z = filtered_z;
|
||
}
|
||
if (fabs(data->acc_x) < 0.03f) data->acc_x = 0.0f;
|
||
if (fabs(data->acc_y) < 0.03f) data->acc_y = 0.0f;
|
||
ESP_LOGD(TAG, "Accel data: X=%.3f, Y=%.3f, Z=%.3f", data->acc_x, data->acc_y, data->acc_z);
|
||
}
|
||
}
|
||
|
||
// 读取陀螺仪数据
|
||
if (config_.mode == QMI8658A_MODE_GYRO_ONLY || config_.mode == QMI8658A_MODE_DUAL) {
|
||
result = ReadGyroData(&data->gyro_x, &data->gyro_y, &data->gyro_z);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to read gyroscope data, error: %d", result);
|
||
// 不再因为陀螺仪读取错误而标记整个数据无效，而是继续处理
|
||
} else {
|
||
static float prev_gyro_x = 0, prev_gyro_y = 0, prev_gyro_z = 0;
|
||
static bool gyro_initialized = false;
|
||
if (!gyro_initialized) {
|
||
prev_gyro_x = data->gyro_x;
|
||
prev_gyro_y = data->gyro_y;
|
||
prev_gyro_z = data->gyro_z;
|
||
data->gyro_x = prev_gyro_x; data->gyro_y = prev_gyro_y; data->gyro_z = prev_gyro_z;
|
||
gyro_initialized = true;
|
||
} else {
|
||
const float alpha = 0.5f;
|
||
float filtered_x = alpha * prev_gyro_x + (1 - alpha) * data->gyro_x;
|
||
float filtered_y = alpha * prev_gyro_y + (1 - alpha) * data->gyro_y;
|
||
float filtered_z = alpha * prev_gyro_z + (1 - alpha) * data->gyro_z;
|
||
prev_gyro_x = filtered_x; prev_gyro_y = filtered_y; prev_gyro_z = filtered_z;
|
||
data->gyro_x = filtered_x; data->gyro_y = filtered_y; data->gyro_z = filtered_z;
|
||
}
|
||
if (fabs(data->gyro_x) < 0.8f) data->gyro_x = 0.0f;
|
||
if (fabs(data->gyro_y) < 0.8f) data->gyro_y = 0.0f;
|
||
if (fabs(data->gyro_z) < 0.8f) data->gyro_z = 0.0f;
|
||
ESP_LOGV(TAG, "Gyro data after filtering: [%.3f, %.3f, %.3f] dps",
|
||
data->gyro_x, data->gyro_y, data->gyro_z);
|
||
}
|
||
}
|
||
|
||
// 读取温度数据
|
||
result = ReadTemperature(&data->temperature);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to read temperature data, error: %d", result);
|
||
data_valid = false;
|
||
} else {
|
||
ESP_LOGD(TAG, "Temperature: %.2f°C", data->temperature);
|
||
}
|
||
|
||
// 如果数据有效，完成读取
|
||
if (data_valid) {
|
||
data->valid = true;
|
||
ESP_LOGD(TAG, "Successfully read sensor data (retry: %d)", retry);
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
// 如果数据无效且未到最大重试次数，等待后重试
|
||
if (retry < max_retries - 1) {
|
||
ESP_LOGW(TAG, "Invalid sensor data, retrying... (%d/%d)", retry + 1, max_retries);
|
||
vTaskDelay(pdMS_TO_TICKS(10));
|
||
}
|
||
}
|
||
|
||
// 所有重试均失败
|
||
ESP_LOGE(TAG, "Failed to read valid sensor data after %d retries", max_retries);
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
bool QMI8658A::IsDataReady() {
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
return false;
|
||
}
|
||
|
||
uint8_t status = ReadReg(QMI8658A_STATUS0);
|
||
bool acc_ready = (status & 0x01) != 0;
|
||
bool gyro_ready = (status & 0x02) != 0;
|
||
switch (config_.mode) {
|
||
case QMI8658A_MODE_ACC_ONLY:
|
||
return acc_ready;
|
||
case QMI8658A_MODE_GYRO_ONLY:
|
||
return gyro_ready;
|
||
case QMI8658A_MODE_DUAL:
|
||
// 任一就绪即可开始读取，读取本身使用突发读保证快照一致性
|
||
return acc_ready || gyro_ready;
|
||
default:
|
||
return false;
|
||
}
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetError(qmi8658a_error_t error) {
|
||
last_error_ = error;
|
||
return error;
|
||
}
|
||
|
||
void QMI8658A::DumpRegisters() {
|
||
uint8_t chip = ReadReg(QMI8658A_WHO_AM_I);
|
||
uint8_t rev = ReadReg(QMI8658A_REVISION_ID);
|
||
uint8_t c1 = ReadReg(QMI8658A_CTRL1);
|
||
uint8_t c2 = ReadReg(QMI8658A_CTRL2);
|
||
uint8_t c3 = ReadReg(QMI8658A_CTRL3);
|
||
uint8_t c4 = ReadReg(QMI8658A_CTRL4);
|
||
uint8_t c5 = ReadReg(QMI8658A_CTRL5);
|
||
uint8_t c6 = ReadReg(QMI8658A_CTRL6);
|
||
uint8_t c7 = ReadReg(QMI8658A_CTRL7);
|
||
uint8_t c8 = ReadReg(QMI8658A_CTRL8);
|
||
uint8_t c9 = ReadReg(QMI8658A_CTRL9);
|
||
uint8_t s0 = ReadReg(QMI8658A_STATUS0);
|
||
uint8_t s1 = ReadReg(QMI8658A_STATUS1);
|
||
ESP_LOGI(TAG, "ID:0x%02X REV:0x%02X", chip, rev);
|
||
ESP_LOGI(TAG, "C1:0x%02X C2:0x%02X C3:0x%02X C4:0x%02X C5:0x%02X C6:0x%02X C7:0x%02X C8:0x%02X C9:0x%02X", c1,c2,c3,c4,c5,c6,c7,c8,c9);
|
||
ESP_LOGI(TAG, "S0:0x%02X S1:0x%02X", s0, s1);
|
||
}
|
||
|
||
void QMI8658A::RunBaselineDiagnostics(uint16_t samples, uint16_t interval_ms) {
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
ESP_LOGE(TAG, "Sensor not ready for diagnostics");
|
||
return;
|
||
}
|
||
double ax_sum = 0, ay_sum = 0, az_sum = 0;
|
||
double gx_sum = 0, gy_sum = 0, gz_sum = 0;
|
||
double ax_sq = 0, ay_sq = 0, az_sq = 0;
|
||
double gx_sq = 0, gy_sq = 0, gz_sq = 0;
|
||
uint16_t ok = 0;
|
||
for (uint16_t i = 0; i < samples; i++) {
|
||
qmi8658a_data_t d;
|
||
qmi8658a_error_t r = ReadSensorData(&d);
|
||
if (r == QMI8658A_OK && d.valid) {
|
||
ax_sum += d.acc_x; ay_sum += d.acc_y; az_sum += d.acc_z;
|
||
gx_sum += d.gyro_x; gy_sum += d.gyro_y; gz_sum += d.gyro_z;
|
||
ax_sq += d.acc_x * d.acc_x; ay_sq += d.acc_y * d.acc_y; az_sq += d.acc_z * d.acc_z;
|
||
gx_sq += d.gyro_x * d.gyro_x; gy_sq += d.gyro_y * d.gyro_y; gz_sq += d.gyro_z * d.gyro_z;
|
||
ok++;
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(interval_ms));
|
||
}
|
||
if (ok == 0) {
|
||
ESP_LOGE(TAG, "No valid samples for diagnostics");
|
||
return;
|
||
}
|
||
double ax_mean = ax_sum / ok, ay_mean = ay_sum / ok, az_mean = az_sum / ok;
|
||
double gx_mean = gx_sum / ok, gy_mean = gy_sum / ok, gz_mean = gz_sum / ok;
|
||
double ax_std = sqrt(ax_sq / ok - ax_mean * ax_mean);
|
||
double ay_std = sqrt(ay_sq / ok - ay_mean * ay_mean);
|
||
double az_std = sqrt(az_sq / ok - az_mean * az_mean);
|
||
double gx_std = sqrt(gx_sq / ok - gx_mean * gx_mean);
|
||
double gy_std = sqrt(gy_sq / ok - gy_mean * gy_mean);
|
||
double gz_std = sqrt(gz_sq / ok - gz_mean * gz_mean);
|
||
double acc_mag = sqrt(ax_mean * ax_mean + ay_mean * ay_mean + az_mean * az_mean);
|
||
double acc_bias = fabs(acc_mag - 1.0);
|
||
ESP_LOGI(TAG, "Baseline accel mean: [%.4f, %.4f, %.4f] g", ax_mean, ay_mean, az_mean);
|
||
ESP_LOGI(TAG, "Baseline accel std: [%.4f, %.4f, %.4f] g", ax_std, ay_std, az_std);
|
||
ESP_LOGI(TAG, "Accel |mean|-1g: %.5f g", acc_bias);
|
||
ESP_LOGI(TAG, "Baseline gyro mean: [%.4f, %.4f, %.4f] dps", gx_mean, gy_mean, gz_mean);
|
||
ESP_LOGI(TAG, "Baseline gyro std: [%.4f, %.4f, %.4f] dps", gx_std, gy_std, gz_std);
|
||
}
|
||
// 新增：带验证的寄存器写入函数
|
||
qmi8658a_error_t QMI8658A::WriteRegWithVerification(uint8_t reg, uint8_t value, uint8_t max_retries) {
|
||
for (uint8_t retry = 0; retry < max_retries; retry++) {
|
||
// 写入寄存器
|
||
esp_err_t ret = WriteRegWithError(reg, value);
|
||
if (ret != ESP_OK) {
|
||
ESP_LOGE(TAG, "Failed to write register 0x%02X (attempt %d/%d): %s",
|
||
reg, retry + 1, max_retries, esp_err_to_name(ret));
|
||
if (retry == max_retries - 1) {
|
||
return SetError(QMI8658A_ERROR_I2C_COMM);
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(10)); // 等待10ms后重试
|
||
continue;
|
||
}
|
||
|
||
// 等待写入完成
|
||
vTaskDelay(pdMS_TO_TICKS(5));
|
||
|
||
// 读回验证
|
||
uint8_t read_value = ReadReg(reg);
|
||
if (read_value == 0xFF) {
|
||
ESP_LOGE(TAG, "Failed to read back register 0x%02X for verification (attempt %d/%d)",
|
||
reg, retry + 1, max_retries);
|
||
if (retry == max_retries - 1) {
|
||
return SetError(QMI8658A_ERROR_I2C_COMM);
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(10));
|
||
continue;
|
||
}
|
||
|
||
if (read_value == value) {
|
||
ESP_LOGI(TAG, "Register 0x%02X successfully written and verified: 0x%02X", reg, value);
|
||
return QMI8658A_OK;
|
||
} else {
|
||
ESP_LOGW(TAG, "Register 0x%02X verification failed (attempt %d/%d): wrote 0x%02X, read 0x%02X",
|
||
reg, retry + 1, max_retries, value, read_value);
|
||
if (retry == max_retries - 1) {
|
||
return SetError(QMI8658A_ERROR_CONFIG_FAILED);
|
||
}
|
||
vTaskDelay(pdMS_TO_TICKS(10));
|
||
}
|
||
}
|
||
|
||
return SetError(QMI8658A_ERROR_CONFIG_FAILED);
|
||
}
|
||
|
||
// 新增：验证寄存器值函数
|
||
qmi8658a_error_t QMI8658A::VerifyRegisterValue(uint8_t reg, uint8_t expected_value, const char* reg_name) {
|
||
uint8_t actual_value = ReadReg(reg);
|
||
if (actual_value == 0xFF) {
|
||
ESP_LOGE(TAG, "Failed to read %s register (0x%02X) for verification", reg_name, reg);
|
||
return SetError(QMI8658A_ERROR_I2C_COMM);
|
||
}
|
||
|
||
if (actual_value != expected_value) {
|
||
ESP_LOGE(TAG, "%s register (0x%02X) verification failed: expected 0x%02X, got 0x%02X",
|
||
reg_name, reg, expected_value, actual_value);
|
||
return SetError(QMI8658A_ERROR_CONFIG_FAILED);
|
||
}
|
||
|
||
ESP_LOGI(TAG, "%s register (0x%02X) verified successfully: 0x%02X", reg_name, reg, actual_value);
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
// 新增：等待数据就绪函数
|
||
qmi8658a_error_t QMI8658A::WaitForDataReady(uint32_t timeout_ms) {
|
||
uint32_t start_time = esp_timer_get_time() / 1000; // 转换为毫秒
|
||
uint32_t elapsed_time = 0;
|
||
|
||
ESP_LOGI(TAG, "Waiting for data ready (timeout: %lu ms)...", timeout_ms);
|
||
|
||
// 分阶段检查：先检查任一传感器就绪，再检查同时就绪
|
||
bool any_ready = false;
|
||
|
||
while (elapsed_time < timeout_ms) {
|
||
uint8_t status0 = ReadReg(QMI8658A_STATUS0);
|
||
if (status0 == 0xFF) {
|
||
ESP_LOGE(TAG, "Failed to read STATUS0 register while waiting for data ready");
|
||
return SetError(QMI8658A_ERROR_I2C_COMM);
|
||
}
|
||
|
||
// 检查加速度计和陀螺仪数据是否就绪
|
||
bool acc_ready = (status0 & 0x01) != 0;
|
||
bool gyro_ready = (status0 & 0x02) != 0;
|
||
|
||
// 检测到任一传感器就绪，记录并继续等待
|
||
if (acc_ready || gyro_ready) {
|
||
if (!any_ready) {
|
||
ESP_LOGI(TAG, "At least one sensor ready after %lu ms (STATUS0: 0x%02X)", elapsed_time, status0);
|
||
any_ready = true;
|
||
}
|
||
|
||
// 如果任一传感器就绪且已等待至少100ms，就返回成功
|
||
// 这解决了某些情况下一个传感器就绪另一个未就绪的问题
|
||
if (any_ready && elapsed_time > 100) {
|
||
ESP_LOGI(TAG, "At least one sensor ready, proceeding after %lu ms (STATUS0: 0x%02X)", elapsed_time, status0);
|
||
return QMI8658A_OK;
|
||
}
|
||
}
|
||
|
||
// 根据是否已检测到传感器就绪调整等待时间
|
||
uint8_t delay_time = any_ready ? 5 : 10; // 已检测到就绪则减少等待时间
|
||
vTaskDelay(pdMS_TO_TICKS(delay_time));
|
||
elapsed_time = (esp_timer_get_time() / 1000) - start_time;
|
||
}
|
||
|
||
ESP_LOGI(TAG, "Timeout waiting for data ready after %lu ms", timeout_ms);
|
||
return SetError(QMI8658A_ERROR_TIMEOUT);
|
||
|
||
}
|
||
|
||
// 新增：自检函数
|
||
qmi8658a_error_t QMI8658A::PerformSelfTest() {
|
||
ESP_LOGI(TAG, "Performing self-test...");
|
||
|
||
// 检查芯片ID
|
||
uint8_t chip_id = GetChipId();
|
||
if (chip_id != QMI8658A_CHIP_ID) {
|
||
ESP_LOGE(TAG, "Self-test failed: Invalid chip ID 0x%02X", chip_id);
|
||
return SetError(QMI8658A_ERROR_CHIP_ID);
|
||
}
|
||
|
||
// 检查关键寄存器的可读性
|
||
uint8_t test_regs[] = {QMI8658A_WHO_AM_I, QMI8658A_REVISION_ID, QMI8658A_STATUS0, QMI8658A_STATUS1};
|
||
const char* test_reg_names[] = {"WHO_AM_I", "REVISION_ID", "STATUS0", "STATUS1"};
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
uint8_t value = ReadReg(test_regs[i]);
|
||
if (value == 0xFF) {
|
||
ESP_LOGE(TAG, "Self-test failed: Cannot read %s register (0x%02X)",
|
||
test_reg_names[i], test_regs[i]);
|
||
return SetError(QMI8658A_ERROR_I2C_COMM);
|
||
}
|
||
ESP_LOGI(TAG, "Self-test: %s (0x%02X) = 0x%02X", test_reg_names[i], test_regs[i], value);
|
||
}
|
||
|
||
ESP_LOGI(TAG, "Self-test completed successfully");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::UpdateConfiguration(const qmi8658a_config_t* new_config) {
|
||
if (!new_config) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
// 验证新配置
|
||
qmi8658a_error_t result = ValidateConfiguration(new_config);
|
||
if (result != QMI8658A_OK) {
|
||
return result;
|
||
}
|
||
|
||
// 保存旧配置以便回滚
|
||
qmi8658a_config_t old_config = config_;
|
||
config_ = *new_config;
|
||
|
||
// 应用配置更改
|
||
result = ApplyConfigurationChanges();
|
||
if (result != QMI8658A_OK) {
|
||
// 回滚到旧配置
|
||
config_ = old_config;
|
||
return result;
|
||
}
|
||
|
||
ESP_LOGI(TAG, "Configuration updated successfully");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ValidateConfiguration(const qmi8658a_config_t* config) {
|
||
if (!config) {
|
||
return QMI8658A_ERROR_INVALID_PARAM;
|
||
}
|
||
|
||
// 验证加速度计量程
|
||
if (config->acc_range > QMI8658A_ACC_RANGE_16G) {
|
||
ESP_LOGE(TAG, "Invalid accelerometer range: %d", config->acc_range);
|
||
return QMI8658A_ERROR_INVALID_PARAM;
|
||
}
|
||
|
||
// 验证陀螺仪量程
|
||
if (config->gyro_range > QMI8658A_GYRO_RANGE_2048DPS) {
|
||
ESP_LOGE(TAG, "Invalid gyroscope range: %d", config->gyro_range);
|
||
return QMI8658A_ERROR_INVALID_PARAM;
|
||
}
|
||
|
||
// 验证ODR设置
|
||
if (config->acc_odr > QMI8658A_ODR_8000HZ || config->gyro_odr > QMI8658A_ODR_8000HZ) {
|
||
ESP_LOGE(TAG, "Invalid ODR setting");
|
||
return QMI8658A_ERROR_INVALID_PARAM;
|
||
}
|
||
|
||
// 验证工作模式
|
||
if (config->mode > QMI8658A_MODE_DUAL) {
|
||
ESP_LOGE(TAG, "Invalid operation mode: %d", config->mode);
|
||
return QMI8658A_ERROR_INVALID_PARAM;
|
||
}
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::GetConfiguration(qmi8658a_config_t* config) {
|
||
if (!config) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
*config = config_;
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetAccelRange(qmi8658a_acc_range_t range) {
|
||
if (range > QMI8658A_ACC_RANGE_16G) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
config_.acc_range = range;
|
||
qmi8658a_error_t result = SetAccelConfig(range, config_.acc_odr);
|
||
if (result == QMI8658A_OK) {
|
||
UpdateScaleFactors();
|
||
}
|
||
return result;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetGyroRange(qmi8658a_gyro_range_t range) {
|
||
if (range > QMI8658A_GYRO_RANGE_2048DPS) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
config_.gyro_range = range;
|
||
qmi8658a_error_t result = SetGyroConfig(range, config_.gyro_odr);
|
||
if (result == QMI8658A_OK) {
|
||
UpdateScaleFactors();
|
||
}
|
||
return result;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetAccelODR(qmi8658a_odr_t odr) {
|
||
if (odr > QMI8658A_ODR_8000HZ) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
config_.acc_odr = odr;
|
||
return SetAccelConfig(config_.acc_range, odr);
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetGyroODR(qmi8658a_odr_t odr) {
|
||
if (odr > QMI8658A_ODR_8000HZ) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
config_.gyro_odr = odr;
|
||
return SetGyroConfig(config_.gyro_range, odr);
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetOperationMode(qmi8658a_mode_t mode) {
|
||
if (mode > QMI8658A_MODE_DUAL) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
config_.mode = mode;
|
||
return SetMode(mode);
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::StartCalibration(uint32_t duration_ms) {
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
return SetError(QMI8658A_ERROR_DATA_NOT_READY);
|
||
}
|
||
|
||
is_calibrating_ = true;
|
||
calibration_start_time_ = esp_timer_get_time() / 1000; // 转换为毫秒
|
||
calibration_duration_ = duration_ms;
|
||
calibration_sample_count_ = 0;
|
||
|
||
// 清零累加器
|
||
memset(calibration_acc_sum_, 0, sizeof(calibration_acc_sum_));
|
||
memset(calibration_gyro_sum_, 0, sizeof(calibration_gyro_sum_));
|
||
|
||
ESP_LOGI(TAG, "Started calibration for %" PRIu32 " ms", duration_ms);
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::GetCalibrationStatus(bool* is_calibrating, float* progress) {
|
||
if (!is_calibrating || !progress) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
*is_calibrating = is_calibrating_;
|
||
|
||
if (is_calibrating_) {
|
||
uint32_t current_time = esp_timer_get_time() / 1000;
|
||
uint32_t elapsed = current_time - calibration_start_time_;
|
||
*progress = (float)elapsed / calibration_duration_;
|
||
|
||
if (elapsed >= calibration_duration_) {
|
||
// 校准完成，计算偏置
|
||
if (calibration_sample_count_ > 0) {
|
||
float mean_ax = calibration_acc_sum_[0] / calibration_sample_count_;
|
||
float mean_ay = calibration_acc_sum_[1] / calibration_sample_count_;
|
||
float mean_az = calibration_acc_sum_[2] / calibration_sample_count_;
|
||
float mean_gx = calibration_gyro_sum_[0] / calibration_sample_count_;
|
||
float mean_gy = calibration_gyro_sum_[1] / calibration_sample_count_;
|
||
float mean_gz = calibration_gyro_sum_[2] / calibration_sample_count_;
|
||
calibration_.acc_bias[0] = mean_ax;
|
||
calibration_.acc_bias[1] = mean_ay;
|
||
calibration_.acc_bias[2] = mean_az - 1.0f;
|
||
calibration_.gyro_bias[0] = mean_gx;
|
||
calibration_.gyro_bias[1] = mean_gy;
|
||
calibration_.gyro_bias[2] = mean_gz;
|
||
calibration_.is_calibrated = true;
|
||
calibration_.calibration_time = current_time;
|
||
}
|
||
|
||
is_calibrating_ = false;
|
||
*is_calibrating = false;
|
||
*progress = 1.0f;
|
||
|
||
ESP_LOGI(TAG, "Calibration completed with %" PRIu32 " samples", calibration_sample_count_);
|
||
}
|
||
} else {
|
||
*progress = 0.0f;
|
||
}
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ApplyCalibration(const qmi8658a_calibration_t* calibration) {
|
||
if (!calibration) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
calibration_ = *calibration;
|
||
ESP_LOGI(TAG, "Applied calibration data");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::GetCalibrationData(qmi8658a_calibration_t* calibration) {
|
||
if (!calibration) {
|
||
return SetError(QMI8658A_ERROR_INVALID_PARAM);
|
||
}
|
||
|
||
*calibration = calibration_;
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
void QMI8658A::UpdateScaleFactors() {
|
||
// 根据加速度计量程计算比例因子
|
||
switch (config_.acc_range) {
|
||
case QMI8658A_ACC_RANGE_2G:
|
||
acc_scale_ = 2.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_ACC_RANGE_4G:
|
||
acc_scale_ = 4.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_ACC_RANGE_8G:
|
||
acc_scale_ = 8.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_ACC_RANGE_16G:
|
||
acc_scale_ = 16.0f / 32768.0f;
|
||
break;
|
||
default:
|
||
acc_scale_ = 2.0f / 32768.0f;
|
||
break;
|
||
}
|
||
|
||
// 根据陀螺仪量程计算比例因子
|
||
switch (config_.gyro_range) {
|
||
case QMI8658A_GYRO_RANGE_16DPS:
|
||
gyro_scale_ = 16.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_32DPS:
|
||
gyro_scale_ = 32.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_64DPS:
|
||
gyro_scale_ = 64.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_128DPS:
|
||
gyro_scale_ = 128.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_256DPS:
|
||
gyro_scale_ = 256.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_512DPS:
|
||
gyro_scale_ = 512.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_1024DPS:
|
||
gyro_scale_ = 1024.0f / 32768.0f;
|
||
break;
|
||
case QMI8658A_GYRO_RANGE_2048DPS:
|
||
gyro_scale_ = 2048.0f / 32768.0f;
|
||
break;
|
||
default:
|
||
gyro_scale_ = 256.0f / 32768.0f;
|
||
break;
|
||
}
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::ApplyConfigurationChanges() {
|
||
qmi8658a_error_t result;
|
||
|
||
// 应用加速度计配置
|
||
result = SetAccelConfig(config_.acc_range, config_.acc_odr);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to configure accelerometer");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 应用陀螺仪配置
|
||
result = SetGyroConfig(config_.gyro_range, config_.gyro_odr);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to configure gyroscope");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 设置工作模式
|
||
uint8_t mode_val = config_.mode;
|
||
result = SetMode(static_cast<qmi8658a_mode_t>(mode_val));
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to set mode");
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 更新比例因子
|
||
UpdateScaleFactors();
|
||
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
QMI8658A::~QMI8658A() {
|
||
// 停止缓冲读取
|
||
if (buffer_enabled_) {
|
||
StopBufferedReading();
|
||
}
|
||
|
||
// 停止校准
|
||
if (is_calibrating_) {
|
||
is_calibrating_ = false;
|
||
}
|
||
|
||
// 清理缓冲区 - 只有在mutex已创建时才释放
|
||
if (data_buffer_.mutex != nullptr) {
|
||
vSemaphoreDelete(data_buffer_.mutex);
|
||
data_buffer_.mutex = nullptr;
|
||
}
|
||
|
||
// 禁用FIFO
|
||
if (fifo_enabled_) {
|
||
DisableFIFO();
|
||
}
|
||
|
||
// 设置为禁用模式 - 只有在传感器已初始化时才调用
|
||
if (state_ != QMI8658A_STATE_UNINITIALIZED) {
|
||
SetMode(QMI8658A_MODE_DISABLE);
|
||
}
|
||
|
||
ESP_LOGI(TAG, "QMI8658A destructor completed");
|
||
}
|