【开源】使用MPU6050与COF屏实现航空姿态仪表
此次分享一个使用MPU6050陀螺仪与COF智能屏实现航空姿态仪表的案例。这个案例展示了COF屏的IIC接口的使用和通过I2C采集MPU6060数据和处理数据的方法，以及最后将这些实时数据在COF屏设计航空仪表界面上显示，使用COF屏的基本图形组件将姿态数据在屏幕上以仪表图形的方式展示出来。
一、 MPU6050传感器介绍
MPU-6050是一款应用非常广泛的陀螺仪，具有六轴姿态传感能力，它的角速度全格感测范围为±250、±500、±1000与±2000°/sec (dps)，可准确追踪快速与慢速动作，并且可程序控制的加速器全格感测范围为±2g、±4g±8g与±16g。数据传输可透过最高至400kHz的IIC（MPU-6000型号可以最高达20MHz的SPI）。MPU-6050可在多种电压下工作，VDD供电电压为2.5V±5%、3.0V±5%或3.3V±5%，逻辑接口VDDIO供电为1.8V±5%。MPU-6050的芯片尺寸4x4x0.9mm(QFN)，非常小的尺寸。其他的特征包含内建的温度感测器、包含在运作环境中仅有±1%变动的振荡器。此次使用MPU6050传感器模块，模块尺寸也很小巧，只有13mmx20mm大小，模块上已经焊接好MPU6050芯片以及必要的外围电路。
二、硬件连接设计
COF屏硬件与MPU6050连接示意图如图2所示，通过COF屏的GPIO口模拟I2C信号与MPU6050通讯。具体硬件连接时通过COF屏的转接板HDL662S使用P32连接MPU6050模块的SDL脚，P33脚连接MPU6050的SDA脚，电源和地分别接转接板的VCC和GND。此次只需要连接这4根线即可。另外再把转接板的Uart2连接到一个上位机，把P21~P26六个IO接入上位机6个LED灯上，方便输出调试信息和通过LED显示运行状态以及向COF屏输出命令。连接完成的整体硬件实物如图3所示。
三、MPU6050数据含义
MPU6050 内部集成了DMP。DMP是数字运动处理器的缩写，顾名思义 mpu6050 并不单单是一款传感器，其内部还包含了可以独立完成姿态解算算法的处理单元。如在设计中使用 DMP 来实现传感器融合算法优势很明显。首先invensense 官方提供的姿态解算算法应该比自己去写算法可靠的多。其次由 DMP 实现姿态解算算法将单片机从算法处理的压力中解放出来，单片机所要做的是等待DMP解算完成后产生的外部中断，在外部中断里去读取姿态解算的结果。这样可以直接运算出四元数和姿态，而不再需要另外进行数学运算。DMP 的使用大大简化了四轴的代码设计。
通过DMP的解算主控机有大量的时间来处理诸如其它出具处理任务，比如数值显示，三维显示和电机调速等其他任务，从而提高了系统的实时性。这样主控有大量的时间来处理诸如电机调速等其他任务，提高了系统的实时性。MPU6050姿态表述如图2所示。
三个轴向加速度分量均以重力加速度g的倍数为单位，能够表示的加速度范围，即倍率可以统一设定，有4个可选倍率：2g、4g、8g、16g。以ACC_X为例，若倍率设定为默认值2g，则意味着ACC_X取最小值-32768时，当前加速度为沿X轴正方向2倍的重力加速度；若设定为2g，取-32768时表示沿X轴正方向2倍的重力加速度，以此类推。显然倍率越低精度越好，倍率越高表示的范围越大，这里根据具体的应用来设定。
以ACCR_X为例，若当前设定的加速度倍率值为0x1，即为2g，那么将ACCR_X读数换算为加速度的公式如下：（g为重力加速度常量，ACC_X为X轴的读数），同样另外两个方向Y，Z的加速度也可以同样方法计算。
三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X为例，若倍率设定为250度/秒，则意味着GYR取正最大值32768时，当前角速度为顺时针250度/秒；倍率越低精度越好，倍率越高表示的范围越大。
以GYR_X为例，若当前设定的角速度倍率值为0x18，即为2000度/秒，那么将GRY_X读数换算为角速度（顺时针）的公式如下，同样另外两个Y,Z轴的角加速度也可以同样方法计算。
另外MPU6050还提供一个温度传感的输出，温度的值（单位摄氏度）由以下公式计算得到，TempReg 为从MPU6050温度寄存器读出的原始数值：
四、通过IIC读取MPU6050数据
COF屏提供的用户可使用的GPIO，可以直接使用GPIO模拟I2C信号来与MPU600通讯。在COF屏所带的C51 V6版本的例程里，带有一个I2C RTC硬件的驱动代码，因此我们可以基于这个代码来修改，得到MPU6050使用的通讯函数。
从原代码中主要移植的代码函数有这几个：
- <font size="3" face="宋体">//****************************************
- // I2C 基础操作函数
- //****************************************
- void IIC_Start(void); //开始一个I2C操作时序
- void IIC_Stop(void); //结束一个I2C操作时序
- void IIC_Wait_Ack(void); //等待I2C设备响应
- void IIC_Send_Ack(bit k); //发送一个响应给I2C设备,有两种 0:ACK , 1:NACK
- void IIC_Send_Byte(u8 txd); //发送一个字节到I2C设备
- u8 IIC_Read_Byte(void); //从I2C 设备读取一个字节</font>
有了这些函数，就可以根据MPU6050的规格书，编写正对MPU6050的各个操作命令，好在网上已经很多MPU6050的操作代码，在参考之后，很快完成MPU6050的操作函数，如下：
- <font size="3" face="宋体">//****************************************
- // MPU6050 操作函数
- //****************************************
- void MPU6050_Init();
- u8 MPU6050_Check(void);
- void MPU6050_Reset(void);
- u8 MPU6050_ReadOneByte(u16 addr)；
- void MPU6050_WriteOneByte(u16 addr, u8 dat)；
- u16 MPU6050_GetData(u8 REG_Address);</font>
在结合MPU6050的寄存器定义，按相应的规则，做好设置，即可成功的读取到数据。MPU主要用到的寄存器如下所示：
- <font size="3" face="宋体">//****************************************
- // 定义MPU6050内部地址
- //****************************************
- #define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
- #define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz)
- #define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
- #define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
- #define ACCEL_XOUT_H 0x3B
- #define ACCEL_XOUT_L 0x3C
- #define ACCEL_YOUT_H 0x3D
- #define ACCEL_YOUT_L 0x3E
- #define ACCEL_ZOUT_H 0x3F
- #define ACCEL_ZOUT_L 0x40
- #define TEMP_OUT_H 0x41
- #define TEMP_OUT_L 0x42
- #define GYRO_XOUT_H 0x43
- #define GYRO_XOUT_L 0x44
- #define GYRO_YOUT_H 0x45
- #define GYRO_YOUT_L 0x46
- #define GYRO_ZOUT_H 0x47
- #define GYRO_ZOUT_L 0x48
- #define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
- #define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
- #define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
- #define SlaveAddressR 0xD1 //IIC读取时的地址字节数据</font>
对应的硬件GPIO连接定义端口代码如下：
- <font size="3" face="宋体">sbit IIC_SCL=P3^2;
- sbit IIC_SDA=P3^3;
- #define SDA_IN() P3MDOUT=P3MDOUT&0xF7;
- #define SDA_OUT() P3MDOUT=P3MDOUT|0x08;</font>
将各个部分代码整合编译调试上载到COF屏后，能够成功的读取到了MPU6050的各个寄存器数值。在调试中，需要注意：
1.MPU6050的I2C主频与原RTC设备的主频有差别，因此适当的调整了I2C函数操作中的时序,实之适配。
2.T5L处理器速度快，在两次读取间要做一定的等待，不然出现读到数据全是零，经过反复的测试，确定可以在20ms间隔来读取传感器数据，这个对于本实验已完全适用。
3.I2C，SDA线接触一定要良好，稍有接触不良就会造成通讯中断，必须重新初始化才能得到正确的数据。
五、姿态数据解算
得到原始数据后，并不能直接使用，而是需要进一步按数据的含义进行解算，通过解算得到我们需要的设备的姿态。通过算法解算处理后，才能得到可以供几何显示和姿态判断使用的数据。姿态解算简单的说就是通过三轴的重力加速度分量得到以地球切面为参照系的三轴夹角转换为以设备自身坐标下的飞行夹角。这里定义如下：横滚角Roll(Ф)表示绕X轴的旋转，杨俯角Pitch(θ)表示绕Y轴的旋转，航向角Yaw (ψ)表示绕Z轴的旋转，如图6示意。
姿态解算使用了传感器一个重要的特性，就是当MPU6050 Z轴向上放置的时候，Z方向的加速度就是当地的地心引力，即 9.8 m/s2 左右。这个地球的重力加速度就是开姿态解算的依据，如图6中的G。
图6 飞航设备的Roll-pitch-yaw模型姿态定义图示
为方便表示以三维的笛卡尔右手系坐标为参照坐标系，图7中蓝色箭头就是 MPU6050 Z轴向上放置时测出的重力加速度，左侧的Ф就是要表示的横滚角 roll，而重力加速度在 y 轴和 z 轴上的分量 Ay 和 Az，显然得到：
tan(Ф) = Ay / Az，即：Ф = arctan(Ay / Az)
在飞控习惯的ardupilot定义下，姿态横滚角（roll）指：重力加速度与“X-Z平面”的夹角。应用勾股定理推广到一般的情况是：重力加速度在 X-Z 平面的投影长度 = sqrt(Ax^2 + Az^2)。因此够得到Ф在三维空间内的通用公式，用同样的方法可以推出俯仰角θ和航向角ψ的公式。
Ф = arctan(Ay / sqrt(Ax^2 + Az^2))
通过以上算法公式可以得到从四元数到各姿态角的计算代码算法如下：
roll = atan2(2* Gy*Gz + 2*G*Gx, -2*Gx*Gx -2*Gy*Gy+ 1)
pitch = asin(-2*Gx*Gz + 2*G* Gy)
yaw = atan2(2*( Gx*Gy +G*Gz),G*G+Gx*Gx-Gy*Gy-Gz*Gz)
图7 四元数与Roll-pitch-yaw模型姿态解算
六、航空仪表姿态可视化设计
有了ardupilot姿态数据之后，我们就可以只用COF屏来展示姿态效果，为了使姿态显示更加直观，这里界面设计参考了航空飞机中常见的G530导航仪仪表面板。仪表面板总体布局分为左侧姿态数据显示区，中右区为姿态主显示区以及下方的操作控制区。
姿态数据显示区通过按钮切换分两组，分别显示轴向加速度X,Y,Z方向的姿态角度，切换按钮后显示另一组数据，各个加速度+X，+Y，+Z，主显示飞行三维姿态可视化。其中三维姿态显示区为三层叠加：最底层为航向角度盘，角度盘刻度为360度，用于显示饶Z轴转向的航向角，同时上方以数值的方式显示精确的航角度。同时航向角度盘上面叠加一个局部透明遮罩图，达到显示局部旋转表盘的效果。中间层放置横滚以及俯仰刻度盘，横滚盘采用天地方式设计，并在纵向分布仰俯角度刻度。同时在仰俯盘上以中心平面旋转，达到显示横滚指示。最上层放置基本图形显示控件，通过程序来画出飞机基准水平线和横滚刻度。
显示部分下部为偏航指示和目标地点方位指示，这里暂时用不上，用一个固定的指示块来指示偏航位置。
最下部分则为操作控制区，操作控制按钮一共有4个，第一个为轴方位姿态数据查看，点此显示传感器的各姿态。第二按钮为转向加速度数据擦好看，第三各按钮为零点标定，即以当前传感器的角度为起始角度，开始显示以此为起始状态的姿态的变化。这个对于使用很重要，因为必须要有一个初始的姿态才能展示动态的变化，同时随时间出现姿态传感漂移误差时，也可以按此按钮进行姿态归零。
为了数字字体显示更加专业，这里使用于航空仪表一致的字体来显示数据。同时所有的数字显示，没有直接使用数值组件，为了对其格式化，采用的文本显示组件。在C51代码里，通过一个格式化函数把输入的数据存在另外一个地址，然后通过函数格式化成为需要的格式字符串在写入显示组件的变量地址里去，这样就完美的解决了一个显示位置同时显示数据和文字的需求，同时也很好的对其了格式化显示效果。
图10 采用灰度ProFontWindows航空仪表专用字体
七、航空仪表姿态可视化C51软件开发
C51软件开发主要有几部分：
1.通过I2C接口完成MPU6050的读写命令操作，从传感器里获得完整正确的数据；
2.姿态解算，通过前面的算法，编写响应的函数对原始数据进行解算，获得姿态显示所需要的显示数据；
3.数据可视化，将姿态的各个角度数据转换成UI组件的控制数据，比如负数和小数的显示、量程的问题等；
4.为了方便开发调试，还需开发必要调试和跟踪工具代码。
这里面通过数据显示窗口主要显示7个数据，分别为三向的姿态角度，三轴的加速度以及温度。同时根据三项的姿态角度计算可视化的控制数据写入显示组件地址里：
COF屏的C51主代码如下，重要部分已经做了注释，很容易看明白流程。
此项目使用 T5L的C51操作I2C读取MPU6050的数据的代码附带在文件附件里，有相同研究需要的朋友可以自取。
八、运行效果图
九、实验总结
通过此实验已经完美的获取了MPU6050六轴陀螺传感器的数据读取，姿态解算的方法，同时结合COF屏完美的展示了MPU6050姿态的可视化。在实验测试中，COF屏在完成负载的仪表界面显示时，依然完成了I2C数据读取，复杂的浮点计算，以及内存地址操作。在实际测试中读取I2C速度可达8KBps，能够在1ms内读取完整的MPU7项数据，并且在18ms内完成了原始数据的三角函数计算工作，解算出飞航的各个姿态数据，并且更新到显示屏上。
在测试中发现，由于MPU6050灵敏度较高，因此即使在传感器静置不动时，数据会出现抖动和漂移，因而在可视化显示中出现表盘抖动，不稳的现象。虽然通过减低采样频率能够有一定的抑制作用，但是带来仪表显示迟钝的效果。通过阅读相关资料得知需要在采集到数据之后对数据做算法的滤波，常用的有卡尔曼滤波器，这样通过滤波器之后就能够有效的抵消抖动的现象。
|Archiver|手机版|小黑屋|迪文科技论坛
( 京ICP备05033781号-1 )
发表于 2024-1-10 16:52:08
发表于 2024-5-6 16:26:22