139 lines
4.3 KiB
C++
139 lines
4.3 KiB
C++
#include "imu_manager.h"
|
|
#include <esp_log.h>
|
|
|
|
#define TAG "ImuManager"
|
|
|
|
ImuManager& ImuManager::GetInstance() {
|
|
static ImuManager instance;
|
|
return instance;
|
|
}
|
|
|
|
bool ImuManager::Initialize() {
|
|
if (initialized_) {
|
|
ESP_LOGW(TAG, "ImuManager already initialized");
|
|
return true;
|
|
}
|
|
|
|
ESP_LOGI(TAG, "Initializing ImuManager...");
|
|
|
|
InitializeImu();
|
|
|
|
// 启动IMU任务
|
|
StartImuTask();
|
|
|
|
initialized_ = true;
|
|
ESP_LOGI(TAG, "ImuManager initialized successfully");
|
|
return true;
|
|
}
|
|
|
|
void ImuManager::InitializeImu() {
|
|
ESP_LOGI(TAG, "Initializing MPU6050 IMU sensor...");
|
|
|
|
// IMU传感器I2C总线
|
|
i2c_master_bus_config_t imu_i2c_cfg = {
|
|
.i2c_port = I2C_NUM_1,
|
|
.sda_io_num = IMU_I2C_SDA_PIN,
|
|
.scl_io_num = IMU_I2C_SCL_PIN,
|
|
.clk_source = I2C_CLK_SRC_DEFAULT,
|
|
.glitch_ignore_cnt = 7,
|
|
.intr_priority = 0,
|
|
.trans_queue_depth = 0,
|
|
.flags = {
|
|
.enable_internal_pullup = 1,
|
|
.allow_pd = false,
|
|
},
|
|
};
|
|
ESP_ERROR_CHECK(i2c_new_master_bus(&imu_i2c_cfg, &imu_i2c_bus_));
|
|
|
|
// 初始化MPU6050传感器
|
|
mpu6050_sensor_ = std::make_unique<Mpu6050Sensor>(imu_i2c_bus_);
|
|
|
|
if (mpu6050_sensor_) {
|
|
uint8_t device_id;
|
|
if (mpu6050_sensor_->GetDeviceId(&device_id)) {
|
|
ESP_LOGI(TAG, "MPU6050 device ID: 0x%02X", device_id);
|
|
if (device_id == MPU6050_WHO_AM_I_VAL) {
|
|
if (mpu6050_sensor_->Initialize(ACCE_FS_4G, GYRO_FS_500DPS)) {
|
|
if (mpu6050_sensor_->WakeUp()) {
|
|
initialized_ = true;
|
|
ESP_LOGI(TAG, "MPU6050 sensor initialized successfully");
|
|
} else {
|
|
ESP_LOGE(TAG, "Failed to wake up MPU6050");
|
|
}
|
|
} else {
|
|
ESP_LOGE(TAG, "Failed to initialize MPU6050");
|
|
}
|
|
} else {
|
|
ESP_LOGE(TAG, "MPU6050 device ID mismatch: expected 0x%02X, got 0x%02X",
|
|
MPU6050_WHO_AM_I_VAL, device_id);
|
|
}
|
|
} else {
|
|
ESP_LOGE(TAG, "Failed to read MPU6050 device ID");
|
|
}
|
|
} else {
|
|
ESP_LOGE(TAG, "Failed to create MPU6050 sensor instance");
|
|
}
|
|
|
|
if (!initialized_) {
|
|
ESP_LOGW(TAG, "IMU sensor initialization failed - continuing without IMU");
|
|
}
|
|
}
|
|
|
|
void ImuManager::StartImuTask() {
|
|
if (!initialized_) {
|
|
ESP_LOGW(TAG, "ImuManager not initialized, skipping IMU task creation");
|
|
return;
|
|
}
|
|
|
|
BaseType_t ret = xTaskCreate(ImuDataTask, "imu_data_task", 4096, this, 5, &imu_task_handle_);
|
|
if (ret != pdPASS) {
|
|
ESP_LOGE(TAG, "Failed to create IMU data task");
|
|
} else {
|
|
ESP_LOGI(TAG, "IMU data task created successfully");
|
|
}
|
|
}
|
|
|
|
void ImuManager::StopImuTask() {
|
|
if (imu_task_handle_) {
|
|
vTaskDelete(imu_task_handle_);
|
|
imu_task_handle_ = nullptr;
|
|
}
|
|
}
|
|
|
|
void ImuManager::ImuDataTask(void *pvParameters) {
|
|
ImuManager *manager = static_cast<ImuManager *>(pvParameters);
|
|
ESP_LOGI(TAG, "IMU data task started");
|
|
|
|
mpu6050_acce_value_t acce;
|
|
mpu6050_gyro_value_t gyro;
|
|
mpu6050_temp_value_t temp;
|
|
complimentary_angle_t angle;
|
|
|
|
while (true) {
|
|
if (manager->mpu6050_sensor_ && manager->initialized_) {
|
|
// 读取加速度计数据
|
|
if (manager->mpu6050_sensor_->GetAccelerometer(&acce)) {
|
|
ESP_LOGI(TAG, "Accelerometer - X:%.2f, Y:%.2f, Z:%.2f", acce.acce_x,
|
|
acce.acce_y, acce.acce_z);
|
|
}
|
|
|
|
// 读取陀螺仪数据
|
|
if (manager->mpu6050_sensor_->GetGyroscope(&gyro)) {
|
|
ESP_LOGI(TAG, "Gyroscope - X:%.2f, Y:%.2f, Z:%.2f", gyro.gyro_x,
|
|
gyro.gyro_y, gyro.gyro_z);
|
|
}
|
|
|
|
// 读取温度数据
|
|
if (manager->mpu6050_sensor_->GetTemperature(&temp)) {
|
|
ESP_LOGI(TAG, "Temperature: %.2f°C", temp.temp);
|
|
}
|
|
|
|
// 计算姿态角
|
|
if (manager->mpu6050_sensor_->ComplimentaryFilter(&acce, &gyro, &angle)) {
|
|
ESP_LOGI(TAG, "Attitude - Pitch:%.2f°, Roll:%.2f°, Yaw:%.2f°",
|
|
angle.pitch, angle.roll, angle.yaw);
|
|
}
|
|
}
|
|
vTaskDelay(pdMS_TO_TICKS(50));
|
|
}
|
|
} |