STM32F103C8T6 + MPU9250 + MPL库实战：从CubeMX配置到姿态解算（附完整代码）
STM32F103C8T6 + MPU9250 + MPL库实战：从CubeMX配置到姿态解算（附完整代码）
在嵌入式开发领域，姿态解算一直是无人机、机器人等项目的核心技术难点。本文将手把手带你用成本不到50元的硬件组合（STM32F103C8T6最小系统板 + GY-91模块），实现工业级精度的三维姿态解算。不同于网上零散的教程，我们从CubeMX配置、MPL库移植到上位机可视化，提供完整闭环解决方案，特别适合需要快速验证方案的开发者。
1. 硬件准备与CubeMX工程配置
1.1 硬件连接与模块选型
推荐使用GY-91模块（集成MPU9250 + BMP280），其核心参数如下：
| 传感器 | 量程 | 分辨率 | 通信接口 |
|---|---|---|---|
| MPU9250 | ±16g / ±2000°/s | 16位ADC | I2C/SPI |
| BMP280 | 300-1100hPa | 0.01hPa | I2C |
硬件连接示意图：
STM32F103C8T6 GY-91模块 PB6 (SCL) ---- SCL PB7 (SDA) ---- SDA 3.3V ---- VCC GND ---- GND注意：GY-91模块的AD0引脚默认接地，I2C地址为0x68。若需使用SPI接口，需修改模块背面电阻配置。
1.2 CubeMX关键配置步骤
时钟树配置：
- 选择HSE（8MHz外部晶振）
- 设置系统时钟为72MHz
- 配置APB1总线时钟为36MHz（I2C时钟源）
I2C1参数设置：
hi2c1.Instance = I2C1; hi2c1.Init.ClockSpeed = 400000; // 快速模式 hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; hi2c1.Init.OwnAddress1 = 0; hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;USART1调试输出：
- 波特率：500000 bps
- 数据位：8 bits
- 停止位：1 bit
- 无校验
生成工程代码：
- 工具链选择MDK-ARM V5
- 勾选"Generate peripheral initialization as a pair of .c/.h files"
2. MPL库移植与工程配置
2.1 获取官方驱动库
从InvenSense官网下载MotionDriver 6.12，解压后重点关注以下文件：
motion_driver_6.12/ ├── eMPL/ │ ├── inv_mpu.c // 传感器底层驱动 │ ├── inv_mpu_dmp_motion_driver.c // DMP固件 │ └── mlmath.c // 数学运算库 └── lib/ └── CM3/ └── libmpllib.a // MPL静态库2.2 工程目录结构调整
建议按如下方式组织项目文件：
YourProject/ ├── Drivers/ ├── Inc/ │ ├── mpl_config.h # 新增MPL配置头文件 │ └── mpu9250.h # 新增传感器驱动头文件 ├── Src/ │ ├── mpu9250.c # 新增传感器驱动源文件 │ └── main.c └── Middlewares/ └── InvenSense/ ├── eMPL/ # 官方驱动文件 └── lib/ # 静态库文件2.3 关键宏定义配置
在mpl_config.h中添加以下定义：
#define EMPL_TARGET_STM32F1 #define MPU9250 #define MPL_LOG_NDEBUG 1 #define INV_XYZ_ACCEL (0x01) #define INV_X_GYRO (0x02) #define INV_Y_GYRO (0x04) #define INV_Z_GYRO (0x08) #define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO) #define INV_XYZ_COMPASS (0x20)3. 传感器初始化与数据读取
3.1 MPU9250初始化流程
完整的传感器初始化代码如下：
uint8_t mpu_init(void) { uint8_t res = 0; MPU_Reset(); HAL_Delay(100); // 设置采样率1kHz，陀螺仪范围±2000°/s，加速度计±16g res |= mpu_set_gyro_fsr(2000); res |= mpu_set_accel_fsr(16); res |= mpu_set_sample_rate(1000); // 启用所有传感器 res |= mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // 配置磁力计为连续测量模式 res |= mpu_set_bypass(1); res |= ak8963_set_mode(AK8963_MODE_CONTINUOUS_100HZ); res |= mpu_set_bypass(0); return res; }3.2 MPL库初始化
uint8_t mpl_init(void) { struct int_param_s int_param; inv_error_t err; // 初始化MPL库 err = inv_init_mpl(); if (err) return 1; // 启用9轴传感器融合 inv_enable_quaternion(); inv_enable_9x_sensor_fusion(); inv_enable_fast_nomot(); // 设置传感器方向矩阵 signed char gyro_orientation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; inv_set_gyro_orientation( inv_orientation_matrix_to_scalar(gyro_orientation) ); return 0; }4. 姿态解算与数据可视化
4.1 实时解算实现
在main.c中创建解算任务：
void attitude_task(void const *argument) { float pitch, roll, yaw; int16_t accel[3], gyro[3], mag[3]; while(1) { // 读取原始数据 mpu_get_accel_reg(accel, NULL); mpu_get_gyro_reg(gyro, NULL); mpu_get_compass_reg(mag, NULL); // 数据送入MPL处理 inv_build_accel(accel, 0, HAL_GetTick()); inv_build_gyro(gyro, HAL_GetTick()); inv_build_compass(mag, 0, HAL_GetTick()); // 执行解算 inv_execute_on_data(); // 获取欧拉角 long data[3]; int8_t accuracy; inv_get_sensor_type_euler(data, &accuracy, NULL); pitch = data[1] / 65536.0f; roll = data[0] / 65536.0f; yaw = data[2] / 65536.0f; // 通过串口输出 printf("Pitch:%.2f\tRoll:%.2f\tYaw:%.2f\r\n", pitch, roll, yaw); osDelay(10); } }4.2 上位机数据可视化
推荐使用匿名四轴上位机或**Vofa+**工具，数据帧格式如下：
void send_attitude_data(float pitch, float roll, float yaw) { uint8_t buf[12]; int16_t pitch_i16 = pitch * 100; // 0.01度精度 int16_t roll_i16 = roll * 100; int16_t yaw_i16 = yaw * 10; // 0.1度精度 buf[0] = (roll_i16 >> 8) & 0xFF; buf[1] = roll_i16 & 0xFF; buf[2] = (pitch_i16 >> 8) & 0xFF; buf[3] = pitch_i16 & 0xFF; buf[4] = (yaw_i16 >> 8) & 0xFF; buf[5] = yaw_i16 & 0xFF; HAL_UART_Transmit(&huart1, buf, 6, 100); }5. 常见问题排查
5.1 I2C通信失败
典型症状：mpu_init()返回非零值
排查步骤：
- 用逻辑分析仪检查SCL/SDA信号
- 确认上拉电阻（4.7kΩ）已接
- 检查地址配置（0x68或0x69）
5.2 姿态解算漂移
优化方案：
- 在静止状态下执行校准：
void calibrate_sensors(void) { int32_t gyro_bias[3] = {0}; int32_t accel_bias[3] = {0}; // 采集100次样本求平均 for(int i=0; i<100; i++) { int16_t accel[3], gyro[3]; mpu_get_accel_reg(accel, NULL); mpu_get_gyro_reg(gyro, NULL); accel_bias[0] += accel[0]; accel_bias[1] += accel[1]; accel_bias[2] += (accel[2] - 16384); // 减去1g gyro_bias[0] += gyro[0]; gyro_bias[1] += gyro[1]; gyro_bias[2] += gyro[2]; HAL_Delay(10); } // 设置零偏补偿 inv_set_gyro_bias(gyro_bias, 3); inv_set_accel_bias(accel_bias, 3); }
5.3 磁力计干扰处理
当存在金属干扰时，添加软铁补偿：
void compass_calibration(void) { static float mag_bias[3] = {0}; static float mag_scale[3] = {1, 1, 1}; // 采集数据时绕8字形旋转设备 inv_set_compass_bias(mag_bias, 3); inv_set_compass_scale(mag_scale, 3); }6. 性能优化技巧
6.1 降低CPU负载
- 将MPL解算频率设置为100Hz：
inv_set_gyro_sample_rate(10000); // 100Hz = 1000000us/100 inv_set_accel_sample_rate(10000); inv_set_compass_sample_rate(100000); // 磁力计10Hz
6.2 内存优化
修改startup_stm32f103xb.s中的堆栈设置：
Stack_Size EQU 0x00000800 ; 2KB栈空间 Heap_Size EQU 0x00000400 ; 1KB堆空间6.3 实时性保障
使用FreeRTOS创建独立传感器任务：
osThreadDef(attitude, attitude_task, osPriorityHigh, 0, 512); osThreadCreate(osThread(attitude), NULL);