feat(motor): 自适应 PDO 窗口与通道活动指示，新增原生 SOEM 时序诊断
后端 PDO 循环改为按在线槽收缩状态窗口，稀疏总线下单槽每周期都能刷新， 并补上尾部槽的发现扫描游标；新增自旋节拍源（阈值/显式覆盖）、P 核识别 与线程级钉核，自旋期间释放 GIL 以免 HTTP 线程被饿死；启动时先关停旧实例， 避免两个进程争用 Npcap 适配器。 记录每拍完整时间线、GC stop-the-world 事件与网格跳拍数，经 /api/motor/dbg 导出用于归因固定频率的超时；配
This commit is contained in:
@@ -0,0 +1,448 @@
|
||||
/*
|
||||
* Licensed under the GNU General Public License version 2 with exceptions. See
|
||||
* LICENSE file in the project root for full license information
|
||||
*/
|
||||
|
||||
/** \file
|
||||
* \brief
|
||||
* Distributed Clock EtherCAT functions.
|
||||
*
|
||||
*/
|
||||
#include "oshw.h"
|
||||
#include "osal.h"
|
||||
#include "ethercattype.h"
|
||||
#include "ethercatbase.h"
|
||||
#include "ethercatmain.h"
|
||||
#include "ethercatdc.h"
|
||||
|
||||
#define PORTM0 0x01
|
||||
#define PORTM1 0x02
|
||||
#define PORTM2 0x04
|
||||
#define PORTM3 0x08
|
||||
|
||||
/** 1st sync pulse delay in ns here 100ms */
|
||||
#define SyncDelay ((int32)100000000)
|
||||
|
||||
/**
|
||||
* Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param [in] slave Slave number.
|
||||
* @param [in] act TRUE = active, FALSE = deactivated
|
||||
* @param [in] CyclTime Cycltime in ns.
|
||||
* @param [in] CyclShift CyclShift in ns.
|
||||
*/
|
||||
void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
|
||||
{
|
||||
uint8 h, RA;
|
||||
uint16 slaveh;
|
||||
int64 t, t1;
|
||||
int32 tc;
|
||||
|
||||
slaveh = context->slavelist[slave].configadr;
|
||||
RA = 0;
|
||||
|
||||
/* stop cyclic operation, ready for next trigger */
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
|
||||
if (act)
|
||||
{
|
||||
RA = 1 + 2; /* act cyclic operation and sync0, sync1 deactivated */
|
||||
}
|
||||
h = 0;
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
|
||||
t1 = 0;
|
||||
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
|
||||
t1 = etohll(t1);
|
||||
|
||||
/* Calculate first trigger time, always a whole multiple of CyclTime rounded up
|
||||
plus the shifttime (can be negative)
|
||||
This insures best synchronization between slaves, slaves with the same CyclTime
|
||||
will sync at the same moment (you can use CyclShift to shift the sync) */
|
||||
if (CyclTime > 0)
|
||||
{
|
||||
t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift;
|
||||
}
|
||||
else
|
||||
{
|
||||
t = t1 + SyncDelay + CyclShift;
|
||||
/* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
|
||||
}
|
||||
t = htoell(t);
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
|
||||
tc = htoel(CyclTime);
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
|
||||
|
||||
// update ec_slave state
|
||||
context->slavelist[slave].DCactive = (uint8)act;
|
||||
context->slavelist[slave].DCshift = CyclShift;
|
||||
context->slavelist[slave].DCcycle = CyclTime;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param [in] slave Slave number.
|
||||
* @param [in] act TRUE = active, FALSE = deactivated
|
||||
* @param [in] CyclTime0 Cycltime SYNC0 in ns.
|
||||
* @param [in] CyclTime1 Cycltime SYNC1 in ns. This time is a delta time in relation to
|
||||
the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time
|
||||
as SYNC0.
|
||||
* @param [in] CyclShift CyclShift in ns.
|
||||
*/
|
||||
void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
|
||||
{
|
||||
uint8 h, RA;
|
||||
uint16 slaveh;
|
||||
int64 t, t1;
|
||||
int32 tc;
|
||||
uint32 TrueCyclTime;
|
||||
|
||||
/* Sync1 can be used as a multiple of Sync0, use true cycle time */
|
||||
TrueCyclTime = ((CyclTime1 / CyclTime0) + 1) * CyclTime0;
|
||||
|
||||
slaveh = context->slavelist[slave].configadr;
|
||||
RA = 0;
|
||||
|
||||
/* stop cyclic operation, ready for next trigger */
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
|
||||
if (act)
|
||||
{
|
||||
RA = 1 + 2 + 4; /* act cyclic operation and sync0 + sync1 */
|
||||
}
|
||||
h = 0;
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
|
||||
t1 = 0;
|
||||
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
|
||||
t1 = etohll(t1);
|
||||
|
||||
/* Calculate first trigger time, always a whole multiple of TrueCyclTime rounded up
|
||||
plus the shifttime (can be negative)
|
||||
This insures best synchronization between slaves, slaves with the same CyclTime
|
||||
will sync at the same moment (you can use CyclShift to shift the sync) */
|
||||
if (CyclTime0 > 0)
|
||||
{
|
||||
t = ((t1 + SyncDelay) / TrueCyclTime) * TrueCyclTime + TrueCyclTime + CyclShift;
|
||||
}
|
||||
else
|
||||
{
|
||||
t = t1 + SyncDelay + CyclShift;
|
||||
/* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
|
||||
}
|
||||
t = htoell(t);
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
|
||||
tc = htoel(CyclTime0);
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
|
||||
tc = htoel(CyclTime1);
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
|
||||
|
||||
// update ec_slave state
|
||||
context->slavelist[slave].DCactive = (uint8)act;
|
||||
context->slavelist[slave].DCshift = CyclShift;
|
||||
context->slavelist[slave].DCcycle = CyclTime0;
|
||||
}
|
||||
|
||||
/* latched port time of slave */
|
||||
static int32 ecx_porttime(ecx_contextt *context, uint16 slave, uint8 port)
|
||||
{
|
||||
int32 ts;
|
||||
switch (port)
|
||||
{
|
||||
case 0:
|
||||
ts = context->slavelist[slave].DCrtA;
|
||||
break;
|
||||
case 1:
|
||||
ts = context->slavelist[slave].DCrtB;
|
||||
break;
|
||||
case 2:
|
||||
ts = context->slavelist[slave].DCrtC;
|
||||
break;
|
||||
case 3:
|
||||
ts = context->slavelist[slave].DCrtD;
|
||||
break;
|
||||
default:
|
||||
ts = 0;
|
||||
break;
|
||||
}
|
||||
return ts;
|
||||
}
|
||||
|
||||
/* calculate previous active port of a slave */
|
||||
static uint8 ecx_prevport(ecx_contextt *context, uint16 slave, uint8 port)
|
||||
{
|
||||
uint8 pport = port;
|
||||
uint8 aport = context->slavelist[slave].activeports;
|
||||
switch(port)
|
||||
{
|
||||
case 0:
|
||||
if(aport & PORTM2)
|
||||
pport = 2;
|
||||
else if (aport & PORTM1)
|
||||
pport = 1;
|
||||
else if (aport & PORTM3)
|
||||
pport = 3;
|
||||
break;
|
||||
case 1:
|
||||
if(aport & PORTM3)
|
||||
pport = 3;
|
||||
else if (aport & PORTM0)
|
||||
pport = 0;
|
||||
else if (aport & PORTM2)
|
||||
pport = 2;
|
||||
break;
|
||||
case 2:
|
||||
if(aport & PORTM1)
|
||||
pport = 1;
|
||||
else if (aport & PORTM3)
|
||||
pport = 3;
|
||||
else if (aport & PORTM0)
|
||||
pport = 0;
|
||||
break;
|
||||
case 3:
|
||||
if(aport & PORTM0)
|
||||
pport = 0;
|
||||
else if (aport & PORTM2)
|
||||
pport = 2;
|
||||
else if (aport & PORTM1)
|
||||
pport = 1;
|
||||
break;
|
||||
}
|
||||
return pport;
|
||||
}
|
||||
|
||||
/* search unconsumed ports in parent, consume and return first open port */
|
||||
static uint8 ecx_parentport(ecx_contextt *context, uint16 parent)
|
||||
{
|
||||
uint8 parentport = 0;
|
||||
uint8 b;
|
||||
/* search order is important, here 3 - 1 - 2 - 0 */
|
||||
b = context->slavelist[parent].consumedports;
|
||||
if (b & PORTM3)
|
||||
{
|
||||
parentport = 3;
|
||||
b &= (uint8)~PORTM3;
|
||||
}
|
||||
else if (b & PORTM1)
|
||||
{
|
||||
parentport = 1;
|
||||
b &= (uint8)~PORTM1;
|
||||
}
|
||||
else if (b & PORTM2)
|
||||
{
|
||||
parentport = 2;
|
||||
b &= (uint8)~PORTM2;
|
||||
}
|
||||
else if (b & PORTM0)
|
||||
{
|
||||
parentport = 0;
|
||||
b &= (uint8)~PORTM0;
|
||||
}
|
||||
context->slavelist[parent].consumedports = b;
|
||||
return parentport;
|
||||
}
|
||||
|
||||
/**
|
||||
* Locate DC slaves, measure propagation delays.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @return boolean if slaves are found with DC
|
||||
*/
|
||||
boolean ecx_configdc(ecx_contextt *context)
|
||||
{
|
||||
uint16 i, slaveh, parent, child;
|
||||
uint16 parenthold = 0;
|
||||
uint16 prevDCslave = 0;
|
||||
int32 ht, dt1, dt2, dt3;
|
||||
int64 hrt;
|
||||
uint8 entryport;
|
||||
int8 nlist;
|
||||
int8 plist[4];
|
||||
int32 tlist[4];
|
||||
ec_timet mastertime;
|
||||
uint64 mastertime64;
|
||||
|
||||
context->slavelist[0].hasdc = FALSE;
|
||||
context->grouplist[0].hasdc = FALSE;
|
||||
ht = 0;
|
||||
|
||||
ecx_BWR(context->port, 0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); /* latch DCrecvTimeA of all slaves */
|
||||
mastertime = osal_current_time();
|
||||
mastertime.sec -= 946684800UL; /* EtherCAT uses 2000-01-01 as epoch start instead of 1970-01-01 */
|
||||
mastertime64 = (((uint64)mastertime.sec * 1000000) + (uint64)mastertime.usec) * 1000;
|
||||
for (i = 1; i <= *(context->slavecount); i++)
|
||||
{
|
||||
context->slavelist[i].consumedports = context->slavelist[i].activeports;
|
||||
if (context->slavelist[i].hasdc)
|
||||
{
|
||||
if (!context->slavelist[0].hasdc)
|
||||
{
|
||||
context->slavelist[0].hasdc = TRUE;
|
||||
context->slavelist[0].DCnext = i;
|
||||
context->slavelist[i].DCprevious = 0;
|
||||
context->grouplist[context->slavelist[i].group].hasdc = TRUE;
|
||||
context->grouplist[context->slavelist[i].group].DCnext = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
context->slavelist[prevDCslave].DCnext = i;
|
||||
context->slavelist[i].DCprevious = prevDCslave;
|
||||
}
|
||||
/* this branch has DC slave so remove parenthold */
|
||||
parenthold = 0;
|
||||
prevDCslave = i;
|
||||
slaveh = context->slavelist[i].configadr;
|
||||
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);
|
||||
context->slavelist[i].DCrtA = etohl(ht);
|
||||
/* 64bit latched DCrecvTimeA of each specific slave */
|
||||
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET);
|
||||
/* use it as offset in order to set local time around 0 + mastertime */
|
||||
hrt = htoell(-etohll(hrt) + mastertime64);
|
||||
/* save it in the offset register */
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET);
|
||||
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET);
|
||||
context->slavelist[i].DCrtB = etohl(ht);
|
||||
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET);
|
||||
context->slavelist[i].DCrtC = etohl(ht);
|
||||
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET);
|
||||
context->slavelist[i].DCrtD = etohl(ht);
|
||||
|
||||
/* make list of active ports and their time stamps */
|
||||
nlist = 0;
|
||||
if (context->slavelist[i].activeports & PORTM0)
|
||||
{
|
||||
plist[nlist] = 0;
|
||||
tlist[nlist] = context->slavelist[i].DCrtA;
|
||||
nlist++;
|
||||
}
|
||||
if (context->slavelist[i].activeports & PORTM3)
|
||||
{
|
||||
plist[nlist] = 3;
|
||||
tlist[nlist] = context->slavelist[i].DCrtD;
|
||||
nlist++;
|
||||
}
|
||||
if (context->slavelist[i].activeports & PORTM1)
|
||||
{
|
||||
plist[nlist] = 1;
|
||||
tlist[nlist] = context->slavelist[i].DCrtB;
|
||||
nlist++;
|
||||
}
|
||||
if (context->slavelist[i].activeports & PORTM2)
|
||||
{
|
||||
plist[nlist] = 2;
|
||||
tlist[nlist] = context->slavelist[i].DCrtC;
|
||||
nlist++;
|
||||
}
|
||||
/* entryport is port with the lowest timestamp */
|
||||
entryport = 0;
|
||||
if((nlist > 1) && (tlist[1] < tlist[entryport]))
|
||||
{
|
||||
entryport = 1;
|
||||
}
|
||||
if((nlist > 2) && (tlist[2] < tlist[entryport]))
|
||||
{
|
||||
entryport = 2;
|
||||
}
|
||||
if((nlist > 3) && (tlist[3] < tlist[entryport]))
|
||||
{
|
||||
entryport = 3;
|
||||
}
|
||||
entryport = plist[entryport];
|
||||
context->slavelist[i].entryport = entryport;
|
||||
/* consume entryport from activeports */
|
||||
context->slavelist[i].consumedports &= (uint8)~(1 << entryport);
|
||||
|
||||
/* finding DC parent of current */
|
||||
parent = i;
|
||||
do
|
||||
{
|
||||
child = parent;
|
||||
parent = context->slavelist[parent].parent;
|
||||
}
|
||||
while (!((parent == 0) || (context->slavelist[parent].hasdc)));
|
||||
/* only calculate propagation delay if slave is not the first */
|
||||
if (parent > 0)
|
||||
{
|
||||
/* find port on parent this slave is connected to */
|
||||
context->slavelist[i].parentport = ecx_parentport(context, parent);
|
||||
if (context->slavelist[parent].topology == 1)
|
||||
{
|
||||
context->slavelist[i].parentport = context->slavelist[parent].entryport;
|
||||
}
|
||||
|
||||
dt1 = 0;
|
||||
dt2 = 0;
|
||||
/* delta time of (parentport - 1) - parentport */
|
||||
/* note: order of ports is 0 - 3 - 1 -2 */
|
||||
/* non active ports are skipped */
|
||||
dt3 = ecx_porttime(context, parent, context->slavelist[i].parentport) -
|
||||
ecx_porttime(context, parent,
|
||||
ecx_prevport(context, parent, context->slavelist[i].parentport));
|
||||
/* current slave has children */
|
||||
/* those children's delays need to be subtracted */
|
||||
if (context->slavelist[i].topology > 1)
|
||||
{
|
||||
dt1 = ecx_porttime(context, i,
|
||||
ecx_prevport(context, i, context->slavelist[i].entryport)) -
|
||||
ecx_porttime(context, i, context->slavelist[i].entryport);
|
||||
}
|
||||
/* we are only interested in positive difference */
|
||||
if (dt1 > dt3) dt1 = -dt1;
|
||||
/* current slave is not the first child of parent */
|
||||
/* previous child's delays need to be added */
|
||||
if ((child - parent) > 1)
|
||||
{
|
||||
dt2 = ecx_porttime(context, parent,
|
||||
ecx_prevport(context, parent, context->slavelist[i].parentport)) -
|
||||
ecx_porttime(context, parent, context->slavelist[parent].entryport);
|
||||
}
|
||||
if (dt2 < 0) dt2 = -dt2;
|
||||
|
||||
/* calculate current slave delay from delta times */
|
||||
/* assumption : forward delay equals return delay */
|
||||
context->slavelist[i].pdelay = ((dt3 - dt1) / 2) + dt2 +
|
||||
context->slavelist[parent].pdelay;
|
||||
ht = htoel(context->slavelist[i].pdelay);
|
||||
/* write propagation delay*/
|
||||
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
context->slavelist[i].DCrtA = 0;
|
||||
context->slavelist[i].DCrtB = 0;
|
||||
context->slavelist[i].DCrtC = 0;
|
||||
context->slavelist[i].DCrtD = 0;
|
||||
parent = context->slavelist[i].parent;
|
||||
/* if non DC slave found on first position on branch hold root parent */
|
||||
if ( (parent > 0) && (context->slavelist[parent].topology > 2))
|
||||
parenthold = parent;
|
||||
/* if branch has no DC slaves consume port on root parent */
|
||||
if ( parenthold && (context->slavelist[i].topology == 1))
|
||||
{
|
||||
ecx_parentport(context, parenthold);
|
||||
parenthold = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return context->slavelist[0].hasdc;
|
||||
}
|
||||
|
||||
#ifdef EC_VER1
|
||||
void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
|
||||
{
|
||||
ecx_dcsync0(&ecx_context, slave, act, CyclTime, CyclShift);
|
||||
}
|
||||
|
||||
void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
|
||||
{
|
||||
ecx_dcsync01(&ecx_context, slave, act, CyclTime0, CyclTime1, CyclShift);
|
||||
}
|
||||
|
||||
boolean ec_configdc(void)
|
||||
{
|
||||
return ecx_configdc(&ecx_context);
|
||||
}
|
||||
#endif
|
||||
Reference in New Issue
Block a user