1248 lines
38 KiB
C++
1248 lines
38 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(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;
|
||
}
|
||
|
||
// 等待传感器稳定
|
||
vTaskDelay(pdMS_TO_TICKS(50));
|
||
|
||
// 验证关键寄存器配置
|
||
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;
|
||
}
|
||
|
||
result = VerifyRegisterValue(QMI8658A_CTRL7, mode_val, "CTRL7 (Mode)");
|
||
if (result != QMI8658A_OK) {
|
||
state_ = QMI8658A_STATE_ERROR;
|
||
return result;
|
||
}
|
||
|
||
// 等待数据就绪
|
||
if (config_.mode != QMI8658A_MODE_DISABLE) {
|
||
ESP_LOGI(TAG, "Waiting for sensor data to be ready...");
|
||
result = WaitForDataReady(2000); // 2秒超时
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGW(TAG, "Data ready timeout, but continuing initialization");
|
||
// 不将此作为致命错误，继续初始化
|
||
}
|
||
}
|
||
|
||
// 诊断寄存器状态
|
||
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 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, CTRL7: 0x%02X",
|
||
ctrl1, ctrl2, ctrl3, ctrl7);
|
||
ESP_LOGI(TAG, "STATUS0: 0x%02X, STATUS1: 0x%02X", status0, status1);
|
||
ESP_LOGI(TAG, "=====================================");
|
||
|
||
// 等待数据就绪
|
||
ESP_LOGI(TAG, "Waiting for data 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;
|
||
ESP_LOGI(TAG, "QMI8658A initialization completed successfully");
|
||
|
||
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_CTRL1, 0xB0);
|
||
if (result != QMI8658A_OK) {
|
||
ESP_LOGE(TAG, "Failed to perform soft reset");
|
||
return result;
|
||
}
|
||
|
||
// 等待复位完成
|
||
vTaskDelay(pdMS_TO_TICKS(20)); // 增加等待时间确保复位完成
|
||
|
||
// 验证复位是否成功 - 检查芯片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);
|
||
|
||
// 使用带验证的写入函数，最多重试3次
|
||
qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL7, mode, 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", mode);
|
||
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;
|
||
|
||
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;
|
||
|
||
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);
|
||
}
|
||
|
||
int16_t raw_x = ReadInt16(QMI8658A_AX_L);
|
||
int16_t raw_y = ReadInt16(QMI8658A_AY_L);
|
||
int16_t raw_z = ReadInt16(QMI8658A_AZ_L);
|
||
|
||
*acc_x = raw_x * acc_scale_;
|
||
*acc_y = raw_y * acc_scale_;
|
||
*acc_z = raw_z * acc_scale_;
|
||
|
||
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);
|
||
}
|
||
|
||
int16_t raw_x = ReadInt16(QMI8658A_GX_L);
|
||
int16_t raw_y = ReadInt16(QMI8658A_GY_L);
|
||
int16_t raw_z = ReadInt16(QMI8658A_GZ_L);
|
||
|
||
*gyro_x = raw_x * gyro_scale_;
|
||
*gyro_y = raw_y * gyro_scale_;
|
||
*gyro_z = raw_z * gyro_scale_;
|
||
|
||
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);
|
||
}
|
||
|
||
int16_t raw_temp = ReadInt16(QMI8658A_TEMP_L);
|
||
// 温度转换公式：温度 = raw_temp / 256.0 (°C)
|
||
*temperature = raw_temp / 256.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 (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);
|
||
}
|
||
|
||
// 检查数据是否就绪
|
||
if (!IsDataReady()) {
|
||
ESP_LOGW(TAG, "Sensor data not ready, STATUS0: 0x%02X", ReadReg(0x2D));
|
||
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;
|
||
|
||
// 读取加速度数据
|
||
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);
|
||
return result;
|
||
}
|
||
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);
|
||
return result;
|
||
}
|
||
ESP_LOGD(TAG, "Gyro data: X=%.3f, Y=%.3f, Z=%.3f", 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);
|
||
return result;
|
||
}
|
||
|
||
ESP_LOGD(TAG, "Temperature: %.2f°C", data->temperature);
|
||
data->valid = true;
|
||
ESP_LOGD(TAG, "Successfully read sensor data");
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
bool QMI8658A::IsDataReady() {
|
||
if (state_ != QMI8658A_STATE_READY) {
|
||
return false;
|
||
}
|
||
|
||
// 读取状态寄存器检查数据是否准备就绪
|
||
uint8_t status = ReadReg(0x2D); // STATUS0寄存器
|
||
return (status & 0x03) != 0; // 检查加速度计和陀螺仪数据就绪位
|
||
}
|
||
|
||
qmi8658a_error_t QMI8658A::SetError(qmi8658a_error_t error) {
|
||
last_error_ = error;
|
||
return error;
|
||
}
|
||
|
||
// 新增：带验证的寄存器写入函数
|
||
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);
|
||
|
||
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) {
|
||
ESP_LOGI(TAG, "Data ready detected after %lu ms (STATUS0: 0x%02X)", elapsed_time, status0);
|
||
return QMI8658A_OK;
|
||
}
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(10)); // 等待10ms
|
||
elapsed_time = (esp_timer_get_time() / 1000) - start_time;
|
||
}
|
||
|
||
ESP_LOGE(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) {
|
||
for (int i = 0; i < 3; i++) {
|
||
calibration_.acc_bias[i] = calibration_acc_sum_[i] / calibration_sample_count_;
|
||
calibration_.gyro_bias[i] = calibration_gyro_sum_[i] / calibration_sample_count_;
|
||
}
|
||
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");
|
||
} |