feat(motor): 自适应 PDO 窗口与通道活动指示，新增原生 SOEM 时序诊断
后端 PDO 循环改为按在线槽收缩状态窗口，稀疏总线下单槽每周期都能刷新， 并补上尾部槽的发现扫描游标；新增自旋节拍源（阈值/显式覆盖）、P 核识别 与线程级钉核，自旋期间释放 GIL 以免 HTTP 线程被饿死；启动时先关停旧实例， 避免两个进程争用 Npcap 适配器。 记录每拍完整时间线、GC stop-the-world 事件与网格跳拍数，经 /api/motor/dbg 导出用于归因固定频率的超时；配
This commit is contained in:
@@ -0,0 +1,260 @@
|
||||
/** \file
|
||||
* \brief Example code for Simple Open EtherCAT master
|
||||
*
|
||||
* Usage : simple_test [ifname1]
|
||||
* ifname is NIC interface, f.e. eth0
|
||||
*
|
||||
* This is a minimal test.
|
||||
*
|
||||
* (c)Arthur Ketels 2010 - 2011
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "ethercat.h"
|
||||
|
||||
#define EC_TIMEOUTMON 500
|
||||
|
||||
char IOmap[4096];
|
||||
OSAL_THREAD_HANDLE thread1;
|
||||
int expectedWKC;
|
||||
boolean needlf;
|
||||
volatile int wkc;
|
||||
boolean inOP;
|
||||
uint8 currentgroup = 0;
|
||||
boolean forceByteAlignment = FALSE;
|
||||
|
||||
void simpletest(char *ifname)
|
||||
{
|
||||
int i, j, oloop, iloop, chk;
|
||||
needlf = FALSE;
|
||||
inOP = FALSE;
|
||||
|
||||
printf("Starting simple test\n");
|
||||
|
||||
/* initialise SOEM, bind socket to ifname */
|
||||
if (ec_init(ifname))
|
||||
{
|
||||
printf("ec_init on %s succeeded.\n",ifname);
|
||||
/* find and auto-config slaves */
|
||||
|
||||
|
||||
if ( ec_config_init(FALSE) > 0 )
|
||||
{
|
||||
printf("%d slaves found and configured.\n",ec_slavecount);
|
||||
|
||||
if (forceByteAlignment)
|
||||
{
|
||||
ec_config_map_aligned(&IOmap);
|
||||
}
|
||||
else
|
||||
{
|
||||
ec_config_map(&IOmap);
|
||||
}
|
||||
|
||||
ec_configdc();
|
||||
|
||||
printf("Slaves mapped, state to SAFE_OP.\n");
|
||||
/* wait for all slaves to reach SAFE_OP state */
|
||||
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
|
||||
|
||||
oloop = ec_slave[0].Obytes;
|
||||
if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
|
||||
if (oloop > 8) oloop = 8;
|
||||
iloop = ec_slave[0].Ibytes;
|
||||
if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
|
||||
if (iloop > 8) iloop = 8;
|
||||
|
||||
printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);
|
||||
|
||||
printf("Request operational state for all slaves\n");
|
||||
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
|
||||
printf("Calculated workcounter %d\n", expectedWKC);
|
||||
ec_slave[0].state = EC_STATE_OPERATIONAL;
|
||||
/* send one valid process data to make outputs in slaves happy*/
|
||||
ec_send_processdata();
|
||||
ec_receive_processdata(EC_TIMEOUTRET);
|
||||
/* request OP state for all slaves */
|
||||
ec_writestate(0);
|
||||
chk = 200;
|
||||
/* wait for all slaves to reach OP state */
|
||||
do
|
||||
{
|
||||
ec_send_processdata();
|
||||
ec_receive_processdata(EC_TIMEOUTRET);
|
||||
ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
|
||||
}
|
||||
while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
|
||||
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
|
||||
{
|
||||
printf("Operational state reached for all slaves.\n");
|
||||
inOP = TRUE;
|
||||
/* cyclic loop */
|
||||
for(i = 1; i <= 10000; i++)
|
||||
{
|
||||
ec_send_processdata();
|
||||
wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
||||
|
||||
if(wkc >= expectedWKC)
|
||||
{
|
||||
printf("Processdata cycle %4d, WKC %d , O:", i, wkc);
|
||||
|
||||
for(j = 0 ; j < oloop; j++)
|
||||
{
|
||||
printf(" %2.2x", *(ec_slave[0].outputs + j));
|
||||
}
|
||||
|
||||
printf(" I:");
|
||||
for(j = 0 ; j < iloop; j++)
|
||||
{
|
||||
printf(" %2.2x", *(ec_slave[0].inputs + j));
|
||||
}
|
||||
printf(" T:%"PRId64"\r",ec_DCtime);
|
||||
needlf = TRUE;
|
||||
}
|
||||
osal_usleep(5000);
|
||||
|
||||
}
|
||||
inOP = FALSE;
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Not all slaves reached operational state.\n");
|
||||
ec_readstate();
|
||||
for(i = 1; i<=ec_slavecount ; i++)
|
||||
{
|
||||
if(ec_slave[i].state != EC_STATE_OPERATIONAL)
|
||||
{
|
||||
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
|
||||
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
|
||||
}
|
||||
}
|
||||
}
|
||||
printf("\nRequest init state for all slaves\n");
|
||||
ec_slave[0].state = EC_STATE_INIT;
|
||||
/* request INIT state for all slaves */
|
||||
ec_writestate(0);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("No slaves found!\n");
|
||||
}
|
||||
printf("End simple test, close socket\n");
|
||||
/* stop SOEM, close socket */
|
||||
ec_close();
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("No socket connection on %s\nExecute as root\n",ifname);
|
||||
}
|
||||
}
|
||||
|
||||
OSAL_THREAD_FUNC ecatcheck( void *ptr )
|
||||
{
|
||||
int slave;
|
||||
(void)ptr; /* Not used */
|
||||
|
||||
while(1)
|
||||
{
|
||||
if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate))
|
||||
{
|
||||
if (needlf)
|
||||
{
|
||||
needlf = FALSE;
|
||||
printf("\n");
|
||||
}
|
||||
/* one ore more slaves are not responding */
|
||||
ec_group[currentgroup].docheckstate = FALSE;
|
||||
ec_readstate();
|
||||
for (slave = 1; slave <= ec_slavecount; slave++)
|
||||
{
|
||||
if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
|
||||
{
|
||||
ec_group[currentgroup].docheckstate = TRUE;
|
||||
if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
|
||||
{
|
||||
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
|
||||
ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
|
||||
ec_writestate(slave);
|
||||
}
|
||||
else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
|
||||
{
|
||||
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
|
||||
ec_slave[slave].state = EC_STATE_OPERATIONAL;
|
||||
ec_writestate(slave);
|
||||
}
|
||||
else if(ec_slave[slave].state > EC_STATE_NONE)
|
||||
{
|
||||
if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
|
||||
{
|
||||
ec_slave[slave].islost = FALSE;
|
||||
printf("MESSAGE : slave %d reconfigured\n",slave);
|
||||
}
|
||||
}
|
||||
else if(!ec_slave[slave].islost)
|
||||
{
|
||||
/* re-check state */
|
||||
ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
|
||||
if (ec_slave[slave].state == EC_STATE_NONE)
|
||||
{
|
||||
ec_slave[slave].islost = TRUE;
|
||||
printf("ERROR : slave %d lost\n",slave);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (ec_slave[slave].islost)
|
||||
{
|
||||
if(ec_slave[slave].state == EC_STATE_NONE)
|
||||
{
|
||||
if (ec_recover_slave(slave, EC_TIMEOUTMON))
|
||||
{
|
||||
ec_slave[slave].islost = FALSE;
|
||||
printf("MESSAGE : slave %d recovered\n",slave);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ec_slave[slave].islost = FALSE;
|
||||
printf("MESSAGE : slave %d found\n",slave);
|
||||
}
|
||||
}
|
||||
}
|
||||
if(!ec_group[currentgroup].docheckstate)
|
||||
printf("OK : all slaves resumed OPERATIONAL.\n");
|
||||
}
|
||||
osal_usleep(10000);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
|
||||
|
||||
if (argc > 1)
|
||||
{
|
||||
/* create thread to handle slave error handling in OP */
|
||||
osal_thread_create(&thread1, 128000, &ecatcheck, NULL);
|
||||
/* start cyclic part */
|
||||
simpletest(argv[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
ec_adaptert * adapter = NULL;
|
||||
ec_adaptert * head = NULL;
|
||||
printf("Usage: simple_test ifname1\nifname = eth0 for example\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);
|
||||
}
|
||||
|
||||
printf("End program\n");
|
||||
return (0);
|
||||
}
|
||||
Reference in New Issue
Block a user