feat(motor): 自适应 PDO 窗口与通道活动指示，新增原生 SOEM 时序诊断
后端 PDO 循环改为按在线槽收缩状态窗口，稀疏总线下单槽每周期都能刷新， 并补上尾部槽的发现扫描游标；新增自旋节拍源（阈值/显式覆盖）、P 核识别 与线程级钉核，自旋期间释放 GIL 以免 HTTP 线程被饿死；启动时先关停旧实例， 避免两个进程争用 Npcap 适配器。 记录每拍完整时间线、GC stop-the-world 事件与网格跳拍数，经 /api/motor/dbg 导出用于归因固定频率的超时；配
This commit is contained in:
@@ -0,0 +1,327 @@
|
||||
/** \file
|
||||
* \brief Example code for Simple Open EtherCAT master
|
||||
*
|
||||
* Usage: simple_ng IFNAME1
|
||||
* IFNAME1 is the NIC interface name, e.g. 'eth0'
|
||||
*
|
||||
* This is a minimal test.
|
||||
*/
|
||||
|
||||
#include "ethercat.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
typedef struct {
|
||||
ecx_contextt context;
|
||||
char * iface;
|
||||
uint8 group;
|
||||
int roundtrip_time;
|
||||
|
||||
/* Used by the context */
|
||||
uint8 map[4096];
|
||||
ecx_portt port;
|
||||
ec_slavet slavelist[EC_MAXSLAVE];
|
||||
int slavecount;
|
||||
ec_groupt grouplist[EC_MAXGROUP];
|
||||
uint8 esibuf[EC_MAXEEPBUF];
|
||||
uint32 esimap[EC_MAXEEPBITMAP];
|
||||
ec_eringt elist;
|
||||
ec_idxstackT idxstack;
|
||||
boolean ecaterror;
|
||||
int64 DCtime;
|
||||
ec_SMcommtypet SMcommtype[EC_MAX_MAPT];
|
||||
ec_PDOassignt PDOassign[EC_MAX_MAPT];
|
||||
ec_PDOdesct PDOdesc[EC_MAX_MAPT];
|
||||
ec_eepromSMt eepSM;
|
||||
ec_eepromFMMUt eepFMMU;
|
||||
} Fieldbus;
|
||||
|
||||
|
||||
static void
|
||||
fieldbus_initialize(Fieldbus *fieldbus, char *iface)
|
||||
{
|
||||
ecx_contextt *context;
|
||||
|
||||
/* Let's start by 0-filling `fieldbus` to avoid surprises */
|
||||
memset(fieldbus, 0, sizeof(*fieldbus));
|
||||
|
||||
fieldbus->iface = iface;
|
||||
fieldbus->group = 0;
|
||||
fieldbus->roundtrip_time = 0;
|
||||
fieldbus->ecaterror = FALSE;
|
||||
|
||||
/* Initialize the ecx_contextt data structure */
|
||||
context = &fieldbus->context;
|
||||
context->port = &fieldbus->port;
|
||||
context->slavelist = fieldbus->slavelist;
|
||||
context->slavecount = &fieldbus->slavecount;
|
||||
context->maxslave = EC_MAXSLAVE;
|
||||
context->grouplist = fieldbus->grouplist;
|
||||
context->maxgroup = EC_MAXGROUP;
|
||||
context->esibuf = fieldbus->esibuf;
|
||||
context->esimap = fieldbus->esimap;
|
||||
context->esislave = 0;
|
||||
context->elist = &fieldbus->elist;
|
||||
context->idxstack = &fieldbus->idxstack;
|
||||
context->ecaterror = &fieldbus->ecaterror;
|
||||
context->DCtime = &fieldbus->DCtime;
|
||||
context->SMcommtype = fieldbus->SMcommtype;
|
||||
context->PDOassign = fieldbus->PDOassign;
|
||||
context->PDOdesc = fieldbus->PDOdesc;
|
||||
context->eepSM = &fieldbus->eepSM;
|
||||
context->eepFMMU = &fieldbus->eepFMMU;
|
||||
context->FOEhook = NULL;
|
||||
context->EOEhook = NULL;
|
||||
context->manualstatechange = 0;
|
||||
}
|
||||
|
||||
static int
|
||||
fieldbus_roundtrip(Fieldbus *fieldbus)
|
||||
{
|
||||
ecx_contextt *context;
|
||||
ec_timet start, end, diff;
|
||||
int wkc;
|
||||
|
||||
context = &fieldbus->context;
|
||||
|
||||
start = osal_current_time();
|
||||
ecx_send_processdata(context);
|
||||
wkc = ecx_receive_processdata(context, EC_TIMEOUTRET);
|
||||
end = osal_current_time();
|
||||
osal_time_diff(&start, &end, &diff);
|
||||
fieldbus->roundtrip_time = diff.sec * 1000000 + diff.usec;
|
||||
|
||||
return wkc;
|
||||
}
|
||||
|
||||
static boolean
|
||||
fieldbus_start(Fieldbus *fieldbus)
|
||||
{
|
||||
ecx_contextt *context;
|
||||
ec_groupt *grp;
|
||||
ec_slavet *slave;
|
||||
int i;
|
||||
|
||||
context = &fieldbus->context;
|
||||
grp = fieldbus->grouplist + fieldbus->group;
|
||||
|
||||
printf("Initializing SOEM on '%s'... ", fieldbus->iface);
|
||||
if (! ecx_init(context, fieldbus->iface)) {
|
||||
printf("no socket connection\n");
|
||||
return FALSE;
|
||||
}
|
||||
printf("done\n");
|
||||
|
||||
printf("Finding autoconfig slaves... ");
|
||||
if (ecx_config_init(context, FALSE) <= 0) {
|
||||
printf("no slaves found\n");
|
||||
return FALSE;
|
||||
}
|
||||
printf("%d slaves found\n", fieldbus->slavecount);
|
||||
|
||||
printf("Sequential mapping of I/O... ");
|
||||
ecx_config_map_group(context, fieldbus->map, fieldbus->group);
|
||||
printf("mapped %dO+%dI bytes from %d segments",
|
||||
grp->Obytes, grp->Ibytes, grp->nsegments);
|
||||
if (grp->nsegments > 1) {
|
||||
/* Show how slaves are distrubuted */
|
||||
for (i = 0; i < grp->nsegments; ++i) {
|
||||
printf("%s%d", i == 0 ? " (" : "+", grp->IOsegment[i]);
|
||||
}
|
||||
printf(" slaves)");
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
printf("Configuring distributed clock... ");
|
||||
ecx_configdc(context);
|
||||
printf("done\n");
|
||||
|
||||
printf("Waiting for all slaves in safe operational... ");
|
||||
ecx_statecheck(context, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
|
||||
printf("done\n");
|
||||
|
||||
printf("Send a roundtrip to make outputs in slaves happy... ");
|
||||
fieldbus_roundtrip(fieldbus);
|
||||
printf("done\n");
|
||||
|
||||
printf("Setting operational state..");
|
||||
/* Act on slave 0 (a virtual slave used for broadcasting) */
|
||||
slave = fieldbus->slavelist;
|
||||
slave->state = EC_STATE_OPERATIONAL;
|
||||
ecx_writestate(context, 0);
|
||||
/* Poll the result ten times before giving up */
|
||||
for (i = 0; i < 10; ++i) {
|
||||
printf(".");
|
||||
fieldbus_roundtrip(fieldbus);
|
||||
ecx_statecheck(context, 0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE / 10);
|
||||
if (slave->state == EC_STATE_OPERATIONAL) {
|
||||
printf(" all slaves are now operational\n");
|
||||
return TRUE;
|
||||
}
|
||||
}
|
||||
|
||||
printf(" failed,");
|
||||
ecx_readstate(context);
|
||||
for (i = 1; i <= fieldbus->slavecount; ++i) {
|
||||
slave = fieldbus->slavelist + i;
|
||||
if (slave->state != EC_STATE_OPERATIONAL) {
|
||||
printf(" slave %d is 0x%04X (AL-status=0x%04X %s)",
|
||||
i, slave->state, slave->ALstatuscode,
|
||||
ec_ALstatuscode2string(slave->ALstatuscode));
|
||||
}
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
static void
|
||||
fieldbus_stop(Fieldbus *fieldbus)
|
||||
{
|
||||
ecx_contextt *context;
|
||||
ec_slavet *slave;
|
||||
|
||||
context = &fieldbus->context;
|
||||
/* Act on slave 0 (a virtual slave used for broadcasting) */
|
||||
slave = fieldbus->slavelist;
|
||||
|
||||
printf("Requesting init state on all slaves... ");
|
||||
slave->state = EC_STATE_INIT;
|
||||
ecx_writestate(context, 0);
|
||||
printf("done\n");
|
||||
|
||||
printf("Close socket... ");
|
||||
ecx_close(context);
|
||||
printf("done\n");
|
||||
}
|
||||
|
||||
static boolean
|
||||
fieldbus_dump(Fieldbus *fieldbus)
|
||||
{
|
||||
ec_groupt *grp;
|
||||
uint32 n;
|
||||
int wkc, expected_wkc;
|
||||
|
||||
grp = fieldbus->grouplist + fieldbus->group;
|
||||
|
||||
wkc = fieldbus_roundtrip(fieldbus);
|
||||
expected_wkc = grp->outputsWKC * 2 + grp->inputsWKC;
|
||||
printf("%6d usec WKC %d", fieldbus->roundtrip_time, wkc);
|
||||
if (wkc < expected_wkc) {
|
||||
printf(" wrong (expected %d)\n", expected_wkc);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
printf(" O:");
|
||||
for (n = 0; n < grp->Obytes; ++n) {
|
||||
printf(" %02X", grp->outputs[n]);
|
||||
}
|
||||
printf(" I:");
|
||||
for (n = 0; n < grp->Ibytes; ++n) {
|
||||
printf(" %02X", grp->inputs[n]);
|
||||
}
|
||||
printf(" T: %lld\r", (long long) fieldbus->DCtime);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void
|
||||
fieldbus_check_state(Fieldbus *fieldbus)
|
||||
{
|
||||
ecx_contextt *context;
|
||||
ec_groupt *grp;
|
||||
ec_slavet *slave;
|
||||
int i;
|
||||
|
||||
context = &fieldbus->context;
|
||||
grp = context->grouplist + fieldbus->group;
|
||||
grp->docheckstate = FALSE;
|
||||
ecx_readstate(context);
|
||||
for (i = 1; i <= fieldbus->slavecount; ++i) {
|
||||
slave = context->slavelist + i;
|
||||
if (slave->group != fieldbus->group) {
|
||||
/* This slave is part of another group: do nothing */
|
||||
} else if (slave->state != EC_STATE_OPERATIONAL) {
|
||||
grp->docheckstate = TRUE;
|
||||
if (slave->state == EC_STATE_SAFE_OP + EC_STATE_ERROR) {
|
||||
printf("* Slave %d is in SAFE_OP+ERROR, attempting ACK\n", i);
|
||||
slave->state = EC_STATE_SAFE_OP + EC_STATE_ACK;
|
||||
ecx_writestate(context, i);
|
||||
} else if(slave->state == EC_STATE_SAFE_OP) {
|
||||
printf("* Slave %d is in SAFE_OP, change to OPERATIONAL\n", i);
|
||||
slave->state = EC_STATE_OPERATIONAL;
|
||||
ecx_writestate(context, i);
|
||||
} else if(slave->state > EC_STATE_NONE) {
|
||||
if (ecx_reconfig_slave(context, i, EC_TIMEOUTRET)) {
|
||||
slave->islost = FALSE;
|
||||
printf("* Slave %d reconfigured\n", i);
|
||||
}
|
||||
} else if(! slave->islost) {
|
||||
ecx_statecheck(context, i, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
|
||||
if (slave->state == EC_STATE_NONE) {
|
||||
slave->islost = TRUE;
|
||||
printf("* Slave %d lost\n", i);
|
||||
}
|
||||
}
|
||||
} else if (slave->islost) {
|
||||
if(slave->state != EC_STATE_NONE) {
|
||||
slave->islost = FALSE;
|
||||
printf("* Slave %d found\n", i);
|
||||
} else if (ecx_recover_slave(context, i, EC_TIMEOUTRET)) {
|
||||
slave->islost = FALSE;
|
||||
printf("* Slave %d recovered\n", i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (! grp->docheckstate) {
|
||||
printf("All slaves resumed OPERATIONAL\n");
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
main(int argc, char *argv[])
|
||||
{
|
||||
Fieldbus fieldbus;
|
||||
|
||||
if (argc != 2) {
|
||||
ec_adaptert * adapter = NULL;
|
||||
ec_adaptert * head = NULL;
|
||||
printf("Usage: simple_ng IFNAME1\n"
|
||||
"IFNAME1 is the NIC interface name, e.g. 'eth0'\n");
|
||||
|
||||
printf("\nAvailable adapters:\n");
|
||||
head = adapter = ec_find_adapters();
|
||||
while (adapter != NULL)
|
||||
{
|
||||
printf(" - %s (%s)\n", adapter->name, adapter->desc);
|
||||
adapter = adapter->next;
|
||||
}
|
||||
ec_free_adapters(head);
|
||||
return 1;
|
||||
}
|
||||
|
||||
fieldbus_initialize(&fieldbus, argv[1]);
|
||||
if (fieldbus_start(&fieldbus)) {
|
||||
int i, min_time, max_time;
|
||||
min_time = max_time = 0;
|
||||
for (i = 1; i <= 10000; ++i) {
|
||||
printf("Iteration %4d:", i);
|
||||
if (! fieldbus_dump(&fieldbus)) {
|
||||
fieldbus_check_state(&fieldbus);
|
||||
} else if (i == 1) {
|
||||
min_time = max_time = fieldbus.roundtrip_time;
|
||||
} else if (fieldbus.roundtrip_time < min_time) {
|
||||
min_time = fieldbus.roundtrip_time;
|
||||
} else if (fieldbus.roundtrip_time > max_time) {
|
||||
max_time = fieldbus.roundtrip_time;
|
||||
}
|
||||
osal_usleep(5000);
|
||||
}
|
||||
printf("\nRoundtrip time (usec): min %d max %d\n", min_time, max_time);
|
||||
fieldbus_stop(&fieldbus);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user