OpenEtherCATsociety / SOEM

Simple Open Source EtherCAT Master
Other
1.35k stars 685 forks source link

EL1252 Latch time missing, but input is working #873

Open BlooKe opened 1 week ago

BlooKe commented 1 week ago

Good day all!

I'm a beginner with SOEM, so sorry if it is a stupid mistake.

I'm trying to read input and its latch time from EL1252 module.

My simple test app connects and gets the input itself correctly:

SOEM (Simple Open EtherCAT Master) Simple test Starting simple test ec_init on enx00e04c3f2bac succeeded. 4 slaves found and configured. Slaves mapped, state to SAFE_OP. segments : 1 : 39 0 0 0 Request operational state for all slaves Calculated workcounter 4 Operational state reached for all slaves. .... Processdata cycle 104, WKC 4 , O: 00 00 I: 08 00 98 fd 4f 1f 78 92 c8 17 37 c2 95 93 59 56 42 15 15 cb 2f c6 0a 5e 7b 65 f5 a9 ed e2 b3 bc 7b d4 f1 00 00 T:784651757621946680 Processdata cycle 110, WKC 4 , O: 00 00 I: 0a 00 98 fd 4f 1f 78 92 c8 17 37 c2 95 93 59 56 42 15 15 cb 2f c6 0a 5e 7b 65 f5 a9 ed e2 b3 bc 7b d4 f1 00 00 T:784651757675837840

so first input byte is correct and later on module should update the Latch time too, but it does not change.

My slave info:

SOEM (Simple Open EtherCAT Master) Slaveinfo Starting slaveinfo ec_init on enx00e04c3f2bac succeeded. 4 slaves found and configured. Calculated workcounter 4

Slave:1 Name:EK1100 Output size: 0bits Input size: 0bits State: 4 Delay: 0[ns] Has DC: 1 DCParentport:0 Activeports:1.1.0.0 Configured address: 1001 Man: 00000002 ID: 044c2c52 Rev: 00120000 FMMUfunc 0:0 1:0 2:0 3:0 MBX length wr: 0 rd: 0 MBX protocols : 00 CoE details: 00 FoE details: 00 EoE details: 00 SoE details: 00 Ebus current: -2000[mA] only LRD/LWR:0 PDO mapping according to SII :

Slave:2 Name:EL1252 Output size: 0bits Input size: 280bits State: 4 Delay: 140[ns] Has DC: 1 DCParentport:1 Activeports:1.1.0.0 Configured address: 1002 Man: 00000002 ID: 04e43052 Rev: 00160000 SM0 A:1000 L: 1 F:00010022 Type:4 SM1 A:09ae L: 34 F:00040000 Type:4 SM2 A:0910 L: 0 F:00040000 Type:0 FMMU0 Ls:00000002 Ll: 35 Lsb:0 Leb:7 Ps:1000 Psb:0 Ty:01 Act:01 FMMUfunc 0:2 1:2 2:2 3:0 MBX length wr: 0 rd: 0 MBX protocols : 00 CoE details: 00 FoE details: 00 EoE details: 00 SoE details: 00 Ebus current: 110[mA] only LRD/LWR:0 PDO mapping according to SII : SM0 TXPDO 0x1A00 Channel 1 addr b index: sub bitl data_type name [0x0002.0] 0x6000:0x01 0x01 BOOLEAN Input SM0 TXPDO 0x1A01 Channel 2 addr b index: sub bitl data_type name [0x0002.1] 0x6000:0x02 0x01 BOOLEAN Input SM0 TXPDO 0x1A02 Reserved addr b index: sub bitl data_type name SM1 TXPDO 0x1A13 Latch addr b index: sub bitl data_type name [0x0003.0] 0x1D09:0xAE 0x08 UNSIGNED8 Status1 [0x0004.0] 0x1D09:0xAF 0x08 UNSIGNED8 Status2 [0x0005.0] 0x1D09:0xB0 0x40 UNSIGNED64 LatchPos1 [0x000D.0] 0x1D09:0xB8 0x40 UNSIGNED64 LatchNeg [0x0015.0] 0x1D09:0xC0 0x40 UNSIGNED64 LatchPos2 [0x001D.0] 0x1D09:0xC8 0x40 UNSIGNED64

Slave:3 Name:EL1809 Output size: 0bits Input size: 16bits State: 4 Delay: 285[ns] Has DC: 1 DCParentport:1 Activeports:1.1.0.0 Configured address: 1003 Man: 00000002 ID: 07113052 Rev: 00120000 SM0 A:1000 L: 2 F:00010000 Type:4 FMMU0 Ls:00000025 Ll: 2 Lsb:0 Leb:7 Ps:1000 Psb:0 Ty:01 Act:01 FMMUfunc 0:2 1:0 2:0 3:0 MBX length wr: 0 rd: 0 MBX protocols : 00 CoE details: 00 FoE details: 00 EoE details: 00 SoE details: 00 Ebus current: 100[mA] only LRD/LWR:0 PDO mapping according to SII :

Slave:4 Name:EL2809 Output size: 16bits Input size: 0bits State: 4 Delay: 435[ns] Has DC: 1 DCParentport:1 Activeports:1.0.0.0 Configured address: 1004 Man: 00000002 ID: 0af93052 Rev: 00120000 SM0 A:0f00 L: 1 F:00090044 Type:3 SM1 A:0f01 L: 1 F:00090044 Type:3 FMMU0 Ls:00000000 Ll: 2 Lsb:0 Leb:7 Ps:0f00 Psb:0 Ty:02 Act:01 FMMUfunc 0:1 1:0 2:0 3:0 MBX length wr: 0 rd: 0 MBX protocols : 00 CoE details: 00 FoE details: 00 EoE details: 00 SoE details: 00 Ebus current: 140[mA] only LRD/LWR:0 PDO mapping according to SII : SM0 RXPDO 0x1600 Channel 1 addr b index: sub bitl data_type name [0x0000.0] 0x7000:0x01 0x01 BOOLEAN Output SM0 RXPDO 0x1601 Channel 2 addr b index: sub bitl data_type name [0x0000.1] 0x7010:0x01 0x01 BOOLEAN Output SM0 RXPDO 0x1602 Channel 3 addr b index: sub bitl data_type name [0x0000.2] 0x7020:0x01 0x01 BOOLEAN Output SM0 RXPDO 0x1603 Channel 4 addr b index: sub bitl data_type name [0x0000.3] 0x7030:0x01 0x01 BOOLEAN Output SM0 RXPDO 0x1604 Channel 5 addr b index: sub bitl data_type name [0x0000.4] 0x7040:0x01 0x01 BOOLEAN Output SM0 RXPDO 0x1605 Channel 6 addr b index: sub bitl data_type name [0x0000.5] 0x7050:0x01 0x01 BOOLEAN Output SM0 RXPDO 0x1606 Channel 7 addr b index: sub bitl data_type name [0x0000.6] 0x7060:0x01 0x01 BOOLEAN Output SM0 RXPDO 0x1607 Channel 8 addr b index: sub bitl data_type name [0x0000.7] 0x7070:0x01 0x01 BOOLEAN Output SM1 RXPDO 0x1608 Channel 9 addr b index: sub bitl data_type name [0x0001.0] 0x7080:0x01 0x01 BOOLEAN Output SM1 RXPDO 0x1609 Channel 10 addr b index: sub bitl data_type name [0x0001.1] 0x7090:0x01 0x01 BOOLEAN Output SM1 RXPDO 0x160A Channel 11 addr b index: sub bitl data_type name [0x0001.2] 0x70A0:0x01 0x01 BOOLEAN Output SM1 RXPDO 0x160B Channel 12 addr b index: sub bitl data_type name [0x0001.3] 0x70B0:0x01 0x01 BOOLEAN Output SM1 RXPDO 0x160C Channel 13 addr b index: sub bitl data_type name [0x0001.4] 0x70C0:0x01 0x01 BOOLEAN Output SM1 RXPDO 0x160D Channel 14 addr b index: sub bitl data_type name [0x0001.5] 0x70D0:0x01 0x01 BOOLEAN Output SM1 RXPDO 0x160E Channel 15 addr b index: sub bitl data_type name [0x0001.6] 0x70E0:0x01 0x01 BOOLEAN Output SM1 RXPDO 0x160F Channel 16 addr b index: sub bitl data_type name [0x0001.7] 0x70F0:0x01 0x01 BOOLEAN Output End slaveinfo, close socket End program

Tried the same HW setup with TwinCat and it works properly and LatchPos2/LatchNeg2 updates properly.

I see that random slaves got lost in simple_test:

Processdata cycle 642, WKC 4 , O: 00 00 I: 08 00 98 fd 4f 1f 78 92 c8 17 37 c2 95 93 59 56 42 15 15 cb 2f c6 0a 5e 7b 65 f5 a9 ed e2 b3 bc 7b d4 f1 00 00 T:784651762837984320 OK : all slaves resumed OPERATIONAL. Processdata cycle 646, WKC 4 , O: 00 00 I: 08 00 98 fd 4f 1f 78 92 c8 17 37 c2 95 93 59 56 42 15 15 cb 2f c6 0a 5e 7b 65 f5 a9 ed e2 b3 bc 7b d4 f1 00 00 T:784651762878002080 OK : all slaves resumed OPERATIONAL.0 00 I: 08 00 98 fd 4f 1f 78 92 c8 17 37 c2 95 93 59 56 42 15 15 cb 2f c6 0a 5e 7b 65 f5 a9 ed e2 b3 bc 7b d4 f1 00 00 T:784651762895776920

ERROR : slave 1 lost MESSAGE : slave 1 recovered Processdata cycle 653, WKC 4 , O: 00 00 I: 08 00 98 fd 4f 1f 78 92 c8 17 37 c2 95 93 59 56 42 15 15 cb 2f c6 0a 5e 7b 65 f5 a9 ed e2 b3 bc 7b d4 f1 00 00 T:784651762943385320 ERROR : slave 1 lost MESSAGE : slave 1 recovered 4 , O: 00 00 I: 08 00 98 fd 4f 1f 78 92 c8 17 37 c2 95 93 59 56 42 15 15 cb 2f c6 0a 5e 7b 65 f5 a9 ed e2 b3 bc 7b d4 f1 00 00 T:784651762972430160 ERROR : slave 4 lost

MESSAGE : slave 4 found OK : all slaves resumed OPERATIONAL. Processdata cycle 658, WKC 4 , O: 00 00 I: 08 00 98 fd 4f 1f 78 92 c8 17 37 c2 95 93 59 56 42 15 15 cb 2f c6 0a 5e 7b 65 f5 a9 ed e2 b3 bc 7b d4 f1 00 00 T:784651763007923240 OK : all slaves resumed OPERATIONAL.

From https://infosys.beckhoff.com/english.php?content=../content/1033/el1202_el1252/7223406347.html&id= I see that I should assign 0x1A15 to SM2, but Twincat does not have this, and even after I tried to do that, nothing changed.

Could this be DC synchronization issue? Maybe somebody has an idea what to check and what could be wrong?

ArthurKetels commented 6 days ago

The problem is that the sync/latch unit is not enabled in the slave. In the ESI file of the EL1252:

                <Dc>
                    <OpMode>
                        <Name>DcLatch</Name>
                        <Desc>DC Latch Settings</Desc>
                        <AssignActivate>#x0100</AssignActivate>
                        <CycleTimeSync0 Factor="0">0</CycleTimeSync0>
                        <ShiftTimeSync0>0</ShiftTimeSync0>
                    </OpMode>
                </Dc>

The standard function ecx_dcsync01() is not suited for this. But you can manually do this in the pre-op to safe-op hook:

uint8 RA = 0x01;
(void)ecx_FPWR(context->port, slave, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);

This will enable the sync/latch unit, which is disabled by default.

BlooKe commented 6 days ago

Thanks a lot @ArthurKetels! Your comment guided me to read older DC sync issues and comments and I managed to update red_test example and get at least somewhere.

Few times I managed to get a bit different behavior with updated latch times in EL1252, but in next run it goes back to the issue before.

run with latch times:

SOEM (Simple Open EtherCAT Master)
Redundancy test
Starting Redundant test
ec_init on enx00e04c3f2bac succeeded.
PO2SO hook for EL1252, slave 2 called
2 slaves found and configured.
Slave:1 Name:EK1100 Output size:  0bits Input size:  0bits State:18 delay:0.1
         Out:(nil),   0 In:(nil),   0
Slave:2 Name:EL1252 Output size:  0bits Input size:295bits State:18 delay:150.1
         Out:(nil),   0 In:0x55660101a1a0,  37
Calculated workcounter 1
Request operational state for all slaves
Operational state reached for all slaves.
OK : all slaves resumed OPERATIONAL.Ctime 784732065618181960, dt       131960, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
OK : all slaves resumed OPERATIONAL.Ctime 784732065642081159, dt        31159, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
ERROR : slave 1 lost 57 , Wck  -1, DCtime 784732065692575520, dt        25520, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
ERROR : slave 2 lost 59 , Wck  -1, DCtime 784732065692575520, dt        25520, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
MESSAGE : slave 2 recovered
MESSAGE : slave 1 found , Wck  -1, DCtime 784732065749053560, dt         3560, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
OK : all slaves resumed OPERATIONAL.
OK : all slaves resumed OPERATIONAL.Ctime 784732065764571960, dt        21960, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
ERROR : slave 1 lost 75 , Wck   1, DCtime 784732065805143000, dt        93000, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
MESSAGE : slave 1 found , Wck   1, DCtime 784732065893678840, dt       128840, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
OK : all slaves resumed OPERATIONAL.
OK : all slaves resumed OPERATIONAL.Ctime 784732065950129280, dt        79280, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
OK : all slaves resumed OPERATIONAL.Ctime 784732065984216840, dt       166840, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
OK : all slaves resumed OPERATIONAL.Ctime 784732066012993680, dt       -56320, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
OK : all slaves resumed OPERATIONAL.Ctime 784732066052846440, dt      -203560, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
OK : all slaves resumed OPERATIONAL.Ctime 784732066070691599, dt       141599, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
OK : all slaves resumed OPERATIONAL.Ctime 784732066089713640, dt       163640, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
ERROR : slave 1 lost204 , Wck  -1, DCtime 784732066172538080, dt       -11920, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
ERROR : slave 2 lost207 , Wck   1, DCtime 784732066196864840, dt      -185160, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00
MESSAGE : slave 1 recoveredck   1, DCtime 784732066254243240, dt       193240, O: I: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6a ec d4 e2 97 ed e3 0a 86 62 b3 ea 97 ed e3 0a 00 00 00

Most runs:

SOEM (Simple Open EtherCAT Master)
Redundancy test
Starting Redundant test
ec_init on enx00e04c3f2bac succeeded.
PO2SO hook for EL1252, slave 2 called
2 slaves found and configured.
Slave:1 Name:EK1100 Output size:  0bits Input size:  0bits State:18 delay:0.1
         Out:(nil),   0 In:(nil),   0
Slave:2 Name:EL1252 Output size:  0bits Input size:272bits State:18 delay:155.1
         Out:(nil),   0 In:0x55d7f01061a0,  34
Calculated workcounter 1
Request operational state for all slaves
Operational state reached for all slaves.
OK : all slaves resumed OPERATIONAL.Ctime 784732726872780280, dt       230280, O: I: 08 00 88 fd 4f 9d 78 92 d8 17 37 c2 95 93 59 56 42 15 15 8a 2f c6 0a 5e 7b 65 f5 a8 e9 a2 bb bc 7f 14
OK : all slaves resumed OPERATIONAL.Ctime 784732726905299520, dt       249520, O: I: 08 00 88 fd 4f 9d 78 92 d8 17 37 c2 95 93 59 56 42 15 15 8a 2f c6 0a 5e 7b 65 f5 a8 e9 a2 bb bc 7f 14
ERROR : slave 1 lost 72 , Wck   1, DCtime 784732726951848480, dt      -201520, O: I: 08 00 88 fd 4f 9d 78 92 d8 17 37 c2 95 93 59 56 42 15 15 8a 2f c6 0a 5e 7b 65 f5 a8 e9 a2 bb bc 7f 14
ERROR : slave 2 lost 77 , Wck  -1, DCtime 784732726951848480, dt      -201520, O: I: 08 00 88 fd 4f 9d 78 92 d8 17 37 c2 95 93 59 56 42 15 15 8a 2f c6 0a 5e 7b 65 f5 a8 e9 a2 bb bc 7f 14
MESSAGE : slave 1 found , Wck  -1, DCtime 784732726951848480, dt      -201520, O: I: 08 00 88 fd 4f 9d 78 92 d8 17 37 c2 95 93 59 56 42 15 15 8a 2f c6 0a 5e 7b 65 f5 a8 e9 a2 bb bc 7f 14
MESSAGE : slave 2 found
OK : all slaves resumed OPERATIONAL.

What I noticed - Input size is different.

with 100ms cycle time I can get stable runs without disconnected slaves:

SOEM (Simple Open EtherCAT Master)
Redundancy test
Starting Redundant test
ec_init on enx00e04c3f2bac succeeded.
PO2SO hook for EL1252, slave 2 called
2 slaves found and configured.
Run processing for 10 sec in SAFE-OP
Continue setup
Slave:1 Name:EK1100 Output size:  0bits Input size:  0bits State:18 delay:0.1
         Out:(nil),   0 In:(nil),   0
Slave:2 Name:EL1252 Output size:  0bits Input size:280bits State:18 delay:155.1
         Out:(nil),   0 In:0x55e17e0481a0,  35
Calculated workcounter 1
Request operational state for all slaves
Operational state reached for all slaves.
^Cocessdata cycle   280 , Wck   1, DCtime 784734331503845640, dt      3795640, O: I: 08 00 88 fd 4f 9d 78 92 d8 17 37 c2 95 93 59 56 42 15 15 8a 2f c6 0a 5e 7b 65 f5 a8 e9 a2 bb bc 7f 14 f1

I moved to minimal setup, so slave info looks like this now:

SOEM (Simple Open EtherCAT Master)
Slaveinfo
Starting slaveinfo
ec_init on enx00e04c3f2bac succeeded.
2 slaves found and configured.
Calculated workcounter 1

Slave:1
 Name:EK1100
 Output size: 0bits
 Input size: 0bits
 State: 4
 Delay: 0[ns]
 Has DC: 1
 DCParentport:0
 Activeports:1.1.0.0
 Configured address: 1001
 Man: 00000002 ID: 044c2c52 Rev: 00120000
 FMMUfunc 0:0 1:0 2:0 3:0
 MBX length wr: 0 rd: 0 MBX protocols : 00
 CoE details: 00 FoE details: 00 EoE details: 00 SoE details: 00
 Ebus current: -2000[mA]
 only LRD/LWR:0
PDO mapping according to SII :

Slave:2
 Name:EL1252
 Output size: 0bits
 Input size: 280bits
 State: 4
 Delay: 155[ns]
 Has DC: 1
 DCParentport:1
 Activeports:1.0.0.0
 Configured address: 1002
 Man: 00000002 ID: 04e43052 Rev: 00160000
 SM0 A:1000 L:   1 F:00010022 Type:4
 SM1 A:09ae L:  34 F:00040000 Type:4
 SM2 A:0910 L:   0 F:00040000 Type:0
 FMMU0 Ls:00000000 Ll:  35 Lsb:0 Leb:7 Ps:1000 Psb:0 Ty:01 Act:01
 FMMUfunc 0:2 1:2 2:2 3:0
 MBX length wr: 0 rd: 0 MBX protocols : 00
 CoE details: 00 FoE details: 00 EoE details: 00 SoE details: 00
 Ebus current: 110[mA]
 only LRD/LWR:0
PDO mapping according to SII :
  SM0 TXPDO 0x0000 Channel 1
     addr b   index: sub bitl data_type    name
  SM7 TXPDO 0x6000 EL1252
     addr b   index: sub bitl data_type    name
  [0x0000.0] 0x1A01:0x02 0x08 dt:0x0000 (8) 
  SM7 TXPDO 0x6000 EL1252
     addr b   index: sub bitl data_type    name
  [0x0001.6] 0x1A02:0x00 0x09 dt:0x0000 (9) 
  SM1 TXPDO 0x1A13 Latch
     addr b   index: sub bitl data_type    name
  [0x0002.7] 0x1D09:0xAE 0x08 UNSIGNED8    Status1
  [0x0003.7] 0x1D09:0xAF 0x00 dt:0x0000 (0) Status2
  [0x0003.7] 0x1D09:0xB0 0x40 UNSIGNED64   LatchPos1
  [0x000B.7] 0x1D09:0xB8 0x40 UNSIGNED64   LatchNeg1
  [0x0013.7] 0x1D09:0xC0 0x40 UNSIGNED64   LatchPos2
  [0x001B.7] 0x1D09:0xC8 0x40 UNSIGNED64   LatchNeg2
End slaveinfo, close socket
End program

My updated test code: simple_test.txt

Maybe somebody sees what am I doing wrong? The PO2SO hook is called, but maybe I rewrite the config later on or something like that?