ubuntu22.04下Ethercat IGH DC同步实现
一.系统环境说明
- 主站系统：
ubuntu22.04 - Igh版本：gitlab上的最新版本1.6.0，https://gitlab.com/etherlab.org/ethercat
- 系统的实时内核：
preempt-rt
该内核安装非常方便，
- 注册https://ubuntu.com/账号
- 在个人账号的订阅下可以看到Token, 可以免费安装5台设备
https://ubuntu.com/pro/dashboard -
ua attach <免费的TOEKN> - 确保你在使用至少 27.8 版本的 ubuntu-advantage-tools 软件包，可通过此命令查看当前版本：
ua version
要在 Ubuntu 22.04 (Jammy Jellyfish) 中升级 ubuntu-advantage-tools 到 27.8，请运行命令：
sudo apt install ubuntu-advantage-tools=27.8~22.04.1
要启用测试版实时内核，可运行：
ua enable realtime-kernel --beta
需要验证实时性能，可以使用cyclitest,主要看max的值，经过14个小时时间，其最大值的延迟在230us左右，满足8ms为周期的控制
# 结果
sudo cyclictest -p 99 -S -i 8000 -m
[sudo] lm 的密码：
/dev/cpu_dma_latency set to 0us
policy: fifo: loadavg: 10.26 9.47 9.38 7/1308 105547
T: 0 (91316) P:99 I:8000 C:6638053 Min: 2 Act: 73 Avg: 30 Max: 193
T: 1 (91317) P:99 I:8500 C:6247579 Min: 2 Act: 72 Avg: 27 Max: 166
T: 2 (91318) P:99 I:9000 C:5900491 Min: 3 Act: 74 Avg: 29 Max: 230
T: 3 (91319) P:99 I:9500 C:5589938 Min: 3 Act: 6 Avg: 28 Max: 155
# 压力：4核满负荷
lm@lm-X550JK:~$ stress-ng -c 4 --cpu-method fft --timerfd-freq 1000000 -t 24h
stress-ng: info: [92633] setting to a 86400 second (1 day, 0.00 secs) run per stressor
stress-ng: info: [92633] dispatching hogs: 4 cpu
^Cstress-ng: info: [92633] successful run completed in 52186.24s (14 hours, 29 mins, 46.24 secs)
- 控制对象：
7个支持DC模式的电机 - 参考代码：
- 官方例程：dc_user，该例程以主站为参考时钟；
- 官方例程：rtai_rtdm_dc，该例程以从站为参考时钟，但其内核环境是RTAI
二.igh使用配置
ecrt_request_master(0);
ecrt_master_create_domain(master);
sc[i] = ecrt_master_slave_config(master, a[i], p[i], VID_PID)
ecrt_slave_config_pdos(sc[i], EC_END, device_syncs)
ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)
ecrt_slave_config_sdo16(sc[i], 0x1c32, 1, 2);
ecrt_slave_config_sdo16(sc[i], 0x1c33, 1, 2);
ecrt_slave_config_dc(sc[i], 0x0300, cycle_ns, cycle_ns/4, 0, 0);//8ms
ecrt_master_activate(master)
ecrt_domain_data(domain1)
三.ethercat DC同步
3.1 dc同步原理
用EtherCAT的分布式时钟(DC)功能使从站设备同步指的是，总线中的第一个DC从站被定义为基准时钟，EtherCAT主站将基准时钟的时间分配至所有的从站。因此，EtherCAT主站周期性发送一个ARMW命令，以此读取存储在时钟主站的ESC（EtherCAT从站控制器）上适当的寄存器中的总线时间，并将这个值写入DC-从站相应的寄存器中。这些DC从站通过整合在ESC中的一个控制器来更新他们的本地时间。为保证请求的精度（可以接收低于1us的值），特殊从站之间的EtherCAT帧延迟必须得到额外的补偿。 对于每个从站来说，一个帧从发送到接受的这段时间将被测量。然后，根据总线拓扑结构，主站计算从站之间的延迟，并将相应延迟补偿值写入到ESC中的寄存器0x928里。
ESC控制器的DC单元提供两个数字输出信号，SYNC0和SYNC1。频率一般对应于EtherCAT总线时钟的SYNC脉冲通常都是基于总线时间生成的。举个例子：如果EtherCAT主站用1ms发送周期性的I/O数据，SYNC脉冲频率通常也设置为1kHz。这些SYNC信号在从站一侧，一方面是可作为一个数字输出信号（例如要激活从站硬件组件），另一方面作为从站软件中断源。也就是说，很明显的要在SYNC脉冲被释放之前为所有从站提供新的信号。要实现这一目标，新的周期性I/O数据到达和SYNC脉冲之间的相隔时间的必须保证最小化。
通常情况下，EtherCAT主站协议栈通过在其控制硬件（例如嵌入式x86PC中的8254计时器）中的硬件计时器来发送循环输出数据。如果系统运行周期为1kHz，那么8254计时器以及负责生成同步脉冲的从站计时器都应设置为1kHz。但8254计时器和从站计时器都不能准确的运行1kHz的周期率。实际上，这两个计时器之间有些偏差，因此，在主站里发送周期性I/O数据和从站中的一系列SYNC脉冲之间可以实现非恒定间隔。为了控制一个定义好的常量值的间隔，EtherCAT主站必须要与DC时钟主站同步（就是主站上的第一个DC从站）。这一机制被称为分布式时钟主站同步（DCM）。
它可以以两种不同的方式实现：
重新调整用来执行EtherCAT主站的硬件中所使用的物理计时器（例如8254定时器）（主站转换）；重新调整DC时钟主站的总线时间时间（总线转换）。
DCM控制器周期性计算EtherCAT主站时间和DC时钟主站时间之间的差异。根据设定值（即SYNC脉冲到主站中计时器中断的距离），PI控制器算法计算重新调整的值。当使用"主站转换"时，重新调整值会影响EtherCAT主站的物理计时器（例如8254)；而当使用的"总线转换"时，它则会影响DC时钟主站的寄存器0x920。
EtherCAT主站ClassA支持这两种方法（主站转换和总线转换)。
主站与从站的同步是EtherCAT最具挑战性的特点之一。
3.2 同步模式
3.2.1 free run
从站的过程数据处理，由内部事件触发，与主站循环无关
1、每个从站的定时周期都不一样
2、每个定时周期到的时候，每个从站去执行自己的程序，<比如检查通道上有没有合适的新的输入数据，有的话就令其输出有效(Output valid)或者检查有没有需要输入的数据，将其放到对应的同步管理器通道上(input prepare)让主站取走>
3、对于Free Run 模式而言，好比每个人都有自己的手表，但是如果没有一起对时的话。手表的本身的时间是不一样的(可能表和表之间存在时间差，一个8点一个10点)，那么在这种情况下，公司如果要求9点上班，那么每个人到公司的时间都是自己手表上的9点，但是不是统一的9点，他们到达公司的时间是不一样，所以Free Run模式没有任何的同步性可言
3.2.2 SM-Synchronous
从站的过程数据处理，由接收到携带过程数据的周期性数据帧时所产生的硬件中断触发
1、一般而言，如果EtherCAT总线通讯时的同步模式不是DC模式，那么就是SM同步模式了，
2、SM(Sync Manager同步管理器)指的是同步管理器的同步，它的触发方式是通过SM Event，也就是我们的数据帧在到达对应的从站的时候，会触发一个叫做同步管理器事件的信号(即SM Event)，当从站接受到这个信号的时候，会进入到对应的中断服务例程(即中断保存当期任务，去执行插入的中断例程，这里指线程中断处理相关数据，这也是SM Event和 Free Run的不同之处)，即SM是通过中断服务例程来处理对应的数据。
3、从站检测到SM Event事件信号的时候会进入到中断服务例程去处理相应的数据(比如把输出数据有效，然后把输入数据放到同步管理器的通道上让主站取走)
4、由于SM同步模式是根据数据帧到达特定从站的时候来触发SM Event事件信号来进行同步，那么对于一个特地给的帧来说，它到达每一个从站的时间必然是不同的，当系统很庞大的时候，每个从站接收到数据帧的时间就会相差很大，越在后边的从站接收到数据帧的时间就越晚，它的同步效果就越差。
5、手表举例：10个人10个表，10个人开始对表，从0开始计时，当上一个人完成了对表计时之后，才通知到下一个人进行对表计时，依次传递，那么到最后的第10个人它开始计时的时间是最晚的。完成对时之后，如果公司要求是10点上班，每个人按照自己手表上的10点到达，那么第10个人一定是最晚的。
3.2.3 DC-Synchronous 简单
从站的过程数据处理，由基于分布时钟和系统时间的硬件中断触发
1、Sync0 Event和SM Event事件信号是类似的，他们都是一个中断事件信号，对于一个从站而言，如果中断信号触发了而且中断屏蔽寄存器没有屏蔽掉该中断信号，那么从站就会进入到中断服务例程
2、中断服务例程就是说从站从主循环中跳出，暂停并保留主程序状态，然后执行中断服务例程这一部分内容，比如说周期性数据的输出,周期性数据的实时性比较强，那么中断信号可以满足这种比较强的实时性需求，不会因为其他信号来耽误它的操作，其它的中断信号会进入等待状态直到前一个中断信号被恢复，即进入到恢复现场阶段.
3、不同于SM Event的地方是Sync0 Event是根据我们自己设定的延时时间触发而不是帧到达时候才触发
4、注意，虽然简单DC同步机制中没有用到SM Event事件信号，但是它依旧是存在的，因为只要当Frame帧到达从站的时候就会触发SM Event事件信号，这里只是不用到该信号来进行触发而已，（但是应该明白不管是哪一种同步机制，SM Event都是存在的，当从站接受到数据帧的时候都会产生SM Event事件信号）
3.2.4 DC-Synchronous 优化
1、优化的DC模式同时使用了SM Event事件信号和Sync0 Event事件信号，而简单DC同步机制只是使用Sync0 Event这一种事件信号进行同步，无论使用哪一种同步模式，只要当数据帧Frame到达从站的时候都会触发对应的SM Event事件信号。
2、在这种优化的DC模式中，当对应的SM Event事件信号触发后，从站会进入到中断服务例程进行数据的处理,把数据帧Frame中对应的所需数据进行计算，然后复制到管理器通道对应的用户区域，等待Sync0 Event同步信号触发之后让从站取走，然后SM Event中断完成，恢复现场，然后等待Sync0 Event信号的触发到来，也就是同步信号的触发，可以看到由于之前SM Event中断中已经完成了前期数据的处理，当Sync0 Event同步信号触发时，程序进入到中断服务例程，就只需要很短的一段Output Delay Time，马上就进入到Output Valid(输出有效)状态。
3、这种优化版的DC同步机制相对于之前的简单DC模式，优点就是Output Delay Time输出延时没有那么长了，主要是因为第二种优化的DC模式利用SM Event事件信号的触发完成了前期从数据帧中对应从站数据的计算和复制到管理器通道上，所以当Sync0 Event同步信号触发后，只需要从管理器通道取走已经计算并复制好了的数据，进入到输出有效Output Valid的延时时间自然就比较短了，而第一种简单的DC模式只是使用到了Sync0 Event同步信号，所以当Sync0 Event触发后，需要在Output Delay Time时间内完成数据的复制和计算，所以简单DC模式下的Output Delay Time延时时间就会比较长。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-pYHXkibk-1666254718808)(/image/sync3.png)]
1、当数据帧Frame依次到达每一个从站的时候，EtherCAT的机制会触发SM Event事件信号，（数据帧依次发送到各个从站的时间是一个慢慢变长的延时时间，这是硬件上必然发生无法改善的事情）。
2、然后从站会把数据帧上从站需要的数据进行计算并复制到同步管理器通道上，然后会进行中断事件恢复，
3、主程序等待一个DC Sync Signal同步事件中断信号（也就是Sync0 Event），这个Sync0 Event同步事件信号是所以从站同时触发的，就像规定的9点上班一样<这里的前提是主站已经通过发送帧对各个从站完成了对表计时步骤，各个从站的时间是同步一致的>。
4、当Sync0 Event事件信号同步触发时候，由于之前已经完成了数据的计算和复制，那么此时的输出抖动会非常小，约为15ns，（输出抖动jitter和主站有关，在SM Event事件之前由主站触发），同时同步性能也会非常好（Output Delay time会很短，Output Valid输出有效很快触发）。
5、在Output Valid输出有效触发之后，从站会等待一个Input Latch信号，它可以是Sync1 Event事件信号，也可以是Sync0 Event事件信号触发后一段固定的延时时间，这取决于我们在TwinCAT上的设置情况。
3.3 DC同步丢帧
1、问题描述：在使用DC模式的时候会出现一种同步丢帧的情况，就是说数据帧在到达尾端从站之前，所有从站的Sync0 Event同步事件信号就已经触发了，也就是说数据帧传输的太慢，可能还来不及到达尾端从站，但是它的同步事件信号已经触发了，而此时从站却从管理器通道上获取不到数据帧中对应的数据，从站就会判断数据帧丢失了，这就是同步丢帧的问题。
这种问题一般在考前的从站中发生较少，当一个系统较大时，尾端的从站接收到数据帧的时间也比较晚，因为存在物理传输时间，所以越后面的从站接收到的数据帧时间就越晚，虽然Sync0 Event事件信号在完成DC对时之后是同步触发的，但是数据帧的传递时间是依次递增的，如果我们刚开始留出的偏移时间(shift time)不够大的话就有可能在尾端从站发生同步丢帧的情况
2、解决方案：把第一个从站和主站之间的偏移时间调大，可以在TwinCAT中对Shift Time进行调整。（由于Sync0 Event是同步触发的，只需要设置好第一个从站和主站之间的Shift time，后面的从站和主站的Shift Time也都是这个值）。
3.4 同步模式细分
3.5 对象0x1c32
同步管理器2 0x1c32
0x1c32 output
ox1c33 input
0x1c32:01h
{//同步模式
rw
//可读可写
uint16
//类型
value:0,1,2,3
//0:free run
//1:同步SM2事件
//2:DC模式,同步sync0事件
//3:DC模式，同步sync1事件
}
0x1c32:02h
{//周期时间
ro
//只可读
uint32
//类型
value:ns
//ns单位，带有SM2事件的本地计时器同步器的循环时间，主DC模式的循环时间，SYNC0/SYNC1 循环时间
}
0x1c32:03h
{//有效输出偏移，output shift time
ro/rw
uint32
value：ns
//sync0事件与输出有效之间的时间，单位ns，仅在DC模式下支持
}
0x1c32:04h
{//支持的同步类型
ro
uint16
value
//支持同步模式下有效
//bit0 = 1:支持free run
//bit1 = 1:支持SM2同步方式
//bit[2,3] = 01 支持DC模式
//bit4 = 1：支持DC同步sync0事件
//bit5 = 1：支持DC同步sync1事件
//bit14 = 1：动态时报，可以通过编写测量 Messen 0 x1c32:08
//其它bit保留
}
0x1c32:05h
{//最小周期时间
ro
uint32
value
//每个从站为了完成所有内部任务需要的最小循环时间，包括过程数据的交换， 邮箱通讯， 状态机处理， 应用相关的功能等等， 最小循环时间取决于多个因素，比如应用的复杂程度以及要交换的过程数据量
//对于链接到 DC-Synchronous 从站的任务，
//应该确保 主站通讯周期（Communication Cycle Time ）≥ （从站最小循环时间）Minimum Cycle Time，
//否则从站应用就可能跟不上主站通讯的节拍。
//如果为了确保主站通讯周期大于所有从站的最小循环周期，
//不得不严重限制某些DC及非DC从站的通讯周期，
//以至于需要快速响应的任务不能执行，建议使用周期不同的多个任务，并把每个从站链接到适当周期的任务
//将数据从ESC拷贝到内存中占用的最小时间
//支持的最小循环时间，以ns为单位
}
0x1c32：06h
{//计算和复制时间，output Calc and copy time
ro
uint32
//minimum time between trigger event(SM or sync0) and sync event(sync0 or sync1)
}
0x1c32:07h
{//最小硬件延迟
ro
uint32
}
0x1c32:08h
{//保留
ro
}
0x1c32:09h
{//硬件延迟时间
ro
uint32
}
0x1c32:10h
{//sync0 周期时间
ro
uint32
}
3.6 igh ethercat官方例程的说明
-
dc_user
以主站为参考时钟，实现方式是：ecrt_master_sync_reference_clock这个函数将最近一次从ecrt_master_application_time传入的时间戳发送给参考时钟ecrt_master_sync_slave_clocks这个函数的作用是将参考时钟发送给所有从站，以实现dc时钟同步 -
rtai_rtdm_dc
以从站为参考时钟
二.以主站时钟为参考
//编译：g++ -o bk14 src/bk14.cpp ipipe_64.a -I/opt/etherlab/include -L./ -L/opt/etherlab/lib -Wl,--rpath=./ -Wl,--rpath=/opt/etherlab/lib -lethercat -lm -lrt -lipipe_64
//运行：sudo ./bk14
//taike电机 以主站作为参考时间
//减小DC_FILTER_CNT, 可以降低dc_diff_ns，
#include 以主站为参考时钟，会出现
lm:~$sudo dmesg
[280374.468550] r8169 0000:04:00.1 enp4s0f1: Link is Down
[280374.468991] EtherCAT 0: Link state of ecm0 changed to DOWN.
[281905.622543] r8169 0000:04:00.1 enp4s0f1: Link is Up - 100Mbps/Full - flow control rx/tx
[281905.624451] EtherCAT 0: Link state of ecm0 changed to UP.
[281905.626454] EtherCAT 0: 7 slave(s) responding on main device.
[281905.626458] EtherCAT 0: Slave states on main device: PREOP.
[281905.626611] EtherCAT 0: Scanning bus.
[281905.736375] EtherCAT WARNING 0: 27 datagrams TIMED OUT!
[281905.736380] EtherCAT WARNING 0: 199 datagrams UNMATCHED!
[281906.004788] EtherCAT 0: Bus scanning completed in 378 ms.
[281906.004792] EtherCAT 0: Using slave 0 as DC reference clock.
[281907.460478] EtherCAT WARNING 0: 7 datagrams TIMED OUT!
[281907.460483] EtherCAT WARNING 0: 7 datagrams UNMATCHED!
[281908.828153] EtherCAT WARNING 0: 3 datagrams TIMED OUT!
[281908.828158] EtherCAT WARNING 0: 3 datagrams UNMATCHED!
[281912.095492] EtherCAT WARNING 0: 2 datagrams TIMED OUT!
[281912.095496] EtherCAT WARNING 0: 2 datagrams UNMATCHED!
[281916.932601] EtherCAT WARNING 0: 2 datagrams TIMED OUT!
[281916.932615] EtherCAT WARNING 0: 2 datagrams UNMATCHED!
[281920.965582] EtherCAT WARNING 0: 2 datagrams TIMED OUT!
[281920.965596] EtherCAT WARNING 0: 2 datagrams UNMATCHED!
[281922.539623] EtherCAT WARNING 0: 1 datagram TIMED OUT!
[281922.539637] EtherCAT WARNING 0: 1 datagram UNMATCHED!
[281923.890630] EtherCAT WARNING 0: 6 datagrams TIMED OUT!
[281923.890644] EtherCAT WARNING 0: 6 datagrams UNMATCHED!
[281927.266652] EtherCAT WARNING 0: 2 datagrams TIMED OUT!
[281927.266657] EtherCAT WARNING 0: 2 datagrams UNMATCHED!
[281936.043685] EtherCAT WARNING 0: 2 datagrams TIMED OUT!
[281936.043690] EtherCAT WARNING 0: 2 datagrams UNMATCHED!
[281936.670449] EtherCAT: Requesting master 0...
[281936.670452] EtherCAT: Successfully requested master 0.
[281936.670729] EtherCAT 0: Domain0: Logical address 0x00000000, 140 byte, expected working counter 21.
[281936.670732] EtherCAT 0: Datagram domain0-0-main: Logical offset 0x00000000, 140 byte, type LRW at 00000000d781fa5f.
[281936.670772] EtherCAT 0: Master thread exited.
[281936.670775] EtherCAT 0: Starting EtherCAT-OP thread.
[281937.043712] EtherCAT WARNING 0: 1 datagram TIMED OUT!
[281937.043717] EtherCAT WARNING 0: 2 datagrams UNMATCHED!
[281942.822919] EtherCAT WARNING 0-0: Slave did not sync after 5008 ms.
[281942.878914] EtherCAT 0: Domain 0: Working counter changed to 2/21.
[281943.886923] EtherCAT 0: Domain 0: 3 working counter changes - now 6/21.
[281949.630973] EtherCAT WARNING 0-2: Slave did not sync after 5000 ms.
[281949.670970] EtherCAT 0: Domain 0: Working counter changed to 8/21.
[281950.678980] EtherCAT 0: Domain 0: Working counter changed to 9/21.
[281952.717962] EtherCAT 0: Domain 0: Working counter changed to 0/21.
[281952.806997] EtherCAT WARNING: Datagram 00000000d781fa5f (domain0-0-main) was SKIPPED 7 times.
[281953.046809] EtherCAT WARNING 0: 17 datagrams UNMATCHED!
[281953.719003] EtherCAT 0: Domain 0: Working counter changed to 9/21.
[281955.359033] EtherCAT WARNING 0-3: Slave did not sync after 5000 ms.
[281955.399037] EtherCAT 0: Domain 0: Working counter changed to 11/21.
[281956.407019] EtherCAT 0: Domain 0: Working counter changed to 12/21.
[281961.495061] EtherCAT WARNING 0-4: Slave did not sync after 5000 ms.
[281961.551058] EtherCAT 0: Domain 0: Working counter changed to 15/21.
[281962.559076] EtherCAT 0: Domain 0: Working counter changed to 18/21.
[281968.271133] EtherCAT WARNING 0-6: Slave did not sync after 5000 ms.
[281968.311135] EtherCAT 0: Domain 0: Working counter changed to 21/21.
[281968.337007] EtherCAT 0: Slave states on main device: OP.
超时5000ms,即从站同步较慢
后续没有出现同步错误的提示
该方法的主要内容：
- 配置从站的dc方式，0x0300是从站DC的激活字，可以从电机厂家给的XML文件的
ecrt_slave_config_dc(sc[i], 0x0300, cycle_ns, cycle_ns/4, 0, 0);//8ms
激活字对应的是从站的0x980和0x981寄存器的内容
0x0300对应寄存器0x980 = 0x00，0x981 = 0x03，即SYNC输出单元控制和激活sync0.
0x0700对应寄存器0x980 = 0x00，0x981 = 0x07,即SYNC输出单元控制和激活sync0、sync1.
这里需要根据从站来决定
2. 将主站时钟同步到参考时钟，该函数将主站的时钟同步到参考时钟(注意，此时参考时钟依然是默认的第一个带dc的slave，但是该时钟的数值被主站修改了，这样间接的实现以主站为参考时钟)
ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time));//将主站时钟同步到参考时钟
ecrt_master_sync_slave_clocks(master);//将参考时钟发送给所有的从站，以实现dc同步
三.以从站为参考时钟
//编译：g++ -o bk14 src/bk14.cpp ipipe_64.a -I/opt/etherlab/include -L./ -L/opt/etherlab/lib -Wl,--rpath=./ -Wl,--rpath=/opt/etherlab/lib -lethercat -lm -lrt -lipipe_64
//运行：sudo ./bk14
//7电机 以从站作为参考时间
#include
// wakeupTime.tv_nsec= cycle_ns % NSEC_PER_SEC;
// std::cout<< "wakeupTime.tv_nsec: " <
// int s = clock_nanosleep(CLOCK_TO_USE, 0, &wakeupTime, NULL);
if (s != 0) { //避免过度睡眠
if (s == EINTR)
printf("Interrupted by signal handler\n");
else
printf("clock_nanosleep:errno=%d\n",s);
}
// done if we got to here
break;
}
// set master time in nano-seconds
ecrt_master_application_time(master, wakeup_time);
// calc next wake time (in sys time)
wakeup_time += cycle_ns;
}
/****************************************************************************/
int32_t current_pos[7], target_pos[7];
int16_t error_co[7];
int32_t follow_err[7];
bool clear_err_zero = true;
bool _release_ = false;
int32_t current_pos_5, target_pos_5;
int16_t error_co_5;
int32_t follow_err_5;
bool state_ok = false;
void *my_cyclic(void *data)
{
static uint16_t command[7] = {0x004F,0x004F,0x004F,0x004F,0x004F,0x004F,0x004F};//用来帮助判断状态字的值
int cycle_counter = 0;
unsigned int blink = 0;
//signed long temp[8]={};
uint16_t status[7]; //读取伺服状态
uint32_t target_position = 0;
int once=0;
uint32_t positions[N];
int i,dir=0;
// oneshot mode to allow adjustable wake time
//rt_set_oneshot_mode();
// set first wake time in a few cycles
wakeup_time = system_time_ns() + 10 * cycle_ns;
// start the timer
//start_rt_timer(nano2count(cycle_ns));
//rt_make_hard_real_time();
bool kk=0;
int k1 = 10000;
while (1) {
// wait for next period (using adjustable system time)
wait_period();
cycle_counter++;
// receive EtherCAT
ecrt_master_receive(master);
ecrt_domain_process(domain1);
if(!(cycle_counter % 500))
{
cycle_counter = 0;
rt_check_domain_state();//检查数据域
rt_check_master_state();//检查主站
for(int i = 0; i < num_; i++)//检查从站
{
check_slave_config_states(sc[i], i);
printf("*****```*****\n");
}
}
if(master_state.al_states == 8 && sc_state[0].al_state == 8 && sc_state[1].al_state == 8
&& sc_state[2].al_state == 8 && sc_state[3].al_state == 8
&& sc_state[4].al_state == 8 && sc_state[5].al_state == 8
&& sc_state[6].al_state == 8)//检测从站激活进度
{
state_ok = true;
}
//taike control
/*Read state*/
if(state_ok == true)
{
for(int i = 0; i < num_; i++)
{
current_pos[i] = EC_READ_S32(domain1_pd + offset.current_position[i]);
status[i] = EC_READ_U16(domain1_pd + offset.status_word[i]);//读取状态字
error_co[i] = EC_READ_S16(domain1_pd + offset.error_code[i]);
printf("-------------------------------\n");
printf("status[%d]:%x\n", i, status[i]);
printf("current_position[%d]:%d\n", i, current_pos[i]);
printf("target_position[%d]:%d\n", i, target_pos[i]);
printf("error_code[%d]:%x\n", i, error_co[i]);
printf("mode[%d]:%d\n", i, EC_READ_S8(domain1_pd + offset.mode_operation[i]));
printf("kk = %d\n", kk);
printf("-------------------------------\n");
}
// init slave
for (int i = 0; i < num_; i++)
{
if( (status[i] & command[i]) == 0x0040 )
{
EC_WRITE_U16(domain1_pd + offset.ctrl_word[i], 0x0006 );
current_pos[i]=EC_READ_S32(domain1_pd+offset.current_position[i]);
EC_WRITE_S32(domain1_pd+offset.target_position[i],current_pos[i]);
target_pos[i] = current_pos[i];
EC_WRITE_S8(domain1_pd + offset.operation_mode[i], 8);
command[i] = 0x006F;
printf("command[%d] = 0x0006", i);
}
else if( (status[i] & command[i]) == 0x0021)
{
EC_WRITE_U16(domain1_pd + offset.ctrl_word[i], 0x0007 );
EC_WRITE_S32(domain1_pd+offset.target_position[i],current_pos[i]);
command[i] = 0x006F;
printf("command[%d] = 0x0007", i);
}
else if( (status[i] & command[i]) == 0x0023)
{
EC_WRITE_U16(domain1_pd + offset.ctrl_word[i], 0x000f );
command[i] = 0x006F;
printf("command[%d] = 0x000f", i);
}
}
//write process data
for(int i = 0; i < num_; i++)
{ //operation enabled
if( (status[i] & command[i]) == 0x0027)
{
// current_pos[i]=EC_READ_S32(domain1_pd+offset.current_position[i]);
// EC_WRITE_S32(domain1_pd+offset.target_position[i],current_pos[i]);
printf("\n slave[%d] is enabled\n", i);
// if (!_release_)
// {
// // target_pos = target_pos + 100 * sin(t);
// // t = t + 0.01;
// if (kk == 0)
// {
// kk = 1;
// target_pos = EC_READ_S32(domain1_pd+offset.current_position) + 100;
// }
// else
// target_pos = target_pos + 100;
// EC_WRITE_S32(domain1_pd + offset.target_position, target_pos);
// }
}
}
}
if(_release_)
{
k1--;
printf("Release time k1 = %d\n", k1);
for(int i = 0; i < num_; i++)
{
break;
}
if(k1 == 0)
{// Prevent endless circulation
break;
}
break;
}
// queue process data
ecrt_domain_queue(domain1);
// sync distributed clock just before master_send to set
// most accurate master clock time
sync_distributed_clocks();
// send EtherCAT data
ecrt_master_send(master);
// update the master clock
// Note: called after ecrt_master_send() to reduce time
// jitter in the sync_distributed_clocks() call
update_master_clock();
}
//rt_make_soft_real_time();
//stop_rt_timer();
return NULL;
}
/****************************************************************************
* Signal handler
***************************************************************************/
void signal_handler(int sig)
{
printf("get the ctrl-c signal : %d\n\n", sig);
_release_ = true;
usleep(1000000);//wait for thread down
}
/****************************************************************************
* Main function
***************************************************************************/
int main(int argc, char *argv[])
{
//ec_slave_config_t *sc_ek1100;
//ec_slave_config_t *sc_zero;
int ret;
int i = 0;
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
perror("mlockall failed");
return -1;
}
printf("Requesting master...\n");
master = ecrt_request_master(0);
if (!master) {
return -1;
}
domain1 = ecrt_master_create_domain(master);
if (!domain1) {
return -1;
}
printf("Creating slave configurations...\n");
for(i = 0; i <num_; i++ )
{
if (!(sc[i] = ecrt_master_slave_config(master, a[i], p[i], VID_PID)))
{
fprintf(stderr, "Failed to get slave %d configuration.\n", i);
return -1;
}
if (ecrt_slave_config_pdos(sc[i], EC_END, device_syncs))
{
fprintf(stderr, "Failed to configure slave %d PDOs.\n", i);
return -1;
}
}
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
fprintf(stderr, "slave PDO entry registration failed!\n");
return -1;
}
/* Set the initial master time and select a slave to use as the DC
* reference clock, otherwise pass NULL to auto select the first capable
* slave. Note: This can be used whether the master or the ref slave will
* be used as the systems master DC clock.
*/
dc_start_time_ns = system_time_ns();
dc_time_ns = dc_start_time_ns;
printf("1.dc_time_ns:%ld\n", dc_time_ns);
for(i = 0; i < num_; i++)
{
ecrt_slave_config_sdo16(sc[i], 0x1c32, 1, 2);
ecrt_slave_config_sdo16(sc[i], 0x1c33, 1, 2);
ecrt_slave_config_dc(sc[i], 0x0300, cycle_ns, cycle_ns/2, 0, 0);//8ms
}
ret = ecrt_master_select_reference_clock(master, sc[0]);
if (ret < 0) {
fprintf(stderr, "Failed to select reference clock: %s\n",
strerror(-ret));
return ret;
}
printf("Activating master...\n");
if (ecrt_master_activate(master)) {
return -1;
}
//usleep(1000000);
if (!(domain1_pd = ecrt_domain_data(domain1))) {
fprintf(stderr, "Failed to get domain data pointer.\n");
return -1;
}
// /* Create cyclic RT-thread */
// struct sched_param param;
// param.sched_priority = 98;
// printf("Using priority %i.", param.sched_priority);
// if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
// puts("ERROR IN SETTING THE SCHEDULER");
// perror("errno");
// return -1;
// }
// my_cyclic();
/* Set priority */
struct sched_param param = {};
pthread_attr_t attr;
pthread_t thread;
int ret_;
/* Initialize pthread attributes (default values) */
ret_ = pthread_attr_init(&attr);
if (ret_) {
printf("init pthread attributes failed\n");
goto out;
}
/* Set a specific stack size */
ret_ = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
if (ret_) {
printf("pthread setstacksize failed\n");
goto out;
}
/* Set scheduler policy and priority of pthread */
ret_ = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
if (ret_) {
printf("pthread setschedpolicy failed\n");
goto out;
}
param.sched_priority = 98;
ret_ = pthread_attr_setschedparam(&attr, ¶m);
if (ret_){
printf("pthread setschedparam failed\n");
goto out;
}
/* Use scheduling parameters of attr */
ret_ = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (ret_) {
printf("pthread setinheritsched failed\n");
goto out;
}
/* Create a pthread with specified attributes */
usleep(1000000/125);
ret_ = pthread_create(&thread, &attr, my_cyclic, NULL);
if (ret_) {
printf("create pthread failed\n");
goto out;
}
/* Join the thread and wait until it is done */
ret_ = pthread_join(thread, NULL);
out:
printf("End of Program\n");
ecrt_release_master(master);
return 0;
}
- 更改睡眠时钟，例程的是rt相关，这里作了修改
即
system2count_change()返回值修改为ns时间，原例是有计数时钟所以返回计数
system_time_ns()修改，clock_gettime获取时间 - 以从站为参考
ret = ecrt_master_select_reference_clock(master, sc[0]); - 获取参考时钟的时间
ecrt_master_reference_clock_time(master, &ref_time);//获取参考时钟的低32位 - 计算主站时钟与参考时钟的偏差，即主站的时钟偏移
dc_diff_ns = (uint32_t) prev_app_time - ref_time; - 使从站同步参考时钟
ecrt_master_sync_slave_clocks(master); - 计算时钟漂移
int32_t delta = dc_diff_ns - prev_dc_diff_ns; - 归一化处理，因为是为保证相位同步
dc_diff_ns = ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2); - 后续对偏移进行滤波操作，最后将偏差添加到基时间
结果如下
[285162.618427] EtherCAT: Requesting master 0...
[285162.618433] EtherCAT: Successfully requested master 0.
[285162.618828] EtherCAT 0: Application selected DC reference clock config (0-0) set by application.
[285162.618834] EtherCAT 0: Using slave 0 as application selected DC reference clock.
[285162.618836] EtherCAT 0: Using slave 0 as DC reference clock.
[285162.618866] EtherCAT 0: Domain0: Logical address 0x00000000, 140 byte, expected working counter 21.
[285162.618870] EtherCAT 0: Datagram domain0-0-main: Logical offset 0x00000000, 140 byte, type LRW at 000000001de6bb29.
[285162.618899] EtherCAT 0: Master thread exited.
[285162.618902] EtherCAT 0: Starting EtherCAT-OP thread.
[285162.619003] EtherCAT WARNING 0: 1 datagram TIMED OUT!
[285162.619007] EtherCAT WARNING 0: 2 datagrams UNMATCHED!
[285163.907391] EtherCAT 0: Domain 0: Working counter changed to 2/21.
[285164.915418] EtherCAT 0: Domain 0: 3 working counter changes - now 6/21.
[285165.923446] EtherCAT 0: Domain 0: 2 working counter changes - now 9/21.
[285171.011438] EtherCAT 0: Domain 0: Working counter changed to 11/21.
[285172.018424] EtherCAT 0: Domain 0: Working counter changed to 12/21.
[285173.025458] EtherCAT 0: Domain 0: Working counter changed to 15/21.
[285174.032442] EtherCAT 0: Domain 0: Working counter changed to 18/21.
[285174.433663] EtherCAT 0: Slave states on main device: OP.
[285175.039456] EtherCAT 0: Domain 0: Working counter changed to 21/21.
解决了超时问题，未出现同步错误
//ipipe_64.h
#include //ipipe_64.c
//针对c包含.c生成的.so链接库，需要以下操作
/*1,gcc -c -fPIC ipipe_64.c
*2,gcc -shared -fPIC -o libipipe_64.so ipipe_64.o
*/
//针对c++包含c生成的.a链接库
//1,gcc -c ipipe_64.c
//2,ar -rc ipipe_64.a ipipe_64.o
#include "ipipe_64.h"
double cpu_mhz;
unsigned long clock_freq;
/*x86_64*/
#define ipipe_read_tsc(t) do { \
unsigned int __a,__d; \
asm volatile("rdtsc" : "=a" (__a), "=d" (__d)); \
(t) = ((unsigned long)__a) | (((unsigned long)__d)<<32); \
} while(0)
#define rtai_rdtsc() ({ unsigned long long t; ipipe_read_tsc(t); t; })
static inline long rtai_imuldiv (long i, long mult, long div) {
/* Returns (int)i = (int)i*(int)(mult)/(int)div. */
int dummy;
__asm__ __volatile__ ( \
"mulq %%rdx\t\n" \
"divq %%rcx\t\n" \
: "=a" (i), "=d" (dummy)
: "a" (i), "d" (mult), "c" (div));
return i;
}
static inline long long rtai_llimd(long long ll, long mult, long div) {
return rtai_imuldiv(ll, mult, div);
}
static double get_cpu_freq(void){
FILE *fp;
char s[81];
memset(s,0,81);
fp=popen("cat /proc/cpuinfo|grep cpu\\ MHz|sed -e 's/.*:[^0-9]//'","r");
if(fp<0){
printf("read CPU freq failed.\n");
return 0;
}
fgets(s,80,fp);
fclose(fp);
return (double)atof(s);
}
RTIME rt_get_time(void)
{
return rtai_rdtsc();
}
RTIME rt_get_time_ns(void)
{
return rtai_llimd(rtai_rdtsc(), 1000000000, clock_freq);
}
/* ++++++++++++++++++++++++++ TIME CONVERSIONS +++++++++++++++++++++++++++++ */
RTIME count2nano(RTIME counts)
{
return (counts >= 0 ? rtai_llimd(counts, 1000000000, clock_freq) : -rtai_llimd(-counts, 1000000000, clock_freq));
}
RTIME nano2count(RTIME ns)
{
return (ns >= 0 ? rtai_llimd(ns, clock_freq, 1000000000) : -rtai_llimd(-ns, clock_freq, 1000000000));
}
/*initialize the library*/
void __attribute__ ((constructor)) my_init(void){
cpu_mhz = get_cpu_freq();
clock_freq = (unsigned long)((double)cpu_mhz * 1000000l);
}
/*finitialize the library*/
void __attribute__ ((destructor)) my_fini(void){
}
/*no more*/