feat(motor): 自适应 PDO 窗口与通道活动指示，新增原生 SOEM 时序诊断
后端 PDO 循环改为按在线槽收缩状态窗口，稀疏总线下单槽每周期都能刷新， 并补上尾部槽的发现扫描游标；新增自旋节拍源（阈值/显式覆盖）、P 核识别 与线程级钉核，自旋期间释放 GIL 以免 HTTP 线程被饿死；启动时先关停旧实例， 避免两个进程争用 Npcap 适配器。 记录每拍完整时间线、GC stop-the-world 事件与网格跳拍数，经 /api/motor/dbg 导出用于归因固定频率的超时；配
This commit is contained in:
@@ -0,0 +1,389 @@
|
||||
/*
|
||||
* Licensed under the GNU General Public License version 2 with exceptions. See
|
||||
* LICENSE file in the project root for full license information
|
||||
*/
|
||||
|
||||
/** \file
|
||||
* \brief
|
||||
* Servo over EtherCAT (SoE) Module.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include "osal.h"
|
||||
#include "oshw.h"
|
||||
#include "ethercattype.h"
|
||||
#include "ethercatbase.h"
|
||||
#include "ethercatmain.h"
|
||||
#include "ethercatsoe.h"
|
||||
|
||||
#define EC_SOE_MAX_DRIVES 8
|
||||
|
||||
/** SoE (Servo over EtherCAT) mailbox structure */
|
||||
PACKED_BEGIN
|
||||
typedef struct PACKED
|
||||
{
|
||||
ec_mbxheadert MbxHeader;
|
||||
uint8 opCode :3;
|
||||
uint8 incomplete :1;
|
||||
uint8 error :1;
|
||||
uint8 driveNo :3;
|
||||
uint8 elementflags;
|
||||
union
|
||||
{
|
||||
uint16 idn;
|
||||
uint16 fragmentsleft;
|
||||
};
|
||||
} ec_SoEt;
|
||||
PACKED_END
|
||||
|
||||
/** Report SoE error.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param[in] Slave = Slave number
|
||||
* @param[in] idn = IDN that generated error
|
||||
* @param[in] Error = Error code, see EtherCAT documentation for list
|
||||
*/
|
||||
void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error)
|
||||
{
|
||||
ec_errort Ec;
|
||||
|
||||
memset(&Ec, 0, sizeof(Ec));
|
||||
Ec.Time = osal_current_time();
|
||||
Ec.Slave = Slave;
|
||||
Ec.Index = idn;
|
||||
Ec.SubIdx = 0;
|
||||
*(context->ecaterror) = TRUE;
|
||||
Ec.Etype = EC_ERR_TYPE_SOE_ERROR;
|
||||
Ec.ErrorCode = Error;
|
||||
ecx_pusherror(context, &Ec);
|
||||
}
|
||||
|
||||
/** SoE read, blocking.
|
||||
*
|
||||
* The IDN object of the selected slave and DriveNo is read. If a response
|
||||
* is larger than the mailbox size then the response is segmented. The function
|
||||
* will combine all segments and copy them to the parameter buffer.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param[in] slave = Slave number
|
||||
* @param[in] driveNo = Drive number in slave
|
||||
* @param[in] elementflags = Flags to select what properties of IDN are to be transferred.
|
||||
* @param[in] idn = IDN.
|
||||
* @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SoE.
|
||||
* @param[out] p = Pointer to parameter buffer
|
||||
* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
|
||||
* @return Workcounter from last slave response
|
||||
*/
|
||||
int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
|
||||
{
|
||||
ec_SoEt *SoEp, *aSoEp;
|
||||
int totalsize, framedatasize;
|
||||
int wkc;
|
||||
uint8 *bp;
|
||||
uint8 *mp;
|
||||
uint16 *errorcode;
|
||||
ec_mbxbuft MbxIn, MbxOut;
|
||||
uint8 cnt;
|
||||
boolean NotLast;
|
||||
|
||||
ec_clearmbx(&MbxIn);
|
||||
/* Empty slave out mailbox if something is in. Timeout set to 0 */
|
||||
wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
|
||||
ec_clearmbx(&MbxOut);
|
||||
aSoEp = (ec_SoEt *)&MbxIn;
|
||||
SoEp = (ec_SoEt *)&MbxOut;
|
||||
SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
|
||||
SoEp->MbxHeader.address = htoes(0x0000);
|
||||
SoEp->MbxHeader.priority = 0x00;
|
||||
/* get new mailbox count value, used as session handle */
|
||||
cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
|
||||
context->slavelist[slave].mbx_cnt = cnt;
|
||||
SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + MBX_HDR_SET_CNT(cnt); /* SoE */
|
||||
SoEp->opCode = ECT_SOE_READREQ;
|
||||
SoEp->incomplete = 0;
|
||||
SoEp->error = 0;
|
||||
SoEp->driveNo = driveNo;
|
||||
SoEp->elementflags = elementflags;
|
||||
SoEp->idn = htoes(idn);
|
||||
totalsize = 0;
|
||||
bp = p;
|
||||
mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
|
||||
NotLast = TRUE;
|
||||
/* send SoE request to slave */
|
||||
wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
|
||||
if (wkc > 0) /* succeeded to place mailbox in slave ? */
|
||||
{
|
||||
while (NotLast)
|
||||
{
|
||||
/* clean mailboxbuffer */
|
||||
ec_clearmbx(&MbxIn);
|
||||
/* read slave response */
|
||||
wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
|
||||
if (wkc > 0) /* succeeded to read slave response ? */
|
||||
{
|
||||
/* slave response should be SoE, ReadRes */
|
||||
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
|
||||
(aSoEp->opCode == ECT_SOE_READRES) &&
|
||||
(aSoEp->error == 0) &&
|
||||
(aSoEp->driveNo == driveNo) &&
|
||||
(aSoEp->elementflags == elementflags))
|
||||
{
|
||||
framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert);
|
||||
totalsize += framedatasize;
|
||||
/* Does parameter fit in parameter buffer ? */
|
||||
if (totalsize <= *psize)
|
||||
{
|
||||
/* copy parameter data in parameter buffer */
|
||||
memcpy(bp, mp, framedatasize);
|
||||
/* increment buffer pointer */
|
||||
bp += framedatasize;
|
||||
}
|
||||
else
|
||||
{
|
||||
framedatasize -= totalsize - *psize;
|
||||
totalsize = *psize;
|
||||
/* copy parameter data in parameter buffer */
|
||||
if (framedatasize > 0) memcpy(bp, mp, framedatasize);
|
||||
}
|
||||
|
||||
if (!aSoEp->incomplete)
|
||||
{
|
||||
NotLast = FALSE;
|
||||
*psize = totalsize;
|
||||
}
|
||||
}
|
||||
/* other slave response */
|
||||
else
|
||||
{
|
||||
NotLast = FALSE;
|
||||
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
|
||||
(aSoEp->opCode == ECT_SOE_READRES) &&
|
||||
(aSoEp->error == 1))
|
||||
{
|
||||
mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
|
||||
errorcode = (uint16 *)mp;
|
||||
ecx_SoEerror(context, slave, idn, *errorcode);
|
||||
}
|
||||
else
|
||||
{
|
||||
ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
|
||||
}
|
||||
wkc = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
NotLast = FALSE;
|
||||
ecx_packeterror(context, slave, idn, 0, 4); /* no response */
|
||||
}
|
||||
}
|
||||
}
|
||||
return wkc;
|
||||
}
|
||||
|
||||
/** SoE write, blocking.
|
||||
*
|
||||
* The IDN object of the selected slave and DriveNo is written. If a response
|
||||
* is larger than the mailbox size then the response is segmented.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param[in] slave = Slave number
|
||||
* @param[in] driveNo = Drive number in slave
|
||||
* @param[in] elementflags = Flags to select what properties of IDN are to be transferred.
|
||||
* @param[in] idn = IDN.
|
||||
* @param[in] psize = Size in bytes of parameter buffer.
|
||||
* @param[out] p = Pointer to parameter buffer
|
||||
* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
|
||||
* @return Workcounter from last slave response
|
||||
*/
|
||||
int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
|
||||
{
|
||||
ec_SoEt *SoEp, *aSoEp;
|
||||
int framedatasize, maxdata;
|
||||
int wkc;
|
||||
uint8 *mp;
|
||||
uint8 *hp;
|
||||
uint16 *errorcode;
|
||||
ec_mbxbuft MbxIn, MbxOut;
|
||||
uint8 cnt;
|
||||
boolean NotLast;
|
||||
|
||||
ec_clearmbx(&MbxIn);
|
||||
/* Empty slave out mailbox if something is in. Timeout set to 0 */
|
||||
wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
|
||||
ec_clearmbx(&MbxOut);
|
||||
aSoEp = (ec_SoEt *)&MbxIn;
|
||||
SoEp = (ec_SoEt *)&MbxOut;
|
||||
SoEp->MbxHeader.address = htoes(0x0000);
|
||||
SoEp->MbxHeader.priority = 0x00;
|
||||
SoEp->opCode = ECT_SOE_WRITEREQ;
|
||||
SoEp->error = 0;
|
||||
SoEp->driveNo = driveNo;
|
||||
SoEp->elementflags = elementflags;
|
||||
hp = p;
|
||||
mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
|
||||
maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt);
|
||||
NotLast = TRUE;
|
||||
while (NotLast)
|
||||
{
|
||||
framedatasize = psize;
|
||||
NotLast = FALSE;
|
||||
SoEp->idn = htoes(idn);
|
||||
SoEp->incomplete = 0;
|
||||
if (framedatasize > maxdata)
|
||||
{
|
||||
framedatasize = maxdata; /* segmented transfer needed */
|
||||
NotLast = TRUE;
|
||||
SoEp->incomplete = 1;
|
||||
SoEp->fragmentsleft = (uint16)(psize / maxdata);
|
||||
}
|
||||
SoEp->MbxHeader.length = htoes((uint16)(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize));
|
||||
/* get new mailbox counter, used for session handle */
|
||||
cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
|
||||
context->slavelist[slave].mbx_cnt = cnt;
|
||||
SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + MBX_HDR_SET_CNT(cnt); /* SoE */
|
||||
/* copy parameter data to mailbox */
|
||||
memcpy(mp, hp, framedatasize);
|
||||
hp += framedatasize;
|
||||
psize -= framedatasize;
|
||||
/* send SoE request to slave */
|
||||
wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
|
||||
if (wkc > 0) /* succeeded to place mailbox in slave ? */
|
||||
{
|
||||
if (!NotLast || !ecx_mbxempty(context, slave, timeout))
|
||||
{
|
||||
/* clean mailboxbuffer */
|
||||
ec_clearmbx(&MbxIn);
|
||||
/* read slave response */
|
||||
wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
|
||||
if (wkc > 0) /* succeeded to read slave response ? */
|
||||
{
|
||||
NotLast = FALSE;
|
||||
/* slave response should be SoE, WriteRes */
|
||||
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
|
||||
(aSoEp->opCode == ECT_SOE_WRITERES) &&
|
||||
(aSoEp->error == 0) &&
|
||||
(aSoEp->driveNo == driveNo) &&
|
||||
(aSoEp->elementflags == elementflags))
|
||||
{
|
||||
/* SoE write succeeded */
|
||||
}
|
||||
/* other slave response */
|
||||
else
|
||||
{
|
||||
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
|
||||
(aSoEp->opCode == ECT_SOE_READRES) &&
|
||||
(aSoEp->error == 1))
|
||||
{
|
||||
mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
|
||||
errorcode = (uint16 *)mp;
|
||||
ecx_SoEerror(context, slave, idn, *errorcode);
|
||||
}
|
||||
else
|
||||
{
|
||||
ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
|
||||
}
|
||||
wkc = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ecx_packeterror(context, slave, idn, 0, 4); /* no response */
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return wkc;
|
||||
}
|
||||
|
||||
/** SoE read AT and MTD mapping.
|
||||
*
|
||||
* SoE has standard indexes defined for mapping. This function
|
||||
* tries to read them and collect a full input and output mapping size
|
||||
* of designated slave.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param[in] slave = Slave number
|
||||
* @param[out] Osize = Size in bits of output mapping (MTD) found
|
||||
* @param[out] Isize = Size in bits of input mapping (AT) found
|
||||
* @return >0 if mapping successful.
|
||||
*/
|
||||
int ecx_readIDNmap(ecx_contextt *context, uint16 slave, uint32 *Osize, uint32 *Isize)
|
||||
{
|
||||
int retVal = 0;
|
||||
int wkc;
|
||||
int psize;
|
||||
uint8 driveNr;
|
||||
uint16 entries, itemcount;
|
||||
ec_SoEmappingt SoEmapping;
|
||||
ec_SoEattributet SoEattribute;
|
||||
|
||||
*Isize = 0;
|
||||
*Osize = 0;
|
||||
for(driveNr = 0; driveNr < EC_SOE_MAX_DRIVES; driveNr++)
|
||||
{
|
||||
psize = sizeof(SoEmapping);
|
||||
/* read output mapping via SoE */
|
||||
wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
|
||||
if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
|
||||
{
|
||||
/* command word (uint16) is always mapped but not in list */
|
||||
*Osize += 16;
|
||||
for (itemcount = 0 ; itemcount < entries ; itemcount++)
|
||||
{
|
||||
psize = sizeof(SoEattribute);
|
||||
/* read attribute of each IDN in mapping list */
|
||||
wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
|
||||
if ((wkc > 0) && (!SoEattribute.list))
|
||||
{
|
||||
/* length : 0 = 8bit, 1 = 16bit .... */
|
||||
*Osize += (int)8 << SoEattribute.length;
|
||||
}
|
||||
}
|
||||
}
|
||||
psize = sizeof(SoEmapping);
|
||||
/* read input mapping via SoE */
|
||||
wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
|
||||
if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
|
||||
{
|
||||
/* status word (uint16) is always mapped but not in list */
|
||||
*Isize += 16;
|
||||
for (itemcount = 0 ; itemcount < entries ; itemcount++)
|
||||
{
|
||||
psize = sizeof(SoEattribute);
|
||||
/* read attribute of each IDN in mapping list */
|
||||
wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
|
||||
if ((wkc > 0) && (!SoEattribute.list))
|
||||
{
|
||||
/* length : 0 = 8bit, 1 = 16bit .... */
|
||||
*Isize += (int)8 << SoEattribute.length;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* found some I/O bits ? */
|
||||
if ((*Isize > 0) || (*Osize > 0))
|
||||
{
|
||||
retVal = 1;
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
#ifdef EC_VER1
|
||||
int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
|
||||
{
|
||||
return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
|
||||
}
|
||||
|
||||
int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
|
||||
{
|
||||
return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
|
||||
}
|
||||
|
||||
int ec_readIDNmap(uint16 slave, uint32 *Osize, uint32 *Isize)
|
||||
{
|
||||
return ecx_readIDNmap(&ecx_context, slave, Osize, Isize);
|
||||
}
|
||||
#endif
|
||||
Reference in New Issue
Block a user