图7 四元数与Roll-pitch-yaw模型姿态解算
六、航空仪表姿态可视化设计
有了ardupilot姿态数据之后,我们就可以只用COF屏来可视化的展示姿态效果,为了使姿态显示更加直观,这里界面设计参考了航空飞机中常见的G530导航仪仪表面板。仪表面板总体布局分为左侧姿态数据显示区,中右区为姿态主显示区以及下方的操作控制区。
姿态数据显示区通过按钮切换分两组,分别显示轴向加速度X,Y,Z方向的姿态角度,切换按钮后显示另一组数据,各个加速度+X,+Y,+Z.
主显示飞行三维姿态可视化。其中三维姿态显示区为三层叠加:最底层为航向角度盘,角度盘刻度为360度,用于显示饶Z轴转向的航向角,同时上方以数值的方式显示精确的航角度。同时航向角度盘上面叠加一个局部透明遮罩图,达到显示局部旋转表盘的效果。中间层放置横滚以及俯仰刻度盘,横滚盘采用天地方式设计,并在纵向分布仰俯角度刻度。同时在仰俯盘上以中心平面旋转,达到显示横滚指示。最上层放置基本图形显示控件,通过程序来画出飞机基准水平线和横滚刻度。
显示部分下部为偏航指示和目标地点方位指示,这里占时用不上,用一个固定的指示块来指示偏航位置。
最下部分则为操作控制区,操作控制按钮一共有4个,第一个为轴方位姿态数据查看,点此显示传感器的各姿态。第二按钮为转向加速度数据擦好看,第三各按钮为零点标定,即以当前传感器的角度为起始角度,开始显示以此为起始状态的姿态的变化。这个对于使用很重要,因为必须要有一个初始的姿态才能展示动态的变化,同时随时间出现姿态传感漂移误差时,也可以按此按钮进行姿态归零。
图8姿态仪表显示分区设计
图9姿态仪表盘DGUS设计
为了数字字体显示更加专业,这里使用于航空仪表一致的字体来显示数据。同时所有的数字显示,没有直接使用数值组件,为了对其格式化,采用的文本显示组件。在C51代码里,通过一个格式化函数把输入的数据存在另外一个地址,然后通过函数格式化成为需要的格式字符串在写入显示组件的变量地址里去,这样就完美的解决了一个显示位置同时显示数据和文字的需求,同时也很好的对其了格式化显示效果。
图10采用灰度ProFontWindows航空仪表专用字体
七、航空仪表姿态可视化C51软件开发
C51软件开发主要有几部分:1.通过I2C接口完成MPU6050的读写命令操作,从传感器里获得完整正确的数据;2.姿态解算,通过前面的算法,编写响应的函数对原始数据进行解算,获得姿态显示所需要的显示数据;3.数据可视化,将姿态的各个角度数据转换成UI组件的控制数据,比如负数和小数的显示问题。量程的问题等4.为了方便开发调试,还需开发必要调试和跟踪工具代码。
这里面通过数据显示窗口主要显示7个数据,分别为三向的姿态角度,三轴的加速度以及温度。同时根据三项的姿态角度计算可视化的控制数据写入显示组件地址里。
图11姿态可视化C51软件开发开发
COF屏的C51主代码如下,重要部分已经做了注释,很容易看明白流程。
- /**
- * @brief check the MPU6050 is normal or not
- * @retval
- * 0---success
- * 1---failed
- * @note
- * dIFferent MPU6050 chip will use the different addr
- */
- u8 MPU6050_Check(void)
- {
- u16 temp=0;
- LED_Diagnosis(12);
- temp = MPU6050_ReadOneByte(WHO_AM_I);
- Uart2_write_string("CHECK:0x%X",temp);
- delay_ms(10);
- return (temp == 0x68 ? 1 : 0);
- }
- /**
- * @brief reset the MPU6050
- * @retval
- *
- *
- * @note
- * different MPU6050 chip will use the different addr
- */
- void MPU6050_Reset(void)
- {
- MPU6050_WriteOneByte(PWR_MGMT_1, 0x80); //复位
- delay_ms(100);
- }
- /**
- * @brief initation the MPU6050
- * @retval
- *
- *
- * @note
- * different MPU6050 chip will use the different addr
- */
- void MPU6050_Init()
- {
- SetPinOut(3,2);
- SetPinOut(3,3);
- LED_Diagnosis(1);
- MPU6050_WriteOneByte(PWR_MGMT_1, 0x00); //解除休眠状态
- delay_ms(100);
- LED_Diagnosis(2);
- MPU6050_WriteOneByte(SMPLRT_DIV, 0x07);
- LED_Diagnosis(3);
- MPU6050_WriteOneByte(CONFIG, 0x06);
- LED_Diagnosis(4);
- MPU6050_WriteOneByte(GYRO_CONFIG, 0x18); //陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s)
- LED_Diagnosis(5);
- MPU6050_WriteOneByte(ACCEL_CONFIG, 0x01); //加速计自检、测量范围及高通滤波频率,典型值:0x01(不自检,2G,5Hz)
- }
复制代码
此项目的MPU6050的C51读取数据的代码附带在文件附件里,有相同研究需要的朋友可以自取。
八、运行效果图
图12六轴姿态和温度数据显示效果
图13六轴加速度和温度数据显示效果
图13六轴姿态在COF屏上可视化显示效果
九、实验总结
通过此实验已经完美的获取了MPU6050六轴陀螺传感器的数据读取,姿态解算的方法,同时结合COF屏完美的展示了MPU6050姿态的可视化。在实验测试中,COF屏在完成负载的仪表界面显示时,依然完成了I2C数据读取,复杂的浮点计算,以及内存地址操作。在实际测试中读取I2C速度可达8KBps,能够在1ms内读取完整的MPU7项数据,并且在18ms内完成了原始数据的三角函数计算工作,解算出飞航的各个姿态数据,并且更新到显示屏上。
在测试中发现,由于MPU6050灵敏度较高,因此即使在传感器静置不动时,数据会出现抖动和漂移,因而在可视化显示中出现表盘抖动,不稳的现象。虽然通过减低采样频率能够有一定的抑制作用,但是带来仪表显示迟钝的效果。通过阅读相关资料得知需要在采集到数据之后对数据做算法的滤波,常用的有卡尔曼滤波器,这样通过滤波器之后就能够有效的抵消抖动的现象。 研究了一番卡尔曼滤波器之后,感觉要一时移植到C51平台上有些麻烦,然后找了一个简单的线性的滑窗均值过滤方案,即读取最近20个数,取平均值,然后移动滑窗,进行处理,得倒输出的结果,实验结果非常理想,基本达到预想的效果,效果见视频,
图14静止状态读取数据并不完全一致(数据抖动)
图15沿相同路径往复运动后数据并不完全一致(数据漂移)·
六轴陀螺仪姿态可视化效果近景