手把手教你做一个实时姿态演示器从MPU6050到3D姿态显示最近有不少朋友在问怎么用单片机读取MPU6050传感器的数据然后实时显示三维姿态。正好我之前用国产的N32G430芯片做了一个姿态演示器今天就来详细分享一下整个实现过程。这个项目特别适合参加电子设计竞赛的同学或者刚接触嵌入式开发想做个有趣小项目的朋友。整个系统用到了MPU6050传感器采集数据N32G430单片机处理数据OLED屏幕和上位机显示三维姿态。我会从硬件连接到软件编程一步步带你完成这个项目。1. 系统整体设计思路咱们先来看看整个系统是怎么工作的。简单来说就是MPU6050传感器负责“感受”自己的姿态变化N32G430单片机负责“计算”出具体的姿态角度最后通过OLED屏幕和上位机“显示”出来。整个系统的架构是这样的MPU6050传感器 → N32G430单片机 → OLED显示 上位机显示MPU6050是一个六轴传感器它能同时测量三个方向的加速度和三个方向的角速度也就是陀螺仪数据。N32G430通过I2C接口读取这些原始数据然后利用MPU6050自带的DMP数字运动处理器进行姿态解算得到我们需要的欧拉角俯仰角pitch、横滚角roll和航向角yaw。注意欧拉角是描述物体在三维空间中姿态的一种方式。想象一架飞机俯仰角就是飞机抬头或低头的角度横滚角是飞机左右倾斜的角度航向角是飞机机头指向的方向。2. 硬件选型与连接2.1 核心控制器N32G430C8L7我选用了国民技术的N32G430C8L7这款单片机主要有几个考虑性能足够采用ARM Cortex-M4F内核主频128MHz带浮点运算单元FPU做姿态解算完全够用资源丰富64KB Flash、16KB RAM还有多个UART、I2C、SPI接口国产芯片现在很多项目都开始用国产芯片提前熟悉一下有好处这款芯片的工作电压是2.4V-3.6V温度范围-40°C到105°C稳定性不错。引脚分布图在原始资料里有大家可以参考。2.2 姿态传感器MPU6050MPU6050真的是个“神器”它把三轴加速度计和三轴陀螺仪集成在一个芯片里还自带DMP处理器。这意味着我们不用在单片机上做复杂的姿态解算算法大大降低了开发难度。MPU6050的主要参数陀螺仪量程±250、±500、±1000、±2000°/sec我们一般用±2000加速度计量程±2g、±4g、±8g、±16g我们一般用±2g通信接口I2C最高400kHz工作电压2.5V-3.3V传感器的检测轴方向很重要一定要记清楚X轴左右方向Y轴前后方向Z轴上下方向2.3 显示模块OLED我用的是一块0.96寸的OLED屏幕分辨率128x64通过I2C接口通信。这种屏幕功耗低、显示清晰很适合嵌入式项目。2.4 硬件连接实际的连接很简单主要就是I2C总线的连接模块引脚N32G430引脚说明MPU6050SCLPB13I2C时钟线MPU6050SDAPB14I2C数据线MPU6050AD0PB2地址选择引脚OLEDSCLPA4I2C时钟线OLEDSDAPA5I2C数据线提示MPU6050的AD0引脚决定了它的I2C地址。如果接GND地址是0x68如果接VCC地址是0x69。我这里把AD0接到PB2在软件里把PB2拉低所以地址是0x68。3. MPU6050驱动与DMP使用3.1 I2C接口初始化为了让初学者更好地理解I2C协议我这个项目里MPU6050的驱动用的是GPIO模拟I2C。虽然硬件I2C更稳定但模拟I2C更容易理解底层通信过程。// GPIO模拟I2C的引脚定义 #define MPU6050_SCL_PIN GPIO_PIN_13 #define MPU6050_SCL_PORT GPIOB #define MPU6050_SDA_PIN GPIO_PIN_14 #define MPU6050_SDA_PORT GPIOB // 初始化GPIO为开漏输出模式 void MPU6050_I2C_Init(void) { GPIO_InitTypeDef GPIO_InitStruct; // 使能GPIOB时钟 RCC_EnableAHBPeriphClk(RCC_AHB_PERIPH_GPIOB, ENABLE); // 配置SCL和SDA为开漏输出 GPIO_InitStruct.Pin MPU6050_SCL_PIN | MPU6050_SDA_PIN; GPIO_InitStruct.GPIO_Mode GPIO_Mode_OUT; GPIO_InitStruct.GPIO_OutType GPIO_OutType_OD; // 开漏输出 GPIO_InitStruct.GPIO_Pull GPIO_Pull_Up; // 上拉 GPIO_InitStruct.GPIO_Alternate GPIO_AF_NONE; GPIO_InitStruct.GPIO_Drive GPIO_Drive_High; GPIO_Init(MPU6050_SCL_PORT, GPIO_InitStruct); // 初始状态拉高 GPIO_SetBits(MPU6050_SCL_PORT, MPU6050_SCL_PIN); GPIO_SetBits(MPU6050_SDA_PORT, MPU6050_SDA_PIN); }3.2 MPU6050初始化配置MPU6050的初始化需要按照特定步骤进行每一步都不能少复位MPU6050// 向电源管理寄存器1(0x6B)的bit7写1复位所有寄存器 MPU6050_Write_Byte(MPU6050_PWR_MGMT_1, 0x80); delay_ms(100); // 等待复位完成 // 唤醒MPU6050进入正常工作模式 MPU6050_Write_Byte(MPU6050_PWR_MGMT_1, 0x00);设置传感器量程// 设置陀螺仪量程为±2000°/sec MPU6050_Write_Byte(MPU6050_GYRO_CONFIG, 0x18); // 设置加速度计量程为±2g MPU6050_Write_Byte(MPU6050_ACCEL_CONFIG, 0x00);配置其他参数// 关闭所有中断 MPU6050_Write_Byte(MPU6050_INT_ENABLE, 0x00); // 关闭AUX I2C接口 MPU6050_Write_Byte(MPU6050_USER_CTRL, 0x00); // 设置采样率分频器采样率 1kHz / (1 SMPLRT_DIV) MPU6050_Write_Byte(MPU6050_SMPLRT_DIV, 0x07); // 125Hz采样率 // 设置数字低通滤波器带宽 MPU6050_Write_Byte(MPU6050_CONFIG, 0x06); // 5Hz带宽使能传感器// 使能所有轴加速度计和陀螺仪 MPU6050_Write_Byte(MPU6050_PWR_MGMT_2, 0x00);3.3 DMP初始化与使用DMP是MPU6050的“黑科技”它内部有个小处理器专门做姿态解算我们单片机直接读取结果就行省去了复杂的算法实现。使用DMP需要移植InvenSense公司提供的驱动库主要文件有inv_mpu.c- MPU6050底层驱动inv_mpu_dmp_motion_driver.c- DMP驱动eMPL_outputs.c- 数据输出处理移植时需要实现4个底层函数// I2C写函数 uint8_t i2c_write(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *data); // I2C读函数 uint8_t i2c_read(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *data); // 毫秒延时函数 void delay_ms(uint32_t ms); // 获取毫秒时间戳 uint32_t get_ms(void);DMP初始化函数比较耗时大概需要3-4秒因为里面有传感器自校准过程uint8_t mpu_dmp_init(void) { uint8_t res 0; // 初始化MPU6050 res mpu_init(); if(res) return 1; // 设置传感器 res mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); if(res) return 2; // 配置FIFO res mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); if(res) return 3; // 加载DMP固件 res dmp_load_motion_driver_firmware(); if(res) return 4; // 设置DMP参数 res dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); if(res) return 5; // 启用DMP功能 res dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL); if(res) return 6; // 设置DMP输出速率 res dmp_set_fifo_rate(100); // 100Hz if(res) return 7; // 启动DMP res mpu_set_dmp_state(1); if(res) return 8; return 0; // 初始化成功 }读取姿态数据的函数uint8_t mpu_dmp_get_data(float *pitch, float *roll, float *yaw) { float q0, q1, q2, q3; // 四元数 unsigned char sensor_fifo_count; long quat[4]; // 读取FIFO中的数据 dmp_read_fifo(gyro, accel, quat, sensor_timestamp, sensors, sensor_fifo_count); if(sensors INV_WXYZ_QUAT) { // 将四元数转换为欧拉角 q0 quat[0] / q30; q1 quat[1] / q30; q2 quat[2] / q30; q3 quat[3] / q30; // 计算俯仰角(pitch) *pitch asin(-2 * q1 * q3 2 * q0 * q2) * 57.3; // 计算横滚角(roll) *roll atan2(2 * q2 * q3 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 1) * 57.3; // 计算航向角(yaw) *yaw atan2(2 * q1 * q2 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 1) * 57.3; return 0; } return 1; }注意DMP初始化需要3-4秒时间这段时间传感器在进行自校准。为了提高用户体验建议在OLED上显示校准中...的提示信息。4. OLED显示三维立方体4.1 OLED初始化OLED用的是SSD1306驱动芯片通过I2C通信。为了提高刷新速度我把I2C时钟设置为400kHz并且使用无ACK应答的方式void OLED_Init(void) { // 初始化硬件I2C1 I2C_InitTypeDef I2C_InitStruct; // 使能I2C1时钟 RCC_EnableAPB1PeriphClk(RCC_APB1_PERIPH_I2C1, ENABLE); // 配置I2C参数 I2C_InitStruct.BusMode I2C_BusMode_I2C; I2C_InitStruct.FmMode I2C_FmMode_Fast; I2C_InitStruct.ClockSpeed 400000; // 400kHz I2C_InitStruct.DutyCycle I2C_DutyCycle_2; I2C_InitStruct.OwnAddr 0x00; I2C_InitStruct.AckEnable I2C_Ack_Disable; // 关闭ACK I2C_InitStruct.AddrMode I2C_AddrMode_7bit; I2C_Init(I2C1, I2C_InitStruct); // 使能I2C1 I2C_Enable(I2C1, ENABLE); // OLED初始化序列 OLED_Write_Cmd(0xAE); // 关闭显示 OLED_Write_Cmd(0xD5); // 设置时钟分频因子 OLED_Write_Cmd(0x80); OLED_Write_Cmd(0xA8); // 设置驱动路数 OLED_Write_Cmd(0x3F); OLED_Write_Cmd(0xD3); // 设置显示偏移 OLED_Write_Cmd(0x00); OLED_Write_Cmd(0x40); // 设置显示开始行 // ... 更多初始化命令 OLED_Write_Cmd(0xAF); // 开启显示 }4.2 三维立方体显示算法在二维屏幕上显示三维立方体需要用到坐标变换。基本思路是定义立方体的8个顶点在自身坐标系中的坐标根据当前的姿态角pitch、roll、yaw计算旋转矩阵将立方体顶点乘以旋转矩阵得到世界坐标系中的坐标将三维坐标投影到二维平面去掉Z坐标连接投影后的点绘制立方体// 定义立方体的8个顶点边长20 int16_t cube_vertices[8][3] { {-10, -10, -10}, // 0: 左前下 { 10, -10, -10}, // 1: 右前下 { 10, 10, -10}, // 2: 右后下 {-10, 10, -10}, // 3: 左后下 {-10, -10, 10}, // 4: 左前上 { 10, -10, 10}, // 5: 右前上 { 10, 10, 10}, // 6: 右后上 {-10, 10, 10} // 7: 左后上 }; // 定义立方体的12条边连接哪两个顶点 uint8_t cube_edges[12][2] { {0, 1}, {1, 2}, {2, 3}, {3, 0}, // 底面 {4, 5}, {5, 6}, {6, 7}, {7, 4}, // 顶面 {0, 4}, {1, 5}, {2, 6}, {3, 7} // 侧面 }; // 根据欧拉角计算旋转矩阵 void calculate_rotation_matrix(float pitch, float roll, float yaw, float R[3][3]) { float cp cos(pitch * PI / 180.0); float sp sin(pitch * PI / 180.0); float cr cos(roll * PI / 180.0); float sr sin(roll * PI / 180.0); float cy cos(yaw * PI / 180.0); float sy sin(yaw * PI / 180.0); // 旋转矩阵按Z-Y-X顺序旋转 R[0][0] cy * cr sy * sp * sr; R[0][1] sy * cp; R[0][2] cy * sr - sy * sp * cr; R[1][0] -sy * cr cy * sp * sr; R[1][1] cy * cp; R[1][2] -sy * sr - cy * sp * cr; R[2][0] -cp * sr; R[2][1] sp; R[2][2] cp * cr; } // 绘制立方体 void draw_cube(float pitch, float roll, float yaw) { float R[3][3]; int16_t projected[8][2]; // 投影后的2D坐标 // 计算旋转矩阵 calculate_rotation_matrix(pitch, roll, yaw, R); // 对每个顶点进行旋转和投影 for(int i 0; i 8; i) { float x cube_vertices[i][0]; float y cube_vertices[i][1]; float z cube_vertices[i][2]; // 旋转 float x_rot R[0][0]*x R[0][1]*y R[0][2]*z; float y_rot R[1][0]*x R[1][1]*y R[1][2]*z; float z_rot R[2][0]*x R[2][1]*y R[2][2]*z; // 投影到XOY平面忽略Z坐标 // 加上偏移量让立方体显示在屏幕中央 projected[i][0] (int16_t)(x_rot) 64; // 屏幕X中心 projected[i][1] (int16_t)(y_rot) 32; // 屏幕Y中心 } // 清屏 OLED_Clear(); // 绘制12条边 for(int i 0; i 12; i) { uint8_t v1 cube_edges[i][0]; uint8_t v2 cube_edges[i][1]; // 绘制直线 OLED_DrawLine(projected[v1][0], projected[v1][1], projected[v2][0], projected[v2][1]); } // 刷新显示 OLED_Refresh(); }5. 上位机通信与显示5.1 串口通信协议为了在上位机显示三维姿态我们需要通过串口把数据发送给电脑。我定义了一个简单的通信协议// 定义数据结构 typedef struct { float pitch; // 俯仰角 float roll; // 横滚角 float yaw; // 航向角 uint8_t checksum; // 校验和 } Attitude_Data_t; // 发送姿态数据到上位机 void send_attitude_to_pc(float pitch, float roll, float yaw) { Attitude_Data_t data; uint8_t *p (uint8_t*)data; // 填充数据 data.pitch pitch; data.roll roll; data.yaw yaw; // 计算校验和简单的异或校验 data.checksum 0; for(int i 0; i sizeof(data) - 1; i) { data.checksum ^ p[i]; } // 发送数据帧 USART_SendData(USART1, 0xAA); // 帧头 USART_SendData(USART1, 0x55); for(int i 0; i sizeof(data); i) { USART_SendData(USART1, p[i]); } }5.2 主程序流程整个系统的主程序流程是这样的int main(void) { float pitch, roll, yaw; // 系统时钟初始化 SystemClock_Config(); // 初始化各个外设 USART1_Init(115200); // 串口初始化用于上位机通信 I2C1_Init(); // 硬件I2C初始化用于OLED MPU6050_I2C_Init(); // 模拟I2C初始化用于MPU6050 OLED_Init(); // OLED初始化 // 显示启动界面 OLED_ShowString(0, 0, MPU6050 Demo); OLED_ShowString(0, 16, Initializing...); OLED_Refresh(); // 初始化MPU6050 while(MPU_Init() ! 0) { OLED_ShowString(0, 32, MPU6050 Error!); OLED_Refresh(); delay_ms(500); } // 初始化DMP这个过程需要3-4秒 OLED_ShowString(0, 32, Calibrating...); OLED_Refresh(); while(mpu_dmp_init() ! 0) { OLED_ShowString(0, 48, DMP Init Error!); OLED_Refresh(); delay_ms(500); } // 初始化完成 OLED_Clear(); OLED_ShowString(0, 0, Ready!); OLED_Refresh(); delay_ms(1000); // 主循环 while(1) { // 读取姿态数据 if(mpu_dmp_get_data(pitch, roll, yaw) 0) { // 在OLED上显示角度值 char buf[32]; sprintf(buf, Pitch:%6.1f, pitch); OLED_ShowString(0, 0, buf); sprintf(buf, Roll :%6.1f, roll); OLED_ShowString(0, 16, buf); sprintf(buf, Yaw :%6.1f, yaw); OLED_ShowString(0, 32, buf); // 绘制三维立方体 draw_cube(pitch, roll, yaw); // 发送数据到上位机 send_attitude_to_pc(pitch, roll, yaw); } // 延时10ms约100Hz更新率 delay_ms(10); } }6. 实际调试中的注意事项在实际做这个项目的过程中我踩过几个坑这里分享给大家DMP初始化时间MPU6050的DMP初始化需要3-4秒这段时间传感器在进行自校准。如果一上电就移动模块校准会不准确。一定要等OLED显示Ready后再移动模块。I2C地址问题MPU6050的AD0引脚决定了I2C地址。如果读不到数据首先检查AD0引脚的连接和软件配置。我的代码里是通过PB2控制AD0的// 设置AD0为低电平地址为0x68 GPIO_ResetBits(GPIOB, GPIO_PIN_2);航向角漂移只用MPU6050没有磁力计时yaw角航向角会有明显的漂移这是正常现象。因为陀螺仪积分会产生累积误差。如果需要稳定的航向角可以加上磁力计如HMC5883L做九轴融合。电源稳定性MPU6050对电源比较敏感如果电源有噪声数据会跳动。建议在MPU6050的电源引脚加一个10uF和一个0.1uF的电容。安装方向MPU6050的X、Y、Z轴方向一定要搞清楚。如果安装方向不对显示的姿态就是错的。可以参考数据手册里的方向图。上位机显示我用的上位机是自己用Python写的通过串口接收数据然后用OpenGL显示三维模型。大家也可以用现成的软件比如匿名上位机、Vofa等。这个项目虽然不大但涉及的知识点很全面I2C通信、传感器驱动、姿态解算、三维图形显示、串口通信等。做完这个项目你对嵌入式系统开发会有更深入的理解。最重要的是看到自己做的立方体在屏幕上随着传感器转动而转动那种成就感真的很棒