EtherCAT从站读取MPU9250数据并进行姿态解算

1.EtherCAT从站硬件设计

18年8月的时候,基于LAN9252、stm32、mpu9250芯片设计了一款EtherCAT从站,是我设计的第一款从站。手工焊芯片和元件,焊得我眼睛都快瞎了~~~~~~

第二版采用了双层设计,将stm32层和lan9252层分开,stm32直接可以买现成的核心板用上,因此我只需要手工焊接lan9252电路部分,大大减少了工作了,哈哈、这下爽多了!!!
EtherCAT从站读取MPU9250数据并进行姿态解算_第1张图片
硬件架构如下,stm32和lan9252之间通过fsmc进行数据交互,stm32通过IIC读取mpu9250的数据。
EtherCAT从站读取MPU9250数据并进行姿态解算_第2张图片

2.修改XML文件

CANopen是基于CAN总线的应用层协议,EtherCAT在应用层支持CANopen协议。从站传递给主站的EtherCAT数据帧中包括横滚角Roll、俯仰角Pitch和偏航角Yaw数据,需要对XML文件进行修改。
EtherCAT从站读取MPU9250数据并进行姿态解算_第3张图片

3.姿态解算程序

通过MPU9250采集到的是3轴角速度、3轴加速度和3轴磁感应强度数据。利用得到的9轴原始数据,进行姿态融合解算,就能求出运动物体的姿态角:Roll、Pitch和Yaw。姿态融合算法有很多,比如著名的基于卡尔曼滤波的解算方法,这些算法在无人机等领域已经大量运用了,我们也可以在网上找到一部分开源算法,这些算法我就不详述了。

4.从站测试

本文采用TwinCAT3.0软件对EtherCAT从站进行测试,使用Qt开发的“三维姿态监测软件”与TwinCAT进行ADS通讯。pc机与EtherCAT从站的连接如图所示。
EtherCAT从站读取MPU9250数据并进行姿态解算_第4张图片
在twincat中新建工程,扫描得到三个从站设备。在PLC程序中添加9个变量,分别对应每个从站的横滚角roll、俯仰角pitch和偏航角yaw。变量声明如下:

PROGRAM MAIN
VAR
	pitch AT%I* : INT:=0;
	roll AT%I* : INT:=0;
	yaw AT%I* : INT:=0;
	pitch1 AT%I* : INT:=0;
	roll1 AT%I* : INT:=0;
	yaw1 AT%I* : INT:=0;
	pitch2 AT%I* : INT:=0;
	roll2 AT%I* : INT:=0;
	yaw2 AT%I* : INT:=0;
END_VAR

然后将PLC程序中定义的变量链接到对象字典,接着在twincat中新建ScopeYT工程。Scope模块能通过ADS协议获取到变量,从而将欧拉角变量曲线在图表栏里显示出来,实验中将监测到的姿态数据放大了100倍。
EtherCAT从站读取MPU9250数据并进行姿态解算_第5张图片
使用Qt开发的“三维姿态监测软件”查看姿态角
EtherCAT从站读取MPU9250数据并进行姿态解算_第6张图片

你可能感兴趣的:(ethercat)