cbb
79633a2b0f
后端 PDO 循环改为按在线槽收缩状态窗口，稀疏总线下单槽每周期都能刷新， 并补上尾部槽的发现扫描游标；新增自旋节拍源（阈值/显式覆盖）、P 核识别 与线程级钉核，自旋期间释放 GIL 以免 HTTP 线程被饿死；启动时先关停旧实例， 避免两个进程争用 Npcap 适配器。 记录每拍完整时间线、GC stop-the-world 事件与网格跳拍数，经 /api/motor/dbg 导出用于归因固定频率的超时；配
398 lines
16 KiB
C
398 lines
16 KiB
C
/* ecat_native.c — 原型 A：原生 PDO 守护进程（SOEM 直用，无 Python/GIL）
|
||
*
|
||
* 目的：验证"换语言"能把 PDO 循环的天花板从 Python 的 ~5kHz 抬到多高、
|
||
* 尾抖动能多干净。总线核心收发本来就是 C（pysoem 只是 SOEM 的包装），
|
||
* 这里把热循环从解释器里拿出来，其余一切不动。
|
||
*
|
||
* 契约：复刻 ecat_server.py 的 3 条热路由（POST /api/motor/state|period|dbg），
|
||
* 只绑 127.0.0.1、无 CORS —— 与 Python 服务同口径，cadence_diag/rate_probe
|
||
* 直接 --base http://127.0.0.1:8091 就能打过来。其余路由仍留在 Python。
|
||
*
|
||
* 时间线元组与 Python 完全同构：(t0, ts, tm, tr, te, k)
|
||
* t0=循环顶 ts=send 前(原生无锁等待，≈t0) tm=send 后 tr=receive 后
|
||
* te=循环底(跳拍判定时刻) k=本拍网格跳过数
|
||
* 时刻用 QueryPerformanceCounter 秒 —— 与 Python perf_counter 同一时间基
|
||
* （实测差 ≤6µs），cadence_diag 的差分口径不变。
|
||
*/
|
||
#include <winsock2.h>
|
||
#include <ws2tcpip.h>
|
||
#include <windows.h>
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include <math.h>
|
||
#include "ethercat.h"
|
||
|
||
#define RING 2048 /* 与 Python _cyc_t maxlen 同 */
|
||
#define IO_MAP_SIZE 4096
|
||
#define STATS_N 400 /* state 里 cyc_us 分位数的样本窗口，同 _cyc_stats(n=400) */
|
||
|
||
/* ── 计时原语 ── */
|
||
static double g_qpc2s;
|
||
static inline double now_s(void)
|
||
{
|
||
LARGE_INTEGER c;
|
||
QueryPerformanceCounter(&c);
|
||
return (double)c.QuadPart * g_qpc2s;
|
||
}
|
||
static inline void spin_until(double deadline)
|
||
{
|
||
while (now_s() < deadline)
|
||
YieldProcessor(); /* PAUSE：SMT 兄弟友好，不烧功耗 */
|
||
}
|
||
|
||
/* ── 共享状态 ── */
|
||
typedef struct { double t0, ts, tm, tr, te; int k; } beat_t;
|
||
|
||
static beat_t g_ring[RING];
|
||
static int g_ring_i; /* 写指针：仅 PDO 线程动 */
|
||
static long long g_total; /* 累计拍数 */
|
||
static long long g_skip_total; /* 累计网格跳过拍 */
|
||
static CRITICAL_SECTION g_ring_lock; /* 保护 ring/total/skip 的读侧 */
|
||
static CRITICAL_SECTION g_cfg_lock; /* 保护 g_period_ms */
|
||
static double g_period_ms = 0.25;
|
||
static volatile LONG g_stop;
|
||
static volatile LONG g_wkc;
|
||
static long long g_expected_wkc;
|
||
static int g_slavecount;
|
||
static char g_slave_names[16][64];
|
||
static uint8 g_iomap[IO_MAP_SIZE];
|
||
static ULONGLONG g_pcore_mask; /* --mask，0=不钉 */
|
||
|
||
/* ── PDO 线程：网格 + 自旋 + 分段计时，逻辑与 _pdo_worker 一一对应 ── */
|
||
static DWORD WINAPI pdo_worker(LPVOID arg)
|
||
{
|
||
double per, t0, ts, tm, tr, now, te, next_t = 0;
|
||
int have_next = 0, k, i, wkc;
|
||
|
||
SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
|
||
if (g_pcore_mask)
|
||
SetThreadAffinityMask(GetCurrentThread(), (DWORD_PTR)g_pcore_mask);
|
||
|
||
while (!InterlockedOr(&g_stop, 0)) {
|
||
t0 = now_s();
|
||
EnterCriticalSection(&g_cfg_lock);
|
||
per = g_period_ms / 1000.0;
|
||
LeaveCriticalSection(&g_cfg_lock);
|
||
if (!have_next) { next_t = t0 + per; have_next = 1; }
|
||
|
||
ts = now_s(); /* 原生无锁无填充 → pre≈0 */
|
||
ec_send_processdata();
|
||
tm = now_s();
|
||
wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
||
tr = now_s();
|
||
InterlockedExchange(&g_wkc, wkc);
|
||
|
||
/* 绝对网格截止点 + 超期跳过（不挪锚点），与 Python 同 */
|
||
now = now_s();
|
||
k = 0;
|
||
if (next_t <= now) {
|
||
while (next_t <= now) { next_t += per; k++; }
|
||
}
|
||
te = now;
|
||
|
||
EnterCriticalSection(&g_ring_lock);
|
||
i = g_ring_i;
|
||
g_ring_i = (i + 1) % RING;
|
||
g_ring[i].t0 = t0; g_ring[i].ts = ts; g_ring[i].tm = tm;
|
||
g_ring[i].tr = tr; g_ring[i].te = te; g_ring[i].k = k;
|
||
g_total++;
|
||
g_skip_total += k;
|
||
LeaveCriticalSection(&g_ring_lock);
|
||
|
||
if (k == 0) { /* 没超期才等下一拍；超期直接落下去补 */
|
||
spin_until(next_t);
|
||
next_t += per;
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
/* ── 分位数（读侧现算，同 _cyc_fresh 的口径）── */
|
||
static int cmp_d(const void *a, const void *b)
|
||
{
|
||
double x = *(const double *)a, y = *(const double *)b;
|
||
return (x > y) - (x < y);
|
||
}
|
||
static void pct3(const double *v, int n, char *out, size_t osz)
|
||
{
|
||
double *s = (double *)malloc(n * sizeof(double));
|
||
int i;
|
||
if (!s) { snprintf(out, osz, "{}"); return; }
|
||
memcpy(s, v, n * sizeof(double));
|
||
qsort(s, n, sizeof(double), cmp_d);
|
||
snprintf(out, osz, "{\"p50\":%.1f,\"p90\":%.1f,\"max\":%.1f}",
|
||
s[(int)(0.5 * (n - 1))], s[(int)(0.9 * (n - 1))], s[n - 1]);
|
||
free(s);
|
||
}
|
||
|
||
/* 取最近 n 拍（按时间序）。ring 单写者，读侧加锁拷贝。 */
|
||
static int recent_beats(beat_t *dst, int n)
|
||
{
|
||
int have, start, i;
|
||
EnterCriticalSection(&g_ring_lock);
|
||
have = (g_total < RING) ? (int)g_total : RING;
|
||
if (n > have) n = have;
|
||
start = (g_ring_i - n + RING * 2) % RING;
|
||
for (i = 0; i < n; i++) dst[i] = g_ring[(start + i) % RING];
|
||
LeaveCriticalSection(&g_ring_lock);
|
||
return n;
|
||
}
|
||
|
||
static void build_cyc_us(char *out, size_t osz)
|
||
{
|
||
static beat_t b[RING];
|
||
static double work[STATS_N], send[STATS_N], rx[STATS_N], tot[STATS_N];
|
||
char sw[96], sr[96], st[96], swork[96];
|
||
int n = recent_beats(b, RING), i, m = n < STATS_N ? n : STATS_N;
|
||
if (m < 2) { snprintf(out, osz, "{}"); return; }
|
||
for (i = 0; i < m; i++) {
|
||
beat_t *p = &b[n - m + i];
|
||
work[i] = (p->te - p->t0) * 1e6;
|
||
send[i] = (p->tm - p->ts) * 1e6;
|
||
rx[i] = (p->tr - p->tm) * 1e6;
|
||
tot[i] = (p->tr - p->t0) * 1e6;
|
||
}
|
||
pct3(work, m, swork, sizeof swork);
|
||
pct3(send, m, sw, sizeof sw);
|
||
pct3(rx, m, sr, sizeof sr);
|
||
pct3(tot, m, st, sizeof st);
|
||
snprintf(out, osz,
|
||
"\"cyc_us\":{\"n\":%d,\"work\":%s,\"send\":%s,\"rx\":%s,\"total\":%s,"
|
||
"\"late\":0,\"lost\":0,\"gc\":{\"n\":[0,0,0],\"max_us\":[0,0,0],\"pdo\":0}}",
|
||
m, swork, sw, sr, st);
|
||
}
|
||
|
||
/* ── 极简 HTTP/1.1（POST-only JSON，thread-per-connection）── */
|
||
static int json_num(const char *body, const char *key, double *val)
|
||
{
|
||
const char *p = strstr(body, key);
|
||
if (!p) return 0;
|
||
p += strlen(key);
|
||
while (*p && *p != ':') p++;
|
||
if (*p != ':') return 0;
|
||
p++;
|
||
while (*p == ' ' || *p == '\t') p++;
|
||
char *end;
|
||
double v = strtod(p, &end);
|
||
if (end == p) return 0;
|
||
*val = v;
|
||
return 1;
|
||
}
|
||
|
||
static void handle_conn(SOCKET cs)
|
||
{
|
||
char *buf = (char *)malloc(65536);
|
||
int len = 0, i, cl = 0;
|
||
char *hdr_end, *body, path[128] = "";
|
||
char *resp = (char *)malloc(256 * 1024);
|
||
int rl = 0;
|
||
char extra[4096];
|
||
|
||
if (!buf || !resp) { closesocket(cs); free(buf); free(resp); return; }
|
||
/* 读到头 */
|
||
for (;;) {
|
||
int n = recv(cs, buf + len, 65535 - len, 0);
|
||
if (n <= 0) goto done;
|
||
len += n; buf[len] = 0;
|
||
hdr_end = strstr(buf, "\r\n\r\n");
|
||
if (hdr_end) break;
|
||
}
|
||
body = hdr_end + 4;
|
||
{ /* Content-Length + 收满 body */
|
||
char *p = buf;
|
||
for (p = strstr(p, "\n"); p && p < hdr_end; p = strstr(p + 1, "\n")) {
|
||
/* p 停在上一行的 \n 上，下一行从 p+1 开始 */
|
||
if (!_strnicmp(p + 1, "content-length:", 15)) { cl = atoi(p + 16); break; }
|
||
}
|
||
while ((body + cl) > (buf + len)) {
|
||
int need = (int)((body + cl) - (buf + len));
|
||
int r = recv(cs, buf + len, need > 65535 - len ? 65535 - len : need, 0);
|
||
if (r <= 0) goto done;
|
||
len += r; buf[len] = 0;
|
||
}
|
||
if (cl > 65535 - (int)(body - buf)) cl = (int)(buf + len - body);
|
||
body[cl] = 0;
|
||
}
|
||
if (sscanf(buf, "%*s %127s", path) != 1) goto done;
|
||
|
||
if (!strcmp(path, "/api/motor/state")) {
|
||
build_cyc_us(extra, sizeof extra);
|
||
rl = snprintf(resp, 256 * 1024,
|
||
"{\"ok\":true,\"log\":[],\"cycles\":%lld,\"period_ms\":%.4f,\"spin\":true,"
|
||
"\"wkc\":%d,\"expected_wkc\":%d,\"slot_period_ms\":%.4f,\"win_bases\":0,"
|
||
"\"queue_depth\":0,\"sent_seq\":0,\"sent_frames\":0,\"ts\":0,"
|
||
"\"head\":{},\"motors\":[",
|
||
g_total, g_period_ms, (int)g_wkc, (int)g_expected_wkc, g_period_ms);
|
||
for (i = 0; i < g_slavecount && i < 16; i++)
|
||
rl += snprintf(resp + rl, 256 * 1024 - rl, "%s{\"name\":\"%s\",\"valid\":true}",
|
||
i ? "," : "", g_slave_names[i]);
|
||
rl += snprintf(resp + rl, 256 * 1024 - rl, "],\"bridge\":{},\"xrx\":{},\"rx_batch\":{},%s}", extra);
|
||
} else if (!strcmp(path, "/api/motor/period")) {
|
||
double v = -1, old = g_period_ms;
|
||
if (json_num(body, "period_us", &v) && v > 0) v /= 1000.0;
|
||
else if (!json_num(body, "period_ms", &v)) v = -1;
|
||
if (v < 0.0625 || v > 1000.0) {
|
||
rl = snprintf(resp, 256 * 1024,
|
||
"{\"ok\":false,\"log\":[\"周期 %s 超出允许范围 [62.5us, 1000ms]\"]}", body);
|
||
} else {
|
||
EnterCriticalSection(&g_cfg_lock);
|
||
g_period_ms = v;
|
||
LeaveCriticalSection(&g_cfg_lock);
|
||
rl = snprintf(resp, 256 * 1024,
|
||
"{\"ok\":true,\"log\":[\"PDO 周期(native) %gms → %gms\"],\"period_ms\":%.4f}",
|
||
old, v, v);
|
||
}
|
||
} else if (!strcmp(path, "/api/motor/dbg")) {
|
||
static beat_t b[RING];
|
||
int n = recent_beats(b, RING);
|
||
rl = snprintf(resp, 256 * 1024,
|
||
"{\"ok\":true,\"period_ms\":%.4f,\"spin\":true,\"gc_enabled\":false,"
|
||
"\"cycles\":%lld,\"skip_total\":%lld,\"cyc\":[",
|
||
g_period_ms, g_total, g_skip_total);
|
||
for (i = 0; i < n; i++)
|
||
rl += snprintf(resp + rl, 256 * 1024 - rl,
|
||
"%s[%.7f,%.7f,%.7f,%.7f,%.7f,%d]",
|
||
i ? "," : "", b[i].t0, b[i].ts, b[i].tm, b[i].tr, b[i].te, b[i].k);
|
||
rl += snprintf(resp + rl, 256 * 1024 - rl, "],\"gc_events\":[],\"http\":[]}");
|
||
} else if (!strcmp(path, "/") || !strcmp(path, "/api/native/health")) {
|
||
rl = snprintf(resp, 256 * 1024,
|
||
"{\"ok\":true,\"name\":\"ecat_native\",\"slaves\":%d,\"wkc\":%d,\"cycles\":%lld}",
|
||
g_slavecount, (int)g_wkc, g_total);
|
||
} else {
|
||
rl = snprintf(resp, 256 * 1024,
|
||
"{\"ok\":false,\"log\":[\"接口不存在(native 原型只有 state/period/dbg): %s\"]}", path);
|
||
}
|
||
{ /* 发送 */
|
||
char hdr[256];
|
||
int hl = snprintf(hdr, sizeof hdr,
|
||
"HTTP/1.1 200 OK\r\nContent-Type: application/json; charset=utf-8\r\n"
|
||
"Cache-Control: no-store\r\nContent-Length: %d\r\nConnection: close\r\n\r\n", rl);
|
||
send(cs, hdr, hl, 0);
|
||
send(cs, resp, rl, 0);
|
||
}
|
||
done:
|
||
closesocket(cs);
|
||
free(buf);
|
||
free(resp);
|
||
}
|
||
|
||
static DWORD WINAPI conn_thread(LPVOID p)
|
||
{
|
||
handle_conn((SOCKET)(intptr_t)p);
|
||
return 0;
|
||
}
|
||
|
||
int main(int argc, char **argv)
|
||
{
|
||
const char *substr = "I226";
|
||
int port = 8091, i;
|
||
LARGE_INTEGER f;
|
||
ec_adaptert *ad, *pick = NULL;
|
||
WSADATA wsa;
|
||
SOCKET ls;
|
||
struct sockaddr_in sa;
|
||
HANDLE th;
|
||
|
||
for (i = 1; i < argc - 0; i++) {
|
||
if (!strcmp(argv[i], "--substr") && i + 1 < argc) substr = argv[++i];
|
||
else if (!strcmp(argv[i], "--port") && i + 1 < argc) port = atoi(argv[++i]);
|
||
else if (!strcmp(argv[i], "--mask") && i + 1 < argc)
|
||
g_pcore_mask = strtoull(argv[++i], NULL, 0);
|
||
}
|
||
|
||
QueryPerformanceFrequency(&f);
|
||
g_qpc2s = 1.0 / (double)f.QuadPart;
|
||
InitializeCriticalSection(&g_ring_lock);
|
||
InitializeCriticalSection(&g_cfg_lock);
|
||
|
||
/* 找网卡（desc/name 含 substr） */
|
||
for (ad = ec_find_adapters(); ad; ad = ad->next) {
|
||
if (strstr(ad->desc, substr) || strstr(ad->name, substr)) { pick = ad; break; }
|
||
}
|
||
if (!pick) {
|
||
fprintf(stderr, "[native] 找不到含 \"%s\" 的网卡\n", substr);
|
||
return 2;
|
||
}
|
||
if (!ec_init(pick->name)) {
|
||
fprintf(stderr, "[native] ec_init 失败: %s\n", pick->name);
|
||
return 3;
|
||
}
|
||
printf("[native] 打开网卡: %s\n", pick->desc);
|
||
|
||
/* 预清理：广播 INIT|ACK 清掉上一主站(可能被强杀)残留的 AL 错误位，再重枚举。
|
||
SSC 的 AL_ControlInd 对"错误位已置 + 请求不含 ACK(bit4)"的迁移直接忽略
|
||
(ecatslv.c)，这是 native 之前卡 PREOP 的根因 —— 与 Python motor/start 的
|
||
预清理同理。config_init 会重写 SM0/SM1，等效"断开重连"。 */
|
||
ec_slave[0].state = EC_STATE_INIT | EC_STATE_ACK;
|
||
ec_writestate(0);
|
||
ec_statecheck(0, EC_STATE_INIT, 1000000);
|
||
if (ec_config_init(FALSE) <= 0) {
|
||
fprintf(stderr, "[native] 枚举从站失败（邮箱楔死需断电重启板子）\n");
|
||
ec_close();
|
||
return 4;
|
||
}
|
||
ec_config_map(&g_iomap);
|
||
ec_readstate();
|
||
/* LRW 的期望 WKC：每从站读输出+1、写输入+1、回读+1 → outputsWKC*2 + inputsWKC
|
||
（与 pysoem 同式；本机 1 从站 = 1*2+1 = 3，与实测 wkc 一致） */
|
||
g_expected_wkc = ec_group[0].outputsWKC * 2 + ec_group[0].inputsWKC;
|
||
g_slavecount = ec_slavecount;
|
||
for (i = 1; i <= ec_slavecount && i <= 16; i++)
|
||
snprintf(g_slave_names[i - 1], 64, "%s", ec_slave[i].name);
|
||
printf("[native] 枚举 %d 从站, config_map 后 AL=0x%02x, expected_wkc=%lld\n",
|
||
ec_slavecount, ec_slave[1].state & 0x0F, g_expected_wkc);
|
||
|
||
/* config_map 内部请求 SAFEOP 不带 ACK，残留错误位下会被忽略 → 显式补一次
|
||
SAFEOP|ACK 写，确保到 SAFEOP。 */
|
||
ec_slave[0].state = EC_STATE_SAFE_OP | EC_STATE_ACK;
|
||
ec_writestate(0);
|
||
ec_statecheck(0, EC_STATE_SAFE_OP, 2000000);
|
||
|
||
/* SAFEOP→OP：从站要先收到≥1帧过程数据才 ack OP，且带错误位时只有 OP|ACK
|
||
才清错并迁移 → 边发 PD 边带 ACK 重写 OP，每 200ms 一轮，readstate 拿真实态。
|
||
进 OP 后再起稳态线程（避免主线程与线程并发操作同一 ecx_port 打架）。 */
|
||
for (i = 0; i < 60; i++) {
|
||
ec_send_processdata();
|
||
ec_receive_processdata(EC_TIMEOUTRET);
|
||
ec_slave[0].state = EC_STATE_OPERATIONAL | EC_STATE_ACK;
|
||
ec_writestate(0);
|
||
ec_readstate();
|
||
if ((ec_slave[1].state & 0x0F) == EC_STATE_OPERATIONAL) break;
|
||
Sleep(200);
|
||
}
|
||
if ((ec_slave[1].state & 0x0F) != EC_STATE_OPERATIONAL) {
|
||
fprintf(stderr, "[native] 进 OP 失败（AL=0x%02x code=0x%04x）—— 若反复如此，"
|
||
"可能是上一主站退出后 SSC 邮箱楔死，需断电重启板子\n",
|
||
ec_slave[1].state & 0x0F, ec_slave[1].ALstatuscode);
|
||
ec_close();
|
||
return 5;
|
||
}
|
||
printf("[native] OP 达成: %d 从站\n", ec_slavecount);
|
||
|
||
th = CreateThread(NULL, 0, pdo_worker, NULL, 0, NULL);
|
||
SetThreadPriority(th, THREAD_PRIORITY_TIME_CRITICAL);
|
||
|
||
WSAStartup(MAKEWORD(2, 2), &wsa);
|
||
ls = socket(AF_INET, SOCK_STREAM, 0);
|
||
{ int one = 1; setsockopt(ls, SOL_SOCKET, SO_REUSEADDR, (char *)&one, sizeof one); }
|
||
sa.sin_family = AF_INET;
|
||
sa.sin_port = htons((u_short)port);
|
||
sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK); /* 只绑 127.0.0.1，同 Python 口径 */
|
||
if (bind(ls, (struct sockaddr *)&sa, sizeof sa) == SOCKET_ERROR) {
|
||
fprintf(stderr, "[native] bind %d 失败: %d\n", port, WSAGetLastError());
|
||
return 6;
|
||
}
|
||
listen(ls, 32);
|
||
printf("[native] 监听 http://127.0.0.1:%d (state/period/dbg)\n", port);
|
||
|
||
for (;;) {
|
||
SOCKET cs = accept(ls, NULL, NULL);
|
||
HANDLE t;
|
||
if (cs == INVALID_SOCKET) continue;
|
||
t = CreateThread(NULL, 0, conn_thread, (LPVOID)(intptr_t)cs, 0, NULL);
|
||
if (t) CloseHandle(t); else closesocket(cs);
|
||
}
|
||
return 0;
|
||
}
|