【训练营官方案例】基于N32G430的实时姿态显示

专业版【训练营官方案例】基于N32G430的实时姿态显示

1.1w
69
91
96

简介：基于 N32G430 系列 MCU 为主控的实时姿态显示演示案例，如何采集姿态数据并将其转化成模块的实时姿态并显示在 OLED上，亦可以通过串口上报给四轴上位机，在上位机上显示实时姿态。

开源协议: GPL 3.0

创建时间： 2022-07-12 13:20:46

更新时间： 2024-03-18 02:22:45

描述

一、方案介绍

1.1 系统方案结构

本方案设计的架构如下图 1 所示，主要分为以下几个部分，上位机，主控制器 N32G430C8L7， MPU6050 传感器，OLED 显示模块几个部分，旨在使用 MPU6050 传感器采集原始的加速度与陀螺仪加速度，N32G430 接收到数据后换算成模块的姿态，使用 OLED 以及上位机显示出来。

1.2 硬件电路连接设计

具体可以参考原理图设计。

二、主控制器 N32G430 介绍

N32G430C8L7 微控制器产品采用高性能 32 位 ARM Cortex™-M4F 内核，集成浮点运算单元（FPU）和数字信号处理（DSP），支持并行计算指令。最高工作主频 128MHz，集成高达64KB 片内加密存储 Flash，并支持多用户分区权限管理，支持 16KB 的嵌入式 SRAM。内置一个内部高速 AHB 总线，两个低速外设时钟总线 APB 及总线矩阵，支持40个可复用 I/Os，提供丰富的高性能模拟接口，包括 1 个 12 位 4.7Msps ADC，支持16个外部输入通道和3个内部通道，同时提供多种数字通信接口，包括 4个U(S)ART、2个I2C、2个SPI/I2S、1个CAN 2.0B通信接口。N32G430C8L7 微控制器产品可稳定工作于-40°C 至+105°C 的温度范围，供电电压 2.4V至 3.6V，提供多种功耗模式供用。下图是N32G430C8L7微控制器的引脚分布图；

三、传感器介绍

3.1 MPU6050模块

MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了安装空间。MPU6050内部整合了3轴陀螺仪和3轴加速度传感器，并且含有一个第二 IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（ DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的9轴融合演算数据。有了DMP，我们可以使用 InvenSense 公司提供的运动处理资料库，非常方便的实现姿态解算，降低了处理运算对操作系统的负荷，同时大大降低了开发难度。

MPU6050 的特点包括

以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）
具有 131 LSBs/° /sec 敏感度与全格感测范围为± 250、± 500、± 1000 与±2000°/sec 的 3 轴角速度感测器(陀螺仪)、
集成可程序控制，范围为± 2g、± 4g、± 8g 和± 16g 的 3 轴加速度传感器、
移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移；
自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算数据、感测器同步化、姿势感应等的负荷；
内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求；
自带一个数字温度传感器；
带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS；
可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、 high-G 中断、零动作感应、触击感应、摇动感应功能；
VDD 供电电压为 2.5V± 5%、 3.0V± 5%、 3.3V± 5%； VLOGIC 可低至 1.8V± 5%；
陀螺仪工作电流： 5mA，陀螺仪待机电流： 5uA；加速器工作电流： 500uA，加速器省电模式电流：40uA@10Hz；
自带 1024 字节 FIFO，有助于降低系统功耗；
高达 400Khz 的 IIC 通信接口；
超小封装尺寸： 4x4x0.9mm（ QFN）

MPU6050传感器的检测轴及其方向如下图1所示：

1.MPU6050 硬件连接

如上图所示，这里我们的MCU通过这个I2C接口（IIC_SCL/IIC_SDA连接MCU的 PB13/PB14）来控制 MPU6050,AD0是从I2C接口（接 MCU）的地址控制引脚，该引脚控制IIC 地址的最低位。如果接 GND，则 MPU6050 的 IIC 地址是：0x68，如果接VDD，则是0x69，本项目中，该引脚直连到MCU的PB2，软件是直接拉低的，故MPU6050的I2C地址为 0x68。

说明： 为了利于I2C协议的学习，本例程的MPU6050的驱动代码采用的时GPIO模拟的I2C接口。

注意： 这里的地址是不包含数据传输的最低位的（最低位用来表示读写）！

2.MPU6050 初始化配置流程

1. 初始化 IIC 接口:

初始化与 MPU6050 连接的SDA和SCL数据线，设置AD0为低电平，进而选择I2C地址为0x68。

2. 复位 MPU6050：

这一步让 MPU6050内部所有寄存器恢复默认值，通过对电源管理寄存器1（0X6B）的bit7写1 实现。复位后，电源管理寄存器1恢复默认值(0X40)，然后必须设置该寄存器为0X00，以唤醒MPU6050，进入正常工作状态。

3. 设置角速度传感器（陀螺仪）和加速度传感器的满量程范围：

这一步，我们设置两个传感器的满量程范围(FSR)，分别通过陀螺仪配置寄存器（0X1B），和加速度传感器配置寄存器（0X1C）设置。我们一般设置陀螺仪的满量程范围为± 2000dps，加速度传感器的满量程范围为±2g。

4. 设置其他参数:

这里，我们还需要配置的参数有：关闭中断、关闭 AUX IIC接口、禁止FIFO、设置陀螺仪采样率和设置数字低通滤波器（DLPF）等。本章我们不用中断方式读取数据，所以关闭中断，然后也没用到AUXIIC接口外接其他传感器，所以也关闭这个接口。分别通过中断使能寄存器（0X38）和用户控制寄存器（0X6A）控制。MPU6050可以使用 FIFO 存储传感器数据，不过本章我们没有用到，所以关闭所有FIFO 通道，这个通过 FIFO使能寄存器（0X23）控制，默认都是0（即禁止FIFO），所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器（0X19）控制，这个采样率我们一般设置为 50 即可。数字低通滤波器(DLPF)则通过配置寄存器（0X1A）设置，一般设置 DLPF 为带宽的 1/2 即可；

5. 配置系统时钟源并使能角速度传感器和加速度传感器:

系统时钟源同样是通过电源管理寄存器1（0X1B）来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是0（内部8MRC震荡）,不过我们一般设置为 1，选择x轴陀螺 PLL作为时钟源，以获得更高精度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器 2（0X6C）来设置，设置对应位为0可开启；

3.MPU6050 重要寄存器说明

该部分可以参考《MPU-6000 & MPU-6050 寄存器表及其描述(中文版)》文档

4.DMP 使用

从MPU6050相关寄存器中，我们可以读出加速度传感器和角速度传感器的原始数据。不过这些原始数据，对想搞四轴之类的初学者来说，用处不大，我们期望得到的是姿态数据，也就是欧拉角：航向角（ yaw）、横滚角（ roll）和俯仰角（ pitch）。有了这三个角，我们就可以得到当前四轴的姿态，这才是我们想要的结果。要得到欧拉角数据，就得利用我们的原始数据，进行姿态融合解算，这个比较复杂，知识点比较多，初学者不易掌握。而 MPU6050 自带了数字运动处理器，即 DMP，并且，InvenSense 提供了一个MPU6050 的嵌入式运动驱动库，结合 MPU6050 的 DMP，可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到yaw、roll和 pitch。使用内置的 DMP，大大简化了四轴的代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。

官方 DMP 驱动库移植起来，还是比较简单的，首先将inv_mpu.c 和inv_mpu_dmp_motion_driver.c两个文件加入到工程中，其次实现这 4 个函数：i2c_write，i2c_read， delay_ms 和 get_ms，具体细节，我们就不详细介绍了，移植后的驱动代码，我们放在本例程src eMPL 文件夹内，总共 6 个文件，如图下图所示：

其中我们在 inv_mpu.c 添加了几个函数，方便我们使用，重点是两个函数：mpu_dmp_init 和mpu_dmp_get_data 这两个函数，这里我们简单介绍下这两个函数。u8 mpu_dmp_init(void)；//mpu初始化，注意：这个函数比较耗时，大概3-4s，无实测。u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)；获取姿态数据pitch，roll，yaw。该函数的具体实现可以参考源码注释。

3.2 OLED 模块

1.OLED 模块硬件连接

如上图7所示，这里我们的MCU通过IIC接口（OLED_SCL/ OLED_SDA连接MCU的 PA4/PA5）来控制OLED的显示。为了尽快熟悉N32G430这颗芯片，OLED的I2C驱动，本案例采用的是硬件I2C1。如上图6所示，该OLED模块背面的地址选择电阻跳到了左边，那么它的从机地址为0x78。注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写）！

2.OLED 初始化配置流程

1. 初始化 IIC 接口

初始化与 OLED 连接的SDA和SCL数据线。

2. 关闭OLED显示，设置OLED寄存器参数

关闭OLED显示之后，配置寄存器的参数，例如设置时钟分频因子，震荡频率，设置显示对比度，扫描方向，预充电周期，亮度等参数，具体含义可以参考OLED的寄存器说明文档，或者网上其他资料，这里就不做具体的阐述。

3. 开启OLED显示

4.3 OLED 重要寄存器说明

该部分可以参考一下网址：https://wenku.baidu.com/view/fb23e3a4bfeb19e8b8f67c1cfad6195f302be849.html；

四、空间坐标转换算法

4.1 三维坐标系转换

空间中三维坐标变换一般由以下三种方式实现：

旋转矩阵和旋转向量；
欧拉角；
四元数。
这里我们介绍旋转矩阵(旋转向量)与欧拉角实现三维空间坐标变换的方法以及两者之间的关系。这里以常见的相机坐标系转换到世界坐标系，也就是比较通用的 body 到世界坐标系间的转换为例。那么旋转的欧拉角按从世界坐标系转换到相机坐标系的过程，先按z轴旋转、之后y轴旋转、之后x轴旋转，最终得到相机坐标系，得到的角度分别是 yaw、pitch、roll，那么从相机坐标系到世界坐标系的旋转矩阵按如下方式定义。

以上的矩阵算法，可以参考百度相关的资料，由于代码中由于立方体的 x，y 轴与传感器的x，y轴方向不一致，代码将Rx，Ry做了交换，以保证 OLED 的立方体的方向与传感器的方向一致，从而将姿态调整一致。由于要将立体的坐标点显示在二维平面上，故假如点 P（x0，y0，z0），需要将 P 点 XOY平面上投影，即 z0 = 0，即在平面上的 P1 坐标为（x0，y0）。基于以上的分析，则如果要显示立方体在平面上，那么只要将转换后的立方体八个坐标点，投影到 XOY 平面上，然后依次连接以上点，即可以得到一个立方体。本例程为了突出显示立方体姿态变化，故多连接了一个x型面。

五、软件设计流程

5.1 软件流程图

软件的初始化流程分为以下几个步骤：

初始化：
部分包括串口，硬件 IIC，GPIO 模拟 I2C，OLED，MPU6050 以及 DMP 初始化; -log_init()：500000Bd, no parity, 8-bit data,1 stop bit。该接口可以收发数据以及 log 打印；-i2c_master_init ()：为了提高 OLED 的刷新速度, IIC 速度设置为 400k,无 ACK 应答的方式；-oled_init()：主要是 OLED 的寄存器参数配置，具体可以参考 SSD1306 控制器；-MPU_Init()：配置 MPU6050 的寄存器参数配置，包括地址，采样率，加速度/陀螺仪满量程范围等参数的配置。-mpu_dmp_init()：DMP 初始化，这里等待 DMP 的初始化完成.

六、实物演示

6.1 上电

上电复位之后，水平静止的模块姿态。

静止时上位机显示画面。

6.2向右倾斜45度的模块姿态

向右倾斜 45 度的 OLED 显示画面。

向右倾斜 45 度的上位机显示画面。

6.3向左倾斜35度，向下倾斜21度的模块姿态

向左倾斜 35 度，向下倾斜 21 度的 OLED 显示画面。

向左倾斜 35 度，向下倾斜 21 度的上位机显示画面

注意事项

复位后，一般要等3-4s，OLED上才正常显示。这个主要是因为在DMP初始化过程中，有MPU6050的自校准过程，因此为了用户的使用体验，可以考虑在OLED上显示“校准中”以做提示；
复位之后必须将模块静止平放，等完成传感器校准后再移动模块；
用户可以通过修改Set_Slaver_Address()函数配置PB2拉高拉低，实现MPU6050地址的修改。代码中是拉低PB2,那么MPU6050的地址为0x68。
欧拉角Yaw放置一段时间，有明显漂移，这个是因为没有加入磁力传感器做算法融合导致的，属于正常现象。有兴趣的可以研究如何解决漂移问题。