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

1.EtherCAT从站硬件设计

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

第二版采用了双层设计，将stm32层和lan9252层分开，stm32直接可以买现成的核心板用上，因此我只需要手工焊接lan9252电路部分，大大减少了工作了，哈哈、这下爽多了！！！

硬件架构如下，stm32和lan9252之间通过fsmc进行数据交互，stm32通过IIC读取mpu9250的数据。

2.修改XML文件

CANopen是基于CAN总线的应用层协议，EtherCAT在应用层支持CANopen协议。从站传递给主站的EtherCAT数据帧中包括横滚角Roll、俯仰角Pitch和偏航角Yaw数据，需要对XML文件进行修改。

3.姿态解算程序

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

4.从站测试

本文采用TwinCAT3.0软件对EtherCAT从站进行测试，使用Qt开发的“三维姿态监测软件”与TwinCAT进行ADS通讯。pc机与EtherCAT从站的连接如图所示。

在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倍。

使用Qt开发的“三维姿态监测软件”查看姿态角