cbb
a56b702d83
big-bang 迁移(硬约束: Npcap 网卡独占, 无法双主站并存), 四阶段真机验收的 完整契约与对拍记录见 MIGRATION.md —— Python 已删, 该文件是唯一行为基准。 - 新增 ecat_native.c: 与 Python 同构的全路由 native 守护 —— 总线生命周期 open/close/status/op.start/op.stop, 电机 motor.start/stop/spin/state/ send/rx/period/dbg, can/send|state, 配置维护 sdo/eeprom.read|write| verify/flash/sync_diag, 静态托管 src/web, shutdown/focus/--parent-pid 看门狗/独立启动清场/端口漂移; 独占 8090 即生产链路。PDO 热循环天花板 ~5kHz → ~7kHz。 - 删除 src/ecat_server.py(2826 行)、src/test_motor_backend.py(684 行)、 ecat_app.vbs(桌面窗口随 pywebview 依赖一并移除): Python 后端不留副本。 - 编码回归改金标准: native_enc_diff.py 对照 native_enc_golden.json (阶段 2 与 Python _fill_can_out 逐字节对拍通过后固化, 不碰网卡), 当场抓掉 json_tok 不跳闭合引号会静默丢 motor/send 参数的真 bug。 - build_native.bat: 编译前跑 gen_eeprom.py 生成 eeprom_image.h 供 native #include —— 同一生成器同一套断言, 断言失败构建中止; 改 SII 重跑即可。 - 修 flash 探针字历史隐患(本文件与 ecat_native.c 同步改): SCRATCH_WORD 0x0200 实际落在 TxPDO 类别区内部(类别区已长到 0x0281), 单独跑 eeprom 探针不烧录会把真数据擦成空洞; 移到 SII END 之后的 0x0400。 - 启动器 ecat_web.vbs/bat 改拉 build_native\ecat_native.exe (--substr I226 --web src\web)。 - 收录 soem-src wpcap 链接库(vendored, 构建必需)。 两轮回归修复(细节见 MIGRATION.md): 1) op/start 卡 AL 0x001B(SM 看门狗): 固件"自己写 1s 看门狗"的假设破产, do_op_start 第 5 步改为 ~5ms 节奏喂帧 ≥1s 再请求 OP, 24s 两次失败 变 1.2s 进 OP。 2) OP 态数据冻结(从站 PDI_Isr SM2 分支被旗标挡住, 固件侧根因): 主站加 楔死看门狗(输出窗口轮转 + 回显冻结 128 周期判死)与 /api/motor/state 自动恢复(10s 冷却, 快照新增 wr 字段)。 已知差异: SDO 失败文本无 pysoem abort code 粒度; 250µs 亚毫秒周期从站 吃不下, 修复前勿用。build_native/ 构建产物入 .gitignore, 由脚本重建。
2230 lines
98 KiB
C
2230 lines
98 KiB
C
/* ecat_native.c — 原生 EtherCAT 守护进程（迁移阶段 4：全路由 + 生命周期契约）
|
||
*
|
||
* 阶段 1：生命周期状态机 + 静态托管（已真机验证）。
|
||
* 阶段 2：PDO 循环接管 CAN-over-EtherCAT 桥 —— 命令队列泵入（单帧+批量区共用 seq 沿）、
|
||
* 电机状态窗口自适应轮转（cover_bases + 发现窗）、批量接收回收（rx log）、
|
||
* 完整快照（motors/bridge/xrx/rx_batch/head）。字节布局与 ecat_server.py
|
||
* 逐字节对齐（布局注释见 Python 686-705 行 / ethercat_can_bridge.h）。
|
||
* 阶段 3：配置/维护面 —— SDO 读写、EEPROM 读/写/校验、flash（默认镜像在构建期由
|
||
* gen_eeprom.py 生成 eeprom_image.h 嵌入，源仍是 gen_eeprom 的自校验断言）、
|
||
* sync_diag（速率基线与 Python _sync_diag_prev 同契约：op 进/出清零）。
|
||
* 全部走独占总线路由：先停 PDO 循环再碰邮箱/EEPROM，镜像
|
||
* Python _BUS_EXCLUSIVE_PATHS —— 邮箱与周期数据并发是历史上邮箱楔死的诱因。
|
||
*
|
||
* 契约：复刻 ecat_server.py 路由与 JSON 口径，只绑 127.0.0.1、无 CORS。
|
||
* 网卡独占 → 与 Python 不同时跑（big-bang 切换，见 MIGRATION.md）。
|
||
*
|
||
* 时间线元组与 Python 同构：(t0, ts, tm, tr, te, k)，QPC 秒（与 perf_counter 同基）。
|
||
*/
|
||
#include <winsock2.h>
|
||
#include <ws2tcpip.h>
|
||
#include <windows.h>
|
||
#include <tlhelp32.h>
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <stdarg.h>
|
||
#include <string.h>
|
||
#include <math.h>
|
||
#include "ethercat.h"
|
||
|
||
/* SII 默认烧录镜像：构建期由 gen_eeprom.py 的 stdout 生成（自校验断言不过则构建失败）。
|
||
* 定义 ESI_EEPROM_IMAGE[4096] + ESI_EEPROM_SIZE，见 build_native.bat。 */
|
||
#include "eeprom_image.h"
|
||
|
||
#define RING 2048
|
||
#define IO_MAP_SIZE 4096
|
||
#define STATS_N 400
|
||
#define BUSY_TIMEOUT 3000
|
||
#define EEPROM_SIZE 4096 /* AT24C32，与 ecat_server.py 常量一致 */
|
||
#define SDO_BUF_MAX 1024
|
||
#define EE_WORDS (EEPROM_SIZE / 2)
|
||
/* flash 字节序探针用的临时字。必须在 SII END 之后的真 0xFF 填充区，
|
||
* 详见 /api/flash 里的注释（老脚本的 0x0200 在 TxPDO 类别内部）。 */
|
||
#define EE_SCRATCH_WORD 0x0400
|
||
|
||
|
||
/* ── CAN 桥线格式常量（与 ecat_server.py 一一对应）── */
|
||
#define PDO_OUT_LEN 384
|
||
#define PDO_IN_LEN 640
|
||
#define CAN_FD_DATA_MAX 64
|
||
#define CAN_TX_DATA_MAX 32 /* can/send 单帧区上限(Python 的越界判据) */
|
||
#define CAN_RX_DATA_MAX 8
|
||
#define CAN_XRX_DATA_MAX 32
|
||
#define TX_OFF_WIN 8
|
||
#define TX_OFF_DATA 16
|
||
#define FRM_OFF_CH 0
|
||
#define FRM_OFF_DLC 1
|
||
#define FRM_OFF_FLAGS 2
|
||
#define FRM_OFF_RES 3
|
||
#define FRM_OFF_ID 4
|
||
#define FRM_HDR_SIZE 8
|
||
#define TXB_OFF_COUNT 48
|
||
#define TXB_OFF_FLAGS 49
|
||
#define TXB_OFF_ITEMS 52
|
||
#define TXB_BYTES (PDO_OUT_LEN - TXB_OFF_ITEMS) /* 332 */
|
||
#define MOTW_OFF_ACTIVE 32
|
||
#define MOTW_OFF_BOOT 33
|
||
#define MOTW_OFF_WIN_BASE 34
|
||
#define MOTW_OFF_WIN_COUNT 35
|
||
#define MOTW_OFF_GLOBAL_SEQ 36
|
||
#define MOTW_OFF_SLOTS 40
|
||
#define MOT_SLOT_SIZE 40
|
||
#define MOT_SLOTS_PER_PDO 7
|
||
#define MOT_STATUS_DLC 32
|
||
#define XRX_OFF_CH 320
|
||
#define XRX_OFF_DLC 321
|
||
#define XRX_OFF_FLAGS 322
|
||
#define XRX_OFF_ID 324
|
||
#define XRX_OFF_DATA 328
|
||
#define RXB_OFF_COUNT 360
|
||
#define RXB_OFF_LOST 361
|
||
#define RXB_OFF_PENDING 362
|
||
#define RXB_OFF_ITEMS 364
|
||
#define RXB_BYTES (PDO_IN_LEN - RXB_OFF_ITEMS) /* 276 */
|
||
#define RXB_MAX_FRAMES (RXB_BYTES / (FRM_HDR_SIZE + CAN_FD_DATA_MAX))
|
||
#define MOTOR_SLOT_COUNT 27
|
||
#define MOTOR_WIN_BASE_MAX (MOTOR_SLOT_COUNT - MOT_SLOTS_PER_PDO) /* 20 */
|
||
#define MOTOR_DISCOVERY_EVERY 64
|
||
#define N_FULL_BASES 4 /* range(0,27,7) */
|
||
static const int g_full_bases[N_FULL_BASES] = { 0, 7, 14, 21 };
|
||
/* 发现基址 = min(0/7/14/21, 20) 去重 = [0,7,14,20]（与 MOTOR_DISCOVERY_BASES 同式） */
|
||
static const int g_discovery_bases[4] = { 0, 7, 14, 20 };
|
||
#define N_DISC_BASES 4
|
||
|
||
/* ── 计时原语 ── */
|
||
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();
|
||
}
|
||
|
||
/* ── 帧与队列 ── */
|
||
typedef struct { int ch; uint32 id; int dlc; uint8 data[CAN_FD_DATA_MAX]; } frame_t;
|
||
|
||
#define CMD_Q 512
|
||
static frame_t g_cmd_q[CMD_Q];
|
||
static int g_cmd_head, g_cmd_len;
|
||
static CRITICAL_SECTION g_cmd_lock;
|
||
|
||
#define RX_Q 2048
|
||
#define RX_BATCH_CAP 34
|
||
static frame_t g_rx_q[RX_Q];
|
||
static int g_rx_valid[RX_Q];
|
||
static int g_rx_head, g_rx_len;
|
||
static CRITICAL_SECTION g_rx_lock;
|
||
|
||
/* ── 解码后的桥状态（PDO 线程写，路由读）── */
|
||
typedef struct { uint8 node, ch, online, dlc; uint32 seq; uint8 data[MOT_STATUS_DLC]; } slot_t;
|
||
typedef struct { int valid; uint8 ack, ch, flags, dlc; uint32 id, rx_count;
|
||
uint8 tx_result, backlog; uint16 tx_count, drop; uint8 data[CAN_RX_DATA_MAX]; } bridge_t;
|
||
typedef struct { int valid; int ch, dlc; uint32 id; int n; uint8 data[CAN_XRX_DATA_MAX]; } xrx_t;
|
||
typedef struct { int count, lost, pending, n; frame_t fr[RXB_MAX_FRAMES]; } rxb_t;
|
||
|
||
static slot_t g_slots[MOTOR_SLOT_COUNT];
|
||
static struct { int valid, active, boot, wbase, wcount; uint32 gseq; } g_head;
|
||
static bridge_t g_bridge;
|
||
static xrx_t g_xrx;
|
||
static rxb_t g_rxb;
|
||
static int g_bases[4], g_n_bases; /* 当前生效的自适应基址 */
|
||
static int g_win;
|
||
static int g_sent_seq, g_sent_frames;
|
||
static CRITICAL_SECTION g_state_lock;
|
||
|
||
/* ── 总线状态机 ── */
|
||
static LONG g_bus_open, g_op, g_loop_running;
|
||
static volatile LONG g_stop;
|
||
static HANDLE g_pdo_th;
|
||
static CRITICAL_SECTION g_bus_lock;
|
||
|
||
/* ── OP 态应用层楔死看门狗（机理与判据见 pdo_worker 内长注释）──
|
||
* 检测在循环线程（每周期比对输入窗口回显），恢复在路由侧（motor/state，UI 每
|
||
* 50ms 轮询）：循环线程不能自己抢 g_bus_lock——互斥路由 loop_stop 等线程退出
|
||
* 时，线程若又在等锁就死锁。TryEnter：有互斥路由在跑就让过，下一拍再试。 */
|
||
static volatile LONG g_wedge_at; /* 检测时刻 (LONG)now_s()；0=无楔死 */
|
||
static volatile LONG g_wedge_cooldown; /* 恢复后冷却截止时刻，防反复重拉 */
|
||
static long long g_wedge_recoveries; /* 已自动恢复次数（motor/state 暴露为 wr） */
|
||
|
||
/* ── 时间线环 + 计数 + 配置 ── */
|
||
typedef struct { double t0, ts, tm, tr, te; int k; } beat_t;
|
||
static beat_t g_ring[RING];
|
||
static int g_ring_i;
|
||
static long long g_total;
|
||
static long long g_skip_total;
|
||
static CRITICAL_SECTION g_ring_lock;
|
||
static CRITICAL_SECTION g_cfg_lock;
|
||
static double g_period_ms = 0.25;
|
||
static volatile LONG g_wkc;
|
||
static long long g_expected_wkc;
|
||
static int g_slavecount;
|
||
static char g_slave_names[16][64];
|
||
static char g_desc[256];
|
||
static uint8 g_iomap[IO_MAP_SIZE];
|
||
static ULONGLONG g_pcore_mask;
|
||
static char g_webdir[512] = "src/web";
|
||
|
||
/* 输出 PDO 暂存：跨周期保持（固件按 seq 变化沿去重，保持不变是为了 seq 可回显） */
|
||
static uint8 g_can_out[PDO_OUT_LEN];
|
||
static int g_can_seq;
|
||
|
||
/* sync_diag 速率基线(对应 Python _sync_diag_prev)。定义在此因 do_op_* 也要清零；
|
||
* 使用见文件尾部阶段 3 路由。 */
|
||
static int g_syncprev_valid;
|
||
static uint32 g_syncprev_sync0;
|
||
static double g_syncprev_ts;
|
||
|
||
/* ── 小工具 ── */
|
||
static void le32put(uint8 *p, uint32 v) { p[0] = (uint8)v; p[1] = (uint8)(v >> 8); p[2] = (uint8)(v >> 16); p[3] = (uint8)(v >> 24); }
|
||
static uint32 le32get(const uint8 *p) { return (uint32)p[0] | ((uint32)p[1] << 8) | ((uint32)p[2] << 16) | ((uint32)p[3] << 24); }
|
||
static int cmp_int(const void *a, const void *b) { return *(const int *)a - *(const int *)b; }
|
||
static int cmp_d(const void *a, const void *b)
|
||
{
|
||
double x = *(const double *)a, y = *(const double *)b;
|
||
return (x > y) - (x < y);
|
||
}
|
||
|
||
/* ── CAN 桥编码（与 _pack_tx_batch/_batch_capacity/_fill_can_out 同语义）── */
|
||
static int batch_capacity(const frame_t *fr, int n)
|
||
{
|
||
int off = TXB_OFF_ITEMS, i, c = 0, dlc;
|
||
for (i = 0; i < n; i++) {
|
||
dlc = fr[i].dlc > CAN_FD_DATA_MAX ? CAN_FD_DATA_MAX : fr[i].dlc;
|
||
if (off + FRM_HDR_SIZE + dlc > PDO_OUT_LEN) break;
|
||
off += FRM_HDR_SIZE + dlc;
|
||
c++;
|
||
}
|
||
return c;
|
||
}
|
||
static int pack_tx_batch(const frame_t *fr, int n)
|
||
{
|
||
int off = TXB_OFF_ITEMS, i, c = 0, dlc;
|
||
g_can_out[TXB_OFF_COUNT] = 0;
|
||
g_can_out[TXB_OFF_FLAGS] = 0; /* 默认 stop_on_error=False */
|
||
for (i = 0; i < n; i++) {
|
||
dlc = fr[i].dlc > CAN_FD_DATA_MAX ? CAN_FD_DATA_MAX : fr[i].dlc;
|
||
if (off + FRM_HDR_SIZE + dlc > PDO_OUT_LEN) break;
|
||
g_can_out[off + FRM_OFF_CH] = (uint8)(fr[i].ch & 0xFF);
|
||
g_can_out[off + FRM_OFF_DLC] = (uint8)dlc;
|
||
g_can_out[off + FRM_OFF_FLAGS] = 0;
|
||
g_can_out[off + FRM_OFF_RES] = 0;
|
||
le32put(g_can_out + off + FRM_OFF_ID, fr[i].id);
|
||
memset(g_can_out + off + FRM_HDR_SIZE, 0, dlc);
|
||
memcpy(g_can_out + off + FRM_HDR_SIZE, fr[i].data, dlc);
|
||
off += FRM_HDR_SIZE + dlc;
|
||
c++;
|
||
}
|
||
g_can_out[TXB_OFF_COUNT] = (uint8)c;
|
||
return c;
|
||
}
|
||
static int fill_can_out(const frame_t *first, const frame_t *batch, int nbatch,
|
||
int clear_stats)
|
||
{
|
||
uint8 win;
|
||
g_can_seq = (g_can_seq + 1) & 0xFF;
|
||
if (g_can_seq == 0) g_can_seq = 1; /* 0 与固件"未初始化"无法区分，跳过 */
|
||
win = g_can_out[TX_OFF_WIN]; /* 窗口由循环独占推进，发送路径保持 */
|
||
memset(g_can_out, 0, sizeof g_can_out);
|
||
g_can_out[0] = (uint8)g_can_seq;
|
||
g_can_out[1] = (uint8)first->ch;
|
||
g_can_out[2] = (uint8)first->dlc;
|
||
g_can_out[3] = clear_stats ? 0x01 : 0x00;
|
||
le32put(g_can_out + 4, first->id);
|
||
g_can_out[TX_OFF_WIN] = win;
|
||
if (first->dlc > 0)
|
||
memcpy(g_can_out + TX_OFF_DATA, first->data, first->dlc);
|
||
if (batch && nbatch > 0) pack_tx_batch(batch, nbatch);
|
||
return g_can_seq;
|
||
}
|
||
|
||
/* ── 窗口覆盖（_cover_bases 同算法；空集回退全表轮扫）── */
|
||
static int cover_bases(int bases[4])
|
||
{
|
||
int todo[MOTOR_SLOT_COUNT], n = 0, i, k = 0, base, end;
|
||
for (i = 0; i < MOTOR_SLOT_COUNT; i++)
|
||
if (g_slots[i].node != 0) todo[n++] = i;
|
||
if (n == 0) { memcpy(bases, g_full_bases, sizeof g_full_bases); return N_FULL_BASES; }
|
||
qsort(todo, n, sizeof(int), cmp_int);
|
||
i = 0;
|
||
while (i < n && k < 4) {
|
||
base = todo[i] > MOTOR_WIN_BASE_MAX ? MOTOR_WIN_BASE_MAX : todo[i];
|
||
end = base + MOT_SLOTS_PER_PDO;
|
||
bases[k++] = base;
|
||
while (i < n && todo[i] < end) i++;
|
||
}
|
||
return k;
|
||
}
|
||
|
||
/* ── 输入解码（字节布局逐一对齐 Python 的 decode 族）── */
|
||
|
||
/* 段1 桥状态解码，从 decode_input 拆出：can/send、can/state 的同步 pump 路径
|
||
* 只要桥字段（不碰槽/批量区），对应 Python 的 _can_decode_input。 */
|
||
static void bridge_decode(const uint8 *inp, int ilen, bridge_t *b)
|
||
{
|
||
int dlc;
|
||
memset(b, 0, sizeof *b);
|
||
if (ilen < 32) return;
|
||
b->valid = 1;
|
||
b->ack = inp[0];
|
||
b->ch = inp[1];
|
||
b->dlc = inp[2];
|
||
b->flags = inp[3];
|
||
b->id = le32get(inp + 4);
|
||
b->rx_count = le32get(inp + 16);
|
||
b->tx_result = inp[20];
|
||
b->backlog = inp[21];
|
||
b->tx_count = (uint16)(inp[22] | (inp[23] << 8));
|
||
b->drop = (uint16)(inp[24] | (inp[25] << 8));
|
||
dlc = inp[2] > CAN_RX_DATA_MAX ? CAN_RX_DATA_MAX : inp[2];
|
||
memcpy(b->data, inp + 8, dlc);
|
||
}
|
||
|
||
/* 就地驱动 n 个 PDO 周期（等价 Python _can_pump）：把当前 g_can_out 推出去、
|
||
* 读回输入并解桥。只在循环未运行、由 HTTP 线程持 g_bus_lock 独占总线时调用 ——
|
||
* 与 worker 并发踩周期是两个写者抢 s.output，正是独占路由要防的事。 */
|
||
static int pump_cycles(int n, bridge_t *b)
|
||
{
|
||
uint8 inp[PDO_IN_LEN];
|
||
int ol, ilen = 0, i, wkc = 0;
|
||
if (b) memset(b, 0, sizeof *b);
|
||
ol = ec_slave[1].Obytes; if (ol > PDO_OUT_LEN) ol = PDO_OUT_LEN;
|
||
for (i = 0; i < (n < 1 ? 1 : n); i++) {
|
||
if (ol > 0) memcpy(ec_slave[1].outputs, g_can_out, ol);
|
||
ec_send_processdata();
|
||
wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
||
ilen = ec_slave[1].Ibytes; if (ilen > (int)sizeof inp) ilen = (int)sizeof inp;
|
||
if (ilen > 0) memcpy(inp, ec_slave[1].inputs, ilen);
|
||
Sleep(2);
|
||
}
|
||
if (b) bridge_decode(inp, ilen, b);
|
||
return wkc;
|
||
}
|
||
|
||
static void decode_input(const uint8 *inp, int ilen)
|
||
{
|
||
int base, count, i, off, dlc, slot;
|
||
frame_t newfr[RXB_MAX_FRAMES];
|
||
int nnew = 0, nfr;
|
||
int frflags[RXB_MAX_FRAMES];
|
||
|
||
bridge_decode(inp, ilen, &g_bridge);
|
||
if (ilen >= MOTW_OFF_SLOTS + MOT_SLOTS_PER_PDO * MOT_SLOT_SIZE) {
|
||
g_head.valid = 1;
|
||
g_head.active = inp[MOTW_OFF_ACTIVE];
|
||
g_head.boot = inp[MOTW_OFF_BOOT];
|
||
base = inp[MOTW_OFF_WIN_BASE];
|
||
count = inp[MOTW_OFF_WIN_COUNT] > MOT_SLOTS_PER_PDO ? MOT_SLOTS_PER_PDO : inp[MOTW_OFF_WIN_COUNT];
|
||
g_head.wbase = base;
|
||
g_head.wcount = count;
|
||
g_head.gseq = le32get(inp + MOTW_OFF_GLOBAL_SEQ);
|
||
for (i = 0; i < count; i++) {
|
||
slot = base + i;
|
||
off = MOTW_OFF_SLOTS + i * MOT_SLOT_SIZE;
|
||
if (slot >= MOTOR_SLOT_COUNT) break;
|
||
if (inp[off] == 0) continue; /* 空槽无意义 */
|
||
g_slots[slot].node = inp[off];
|
||
g_slots[slot].ch = inp[off + 1];
|
||
g_slots[slot].online = inp[off + 2] ? 1 : 0;
|
||
g_slots[slot].dlc = inp[off + 3];
|
||
g_slots[slot].seq = le32get(inp + off + 4);
|
||
memcpy(g_slots[slot].data, inp + off + 8, MOT_STATUS_DLC);
|
||
}
|
||
}
|
||
if (ilen >= XRX_OFF_DATA + CAN_XRX_DATA_MAX && (inp[XRX_OFF_FLAGS] & 0x01)) {
|
||
g_xrx.valid = 1;
|
||
g_xrx.ch = inp[XRX_OFF_CH];
|
||
g_xrx.dlc = inp[XRX_OFF_DLC];
|
||
g_xrx.id = le32get(inp + XRX_OFF_ID);
|
||
g_xrx.n = g_xrx.dlc > CAN_XRX_DATA_MAX ? CAN_XRX_DATA_MAX : g_xrx.dlc;
|
||
memset(g_xrx.data, 0, sizeof g_xrx.data);
|
||
memcpy(g_xrx.data, inp + XRX_OFF_DATA, g_xrx.n);
|
||
} else {
|
||
g_xrx.valid = 0;
|
||
}
|
||
g_rxb.count = g_rxb.lost = g_rxb.pending = g_rxb.n = 0;
|
||
if (ilen > RXB_OFF_ITEMS) {
|
||
off = RXB_OFF_ITEMS;
|
||
g_rxb.count = inp[RXB_OFF_COUNT];
|
||
g_rxb.lost = inp[RXB_OFF_LOST];
|
||
g_rxb.pending = inp[RXB_OFF_PENDING];
|
||
nfr = g_rxb.count;
|
||
/* 不信任对端计数：边界判据始终是头+数据完整落在 PDO 内 */
|
||
for (i = 0; i < nfr && nnew < RXB_MAX_FRAMES; i++) {
|
||
if (off + FRM_HDR_SIZE > ilen) break;
|
||
dlc = inp[off + FRM_OFF_DLC];
|
||
if (dlc > CAN_FD_DATA_MAX || off + FRM_HDR_SIZE + dlc > ilen) break;
|
||
memset(&newfr[nnew], 0, sizeof newfr[nnew]);
|
||
newfr[nnew].ch = inp[off + FRM_OFF_CH];
|
||
newfr[nnew].dlc = dlc;
|
||
newfr[nnew].id = le32get(inp + off + FRM_OFF_ID);
|
||
memcpy(newfr[nnew].data, inp + off + FRM_HDR_SIZE, dlc);
|
||
frflags[nnew] = (inp[off + FRM_OFF_FLAGS] & 0x01) ? 1 : 0;
|
||
if (g_rxb.n < RXB_MAX_FRAMES) {
|
||
g_rxb.fr[g_rxb.n] = newfr[nnew];
|
||
g_rxb.n++;
|
||
}
|
||
nnew++;
|
||
off += FRM_HDR_SIZE + dlc;
|
||
}
|
||
}
|
||
if (nnew > 0) {
|
||
EnterCriticalSection(&g_rx_lock);
|
||
for (i = 0; i < nnew; i++) {
|
||
if (g_rx_len >= RX_Q) break; /* 有界：满了丢最旧语义由 maxlen 近似 */
|
||
{
|
||
int t = (g_rx_head + g_rx_len) % RX_Q;
|
||
g_rx_q[t] = newfr[i];
|
||
g_rx_valid[t] = frflags[i];
|
||
g_rx_len++;
|
||
}
|
||
}
|
||
LeaveCriticalSection(&g_rx_lock);
|
||
}
|
||
}
|
||
|
||
/* ── 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, wkc, out_len, in_len;
|
||
int bases[4], n_bases, bi = 0, discovery = 0, disco_base = 0, sweep_left;
|
||
int n_known_prev = 0;
|
||
double wedge_change; /* 楔死看门狗：回显上次变化时刻 */
|
||
uint8 wedge_echo = 0xFFu; /* 楔死看门狗：上次窗口回显值 */
|
||
uint8 inp[PDO_IN_LEN];
|
||
frame_t first, batch[32], pend[32];
|
||
|
||
SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
|
||
if (g_pcore_mask)
|
||
SetThreadAffinityMask(GetCurrentThread(), (DWORD_PTR)g_pcore_mask);
|
||
|
||
EnterCriticalSection(&g_state_lock);
|
||
n_bases = cover_bases(bases);
|
||
memcpy(g_bases, bases, n_bases * sizeof(int));
|
||
g_n_bases = n_bases;
|
||
LeaveCriticalSection(&g_state_lock);
|
||
sweep_left = n_bases;
|
||
bi = 0;
|
||
g_win = bases[0];
|
||
wedge_change = now_s();
|
||
InterlockedExchange(&g_wedge_at, 0);
|
||
|
||
while (!InterlockedOr(&g_stop, 0)) {
|
||
t0 = now_s();
|
||
EnterCriticalSection(&g_cfg_lock);
|
||
per = g_period_ms / 1000.0;
|
||
LeaveCriticalSection(&g_cfg_lock);
|
||
if (per <= 0) per = 0.004;
|
||
if (!have_next) { next_t = t0 + per; have_next = 1; }
|
||
|
||
/* 1) 泵命令队列：首帧段1，其余按字节容量打包段2，共用一个 seq 沿 */
|
||
{
|
||
int take = 0;
|
||
EnterCriticalSection(&g_cmd_lock);
|
||
if (g_cmd_len > 0) {
|
||
int j, avail;
|
||
first = g_cmd_q[g_cmd_head];
|
||
g_cmd_head = (g_cmd_head + 1) % CMD_Q;
|
||
g_cmd_len--;
|
||
avail = g_cmd_len < 32 ? g_cmd_len : 32;
|
||
for (j = 0; j < avail; j++) pend[j] = g_cmd_q[(g_cmd_head + j) % CMD_Q];
|
||
take = batch_capacity(pend, avail);
|
||
for (j = 0; j < take; j++) {
|
||
batch[j] = g_cmd_q[g_cmd_head];
|
||
g_cmd_head = (g_cmd_head + 1) % CMD_Q;
|
||
g_cmd_len--;
|
||
}
|
||
g_sent_seq = fill_can_out(&first, batch, take, 0);
|
||
g_sent_frames = 1 + take;
|
||
} else {
|
||
g_sent_frames = 0;
|
||
}
|
||
LeaveCriticalSection(&g_cmd_lock);
|
||
}
|
||
/* 2) 窗口基址写段1，输出拷进过程数据镜像，交换 */
|
||
g_can_out[TX_OFF_WIN] = (uint8)(g_win & 0xFF);
|
||
out_len = (int)ec_slave[1].Obytes;
|
||
if (out_len > 0) {
|
||
int ol = out_len > PDO_OUT_LEN ? PDO_OUT_LEN : out_len;
|
||
memcpy(ec_slave[1].outputs, g_can_out, ol);
|
||
}
|
||
|
||
ts = now_s();
|
||
ec_send_processdata();
|
||
tm = now_s();
|
||
wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
||
tr = now_s();
|
||
InterlockedExchange(&g_wkc, wkc);
|
||
|
||
in_len = (int)ec_slave[1].Ibytes;
|
||
if (in_len > PDO_IN_LEN) in_len = PDO_IN_LEN;
|
||
memcpy(inp, ec_slave[1].inputs, in_len);
|
||
EnterCriticalSection(&g_state_lock);
|
||
decode_input(inp, in_len);
|
||
{ /* 出现没见过的槽 → 立刻重算覆盖；首圈扫完前保持全表轮扫 */
|
||
int n_known = 0, i;
|
||
for (i = 0; i < MOTOR_SLOT_COUNT; i++)
|
||
if (g_slots[i].node != 0) n_known++;
|
||
if (n_known != n_known_prev) {
|
||
n_known_prev = n_known;
|
||
if (sweep_left <= 0) {
|
||
n_bases = cover_bases(bases);
|
||
if (bi >= n_bases) bi = 0;
|
||
memcpy(g_bases, bases, n_bases * sizeof(int));
|
||
g_n_bases = n_bases;
|
||
}
|
||
}
|
||
}
|
||
/* ── OP 态应用层楔死看门狗 ──
|
||
* 固件 dc_app.c 1.5.48/1.5.49 注释自述的已知问题：INIT↔OP 循环概率性
|
||
* 楔死 PDI_Isr 的 SM2 分支——ESC 硬件层一切正常（wkc=3、AL=OP、SDO/邮箱
|
||
* 活），但 ECAT_Application→EcatPdo_Process→buildInput 不再被调：输入区
|
||
* 停更、ack 不回显。固件自愈"仅 SAFEOP 触发"（其注释原文），OP 态无人管，
|
||
* 数据就此永久冻结——实测"切到 ECAT 一瞬间有数据然后没了"即此。
|
||
* 判据：主站每周期轮转输出窗口字节 WIN（自适应窗 + 每 64 周期发现窗），
|
||
* 健康从站输入段 2 的 win_base 回显必然跟着变；单基窗时发现窗可能恰与
|
||
* 自适应窗同值，回显最久 128 周期才变，阈值取 128 周期+2s 余量。回显冻结
|
||
* 超过阈值即楔死。与电机流量无关（电机静默时回显照样轮转），零误报。 */
|
||
if (g_op && g_head.valid && in_len >= MOTW_OFF_SLOTS) {
|
||
if ((uint8)g_head.wbase != wedge_echo) {
|
||
wedge_echo = (uint8)g_head.wbase;
|
||
wedge_change = t0;
|
||
} else if (t0 - wedge_change > 128.0 * per + 2.0
|
||
&& !InterlockedOr(&g_wedge_at, 0)
|
||
&& t0 >= (double)InterlockedOr(&g_wedge_cooldown, 0)) {
|
||
InterlockedExchange(&g_wedge_at, (LONG)t0);
|
||
printf("[native][WEDGE] OP 态输入回显冻结 %.1fs（win_echo=%u ack=%u "
|
||
"gseq=%u rx=%u wkc=%d period=%gms）——固件应用层楔死，待路由侧自动重拉\n",
|
||
t0 - wedge_change, (unsigned)wedge_echo,
|
||
(unsigned)g_bridge.ack, (unsigned)g_head.gseq,
|
||
(unsigned)g_bridge.rx_count, (int)InterlockedOr(&g_wkc, 0),
|
||
g_period_ms);
|
||
fflush(stdout);
|
||
}
|
||
} else {
|
||
wedge_echo = 0xFFu;
|
||
wedge_change = t0;
|
||
}
|
||
LeaveCriticalSection(&g_state_lock);
|
||
|
||
now = now_s();
|
||
k = 0;
|
||
if (next_t <= now) {
|
||
while (next_t <= now) { next_t += per; k++; }
|
||
}
|
||
te = now;
|
||
|
||
{
|
||
int ri;
|
||
EnterCriticalSection(&g_ring_lock);
|
||
ri = g_ring_i;
|
||
g_ring_i = (ri + 1) % RING;
|
||
g_ring[ri].t0 = t0; g_ring[ri].ts = ts; g_ring[ri].tm = tm;
|
||
g_ring[ri].tr = tr; g_ring[ri].te = te; g_ring[ri].k = k;
|
||
g_total++;
|
||
g_skip_total += k;
|
||
LeaveCriticalSection(&g_ring_lock);
|
||
}
|
||
|
||
/* 3) 窗口推进：首圈全扫 → 自适应轮转，每 64 周期插一个发现窗 */
|
||
if (sweep_left > 0) {
|
||
sweep_left--;
|
||
if (sweep_left == 0) {
|
||
/* 首圈扫完 → 按已发现的槽重算覆盖（与 Python sweep 结束处
|
||
`bases = _cover_bases(slots.keys())` 一致；不能读 g_bases ——
|
||
首圈期间 fresh 重算被 sweep_left<=0 门挡着，g_bases 还是全表） */
|
||
EnterCriticalSection(&g_state_lock);
|
||
n_bases = cover_bases(bases);
|
||
memcpy(g_bases, bases, n_bases * sizeof(int));
|
||
g_n_bases = n_bases;
|
||
LeaveCriticalSection(&g_state_lock);
|
||
bi = 0;
|
||
g_win = bases[0];
|
||
} else {
|
||
bi = (bi + 1) % n_bases;
|
||
g_win = bases[bi];
|
||
}
|
||
} else if (++discovery >= MOTOR_DISCOVERY_EVERY) {
|
||
discovery = 0;
|
||
g_win = g_discovery_bases[disco_base];
|
||
disco_base = (disco_base + 1) % N_DISC_BASES;
|
||
} else {
|
||
bi = (bi + 1) % n_bases;
|
||
g_win = bases[bi];
|
||
}
|
||
|
||
if (k == 0) {
|
||
spin_until(next_t);
|
||
next_t += per;
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
static void loop_stop(void)
|
||
{
|
||
if (g_loop_running) {
|
||
InterlockedExchange(&g_stop, 1);
|
||
if (g_pdo_th) { WaitForSingleObject(g_pdo_th, BUSY_TIMEOUT); CloseHandle(g_pdo_th); g_pdo_th = NULL; }
|
||
g_loop_running = 0;
|
||
InterlockedExchange(&g_stop, 0);
|
||
}
|
||
}
|
||
static void loop_start(void)
|
||
{
|
||
if (!g_loop_running) {
|
||
InterlockedExchange(&g_stop, 0);
|
||
g_pdo_th = CreateThread(NULL, 0, pdo_worker, NULL, 0, NULL);
|
||
g_loop_running = 1;
|
||
}
|
||
}
|
||
|
||
static void do_close(void);
|
||
static int logs_add(char *logs, int cap, int *lp, const char *fmt, ...);
|
||
|
||
/* ── 总线 bring-up ── */
|
||
static int do_open(const char *substr, char *out, int osz)
|
||
{
|
||
ec_adaptert *ad, *pick = NULL;
|
||
for (ad = ec_find_adapters(); ad; ad = ad->next)
|
||
if (strstr(ad->desc, substr) || strstr(ad->name, substr)) { pick = ad; break; }
|
||
if (!pick) { snprintf(out, osz, "没有找到描述包含 '%s' 的适配器", substr); return 0; }
|
||
if (g_bus_open) { ec_close(); g_bus_open = 0; }
|
||
if (!ec_init(pick->name)) { snprintf(out, osz, "ec_init 失败: %s", pick->name); return 0; }
|
||
/* 预清理：广播 INIT|ACK 清残留错误位（SSC 对带错误且不含 ACK 的请求直接忽略） */
|
||
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) {
|
||
ec_close();
|
||
snprintf(out, osz, "总线上没有发现从站——检查 DCU 板供电、网线");
|
||
return 0;
|
||
}
|
||
memset(g_can_out, 0, sizeof g_can_out);
|
||
memset(g_slots, 0, sizeof g_slots);
|
||
memset(&g_head, 0, sizeof g_head);
|
||
memset(&g_bridge, 0, sizeof g_bridge);
|
||
memset(&g_xrx, 0, sizeof g_xrx);
|
||
g_bus_open = 1;
|
||
g_slavecount = ec_slavecount;
|
||
snprintf(g_desc, sizeof g_desc, "%s", pick->desc);
|
||
snprintf(out, osz, "已连接 %s（枚举 %d 从站，停在 PREOP）", pick->desc, ec_slavecount);
|
||
return 1;
|
||
}
|
||
/* PREOP→(DC)→SAFEOP→OP 全流程，镜像 Python _require_op_ready：
|
||
* 入口无条件预清理、AL Control 一律带 ACK、free-run 清残留 DC 激活、进 OP 边发边写。
|
||
* dc_cycle_us=0 保持 free-run。日志逐行追加进调用方缓冲（对应 Python 的多行 log[]）。 */
|
||
static int do_op_start(int dc_cycle_us, int dc_shift_us, char *logs, int cap, int *lsz)
|
||
{
|
||
int i;
|
||
char mode[48];
|
||
g_syncprev_valid = 0; /* 新一轮运行，速率基线清零(Python api_op_start 同款) */
|
||
if (!g_bus_open) { logs_add(logs, cap, lsz, "未 open，先调 /api/open"); return 0; }
|
||
|
||
/* 0) 预清理：回 INIT + config_init 重枚举（等效断开重连）。Python 的实测结论
|
||
* (2026-09-03 01:49)：同一连接反复进出 OP 而不重枚举，从站邮箱/PDI 通路逐轮
|
||
* 退化，config_map 内 SOEM CoE 分段读可能吃到楔死邮箱的异常帧 → 原生崩溃。 */
|
||
logs_add(logs, cap, lsz, "预清理:回 INIT + config_init 重枚举(等效断开重连,防楔死邮箱上 config_map 原生崩溃)…");
|
||
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)
|
||
logs_add(logs, cap, lsz, "WARN: 预清理重枚举没找到从站(供电/网线?)，继续原序列");
|
||
|
||
/* 1) INIT→PREOP：AL Control 必须带 ACK —— SSC 对"错误位已置且请求不带 ACK"直接忽略 */
|
||
ec_slave[0].state = EC_STATE_PRE_OP | EC_STATE_ACK;
|
||
ec_writestate(0);
|
||
if (ec_statecheck(0, EC_STATE_PRE_OP, 2000000) != EC_STATE_PRE_OP) {
|
||
ec_readstate();
|
||
logs_add(logs, cap, lsz, "进 PREOP 失败: AL=0x%02x code=0x%04x —— 从站未 ack(0x0016/0x001D 类=邮箱 SM 被停止时禁用，重试即恢复)",
|
||
ec_slave[1].state & 0x0F, ec_slave[1].ALstatuscode);
|
||
return 0;
|
||
}
|
||
logs_add(logs, cap, lsz, "已进入 PREOP");
|
||
|
||
/* 1.4) 过程数据看门狗兜底写 1s：固件 configPdWatchdog 想写 0x420=10000，
|
||
* 实测读回恒 1000(=100ms)。进 OP 尝试期主站 200ms 才发一帧 LRW，
|
||
* 100ms 看门狗必过期 → ESC 锁存 0x440 并停发 SM2 事件；本从站
|
||
* 0x1C32/0x1C33 是 SM 同步模式(bEscIntEnabled=TRUE)，PDO 桥双向全靠
|
||
* SM2 事件驱动 → 事件一被抑制，输出不处理(ack 冻结)、输入不刷新
|
||
* (global_seq 冻结)，且 ESC 收写仍计 WKC，主站侧看似一切正常。
|
||
* PREOP 阶段写 1s：尝试期 200ms 节拍与稳态循环都远小于 1s。 */
|
||
if (ec_FPWRw(ec_slave[1].configadr, 0x0420, 10000, EC_TIMEOUTRET) > 0)
|
||
logs_add(logs, cap, lsz, "过程数据看门狗已写 1s (0x420=10000)");
|
||
else
|
||
logs_add(logs, cap, lsz, "WARN: 0x420 写 1s 失败，进 OP 尝试期可能被 100ms 看门狗熬过期");
|
||
|
||
/* 1.5) free-run 清残留 DC 激活：0x0981 跨断开重连不消失，残留会让从站在
|
||
* SAFEOP→OP 改等 PLL 序列而永久卡住。必须在 config_map 之前清。 */
|
||
if (!dc_cycle_us) {
|
||
ec_dcsync0(1, FALSE, 0, 0);
|
||
logs_add(logs, cap, lsz, "已清除残留 DC 激活(free-run)");
|
||
}
|
||
|
||
/* 2) 映射 PDO（config_map 内部经 CoE 读 0x1C12/0x1C13，并把从站推到 SAFEOP）*/
|
||
ec_config_map(&g_iomap);
|
||
ec_readstate();
|
||
g_expected_wkc = ec_group[0].outputsWKC * 2 + ec_group[0].inputsWKC;
|
||
for (i = 1; i <= ec_slavecount && i <= 16; i++)
|
||
snprintf(g_slave_names[i - 1], 64, "%s", ec_slave[i].name);
|
||
|
||
/* 3) 可选分布时钟 */
|
||
if (dc_cycle_us) {
|
||
if (!ec_configdc()) {
|
||
logs_add(logs, cap, lsz, "config_dc(): 总线上没有支持 DC 的从站 —— 检查 SII word1 高字节(0x0151 Sync/Latch)是否为 0xCC");
|
||
return 0;
|
||
}
|
||
ec_dcsync0(1, TRUE, (uint32)dc_cycle_us * 1000u, (int32)(dc_shift_us * 1000));
|
||
if (dc_shift_us) logs_add(logs, cap, lsz, "DC 已启用: SYNC0 周期 %dus, shift %dus", dc_cycle_us, dc_shift_us);
|
||
else logs_add(logs, cap, lsz, "DC 已启用: SYNC0 周期 %dus", dc_cycle_us);
|
||
}
|
||
snprintf(mode, sizeof mode, dc_cycle_us ? "DC %dus" : "free-run", dc_cycle_us);
|
||
|
||
/* 4) SAFEOP|ACK（幂等确认，SOEM 的 config_map 已请求过一次）*/
|
||
ec_slave[0].state = EC_STATE_SAFE_OP | EC_STATE_ACK;
|
||
ec_writestate(0);
|
||
if (ec_statecheck(0, EC_STATE_SAFE_OP, 2000000) != EC_STATE_SAFE_OP) {
|
||
ec_readstate();
|
||
logs_add(logs, cap, lsz, "进 SAFEOP 失败: AL=0x%02x code=0x%04x", ec_slave[1].state & 0x0F, ec_slave[1].ALstatuscode);
|
||
return 0;
|
||
}
|
||
logs_add(logs, cap, lsz, "已进入 SAFEOP");
|
||
|
||
/* 5) SAFEOP→OP：SSC 要求过程数据在流动才放行。喂帧间隔必须远小于从站
|
||
* 过程数据看门狗(0x420)：SSC 的 CheckIfEcatError 在 WD 触发位过期时
|
||
* 每 1ms 清一次 bEcatFirstOutputsReceived，挂起的 SAFEOP→OP 转换就永远
|
||
* 完成不了，~9s 后超时写 AL 0x001B 并禁用 SM2（之后 LRW 只剩读匹配，
|
||
* wkc 3→1）。从站固件 1.5.45 起自己把 0x420 写成 1s，但实测该写会不
|
||
* 生效（复位默认 100ms：divider 0x400=2498→100µs × time 0x420=1000），
|
||
* 所以主站不能依赖 WD 长度 —— 先以 ~5ms 喂 ≥1s 再请求 OP，之后继续
|
||
* 喂帧轮询状态（最多 ~3s）。对应 Python 时代"直写 0x420 双保险"的
|
||
* 替代方案：节奏比看门狗快，比任何 WD 设置都稳。 */
|
||
for (i = 0; i < 250; i++) {
|
||
ec_send_processdata();
|
||
ec_receive_processdata(EC_TIMEOUTRET);
|
||
Sleep(4);
|
||
}
|
||
ec_slave[0].state = EC_STATE_OPERATIONAL | EC_STATE_ACK;
|
||
ec_writestate(0);
|
||
for (i = 0; i < 600; i++) {
|
||
ec_send_processdata();
|
||
ec_receive_processdata(EC_TIMEOUTRET);
|
||
ec_readstate();
|
||
if ((ec_slave[1].state & 0x0F) == EC_STATE_OPERATIONAL) break;
|
||
if ((i % 50) == 49) { ec_slave[0].state = EC_STATE_OPERATIONAL | EC_STATE_ACK; ec_writestate(0); }
|
||
Sleep(4);
|
||
}
|
||
if ((ec_slave[1].state & 0x0F) != EC_STATE_OPERATIONAL) {
|
||
logs_add(logs, cap, lsz, "进 OP 失败（AL=0x%02x code=0x%04x）",
|
||
ec_slave[1].state & 0x0F, ec_slave[1].ALstatuscode);
|
||
return 0;
|
||
}
|
||
g_op = 1;
|
||
logs_add(logs, cap, lsz, "已进 OP(%s): 输出 %dB / 输入 %dB，expected_wkc=%lld",
|
||
mode, ec_slave[1].Obytes, ec_slave[1].Ibytes, g_expected_wkc);
|
||
return 1;
|
||
}
|
||
static void do_op_stop(void)
|
||
{
|
||
g_syncprev_valid = 0;
|
||
ec_slave[0].state = EC_STATE_INIT | EC_STATE_ACK;
|
||
ec_writestate(0);
|
||
ec_statecheck(0, EC_STATE_INIT, 1000000);
|
||
g_op = 0;
|
||
InterlockedExchange(&g_wedge_at, 0);
|
||
}
|
||
static void do_close(void)
|
||
{
|
||
g_syncprev_valid = 0;
|
||
if (g_bus_open) {
|
||
ec_slave[0].state = EC_STATE_INIT;
|
||
ec_writestate(0);
|
||
ec_close();
|
||
}
|
||
g_bus_open = 0;
|
||
g_op = 0;
|
||
g_desc[0] = 0;
|
||
InterlockedExchange(&g_wedge_at, 0);
|
||
}
|
||
|
||
/* 优雅退出：停循环 → 从站回 INIT → 关适配器 → 结束进程。shutdown 路由与
|
||
* --parent-pid 看门狗共用。强杀会让 LAN9252 停在 OP、SSC 邮箱可能楔死，
|
||
* 下次开机 config_map 一片 MailboxError，只能断电 —— 见记忆
|
||
* ecat-mailbox-wedge-needs-mcu-reset。 */
|
||
static void graceful_exit(const char *why)
|
||
{
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
do_close();
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
fprintf(stderr, "[native] 优雅退出(%s)\n", why);
|
||
fflush(stderr);
|
||
Sleep(150);
|
||
ExitProcess(0);
|
||
}
|
||
static DWORD WINAPI parent_watch(LPVOID p)
|
||
{
|
||
HANDLE h = OpenProcess(SYNCHRONIZE, FALSE, (DWORD)(DWORD_PTR)p);
|
||
if (!h) return 0;
|
||
WaitForSingleObject(h, INFINITE);
|
||
graceful_exit("父进程已退出");
|
||
return 0;
|
||
}
|
||
|
||
/* ── 陈旧实例清场（独立启动时；镜像 Python _kill_stale_instances）──
|
||
* 本工具要独占 Npcap 适配器：两个实例并存时后起的抓不到从站(身份读成
|
||
* M:00010001)，端口漂移又让它静默跑到别的口，页面永远"探测中→连接失败"。
|
||
* 优雅优先：对旧实例调 /api/shutdown —— 对 native 和 Python 实例都成立。
|
||
* 强杀兜底只按进程名杀 ecat_native.exe，绝不碰 python*.exe(会误杀用户程序)。 */
|
||
static int port_taken(int port)
|
||
{
|
||
SOCKET s = socket(AF_INET, SOCK_STREAM, 0);
|
||
struct sockaddr_in sa;
|
||
int r;
|
||
if (s == INVALID_SOCKET) return 0;
|
||
sa.sin_family = AF_INET;
|
||
sa.sin_port = htons((u_short)port);
|
||
sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
|
||
r = connect(s, (struct sockaddr *)&sa, sizeof sa); /* 回环上拒绝是瞬时的 */
|
||
closesocket(s);
|
||
return r == 0;
|
||
}
|
||
static void post_shutdown(int port)
|
||
{
|
||
static const char req[] =
|
||
"POST /api/shutdown HTTP/1.1\r\nHost: 127.0.0.1\r\n"
|
||
"Content-Type: application/json\r\nContent-Length: 2\r\n"
|
||
"Connection: close\r\n\r\n{}";
|
||
SOCKET s = socket(AF_INET, SOCK_STREAM, 0);
|
||
struct sockaddr_in sa;
|
||
char rb[256];
|
||
DWORD tv = 3000;
|
||
if (s == INVALID_SOCKET) return;
|
||
setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof tv);
|
||
sa.sin_family = AF_INET;
|
||
sa.sin_port = htons((u_short)port);
|
||
sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
|
||
if (connect(s, (struct sockaddr *)&sa, sizeof sa) == 0) {
|
||
send(s, req, (int)sizeof req - 1, 0);
|
||
while (recv(s, rb, sizeof rb, 0) > 0) { }
|
||
}
|
||
closesocket(s);
|
||
}
|
||
static void kill_stale_instances(void)
|
||
{
|
||
int p, acted = 0, killed = 0;
|
||
DWORD me = GetCurrentProcessId();
|
||
PROCESSENTRY32 pe;
|
||
HANDLE snap;
|
||
for (p = 8090; p <= 8095; p++) {
|
||
if (!port_taken(p)) continue;
|
||
post_shutdown(p);
|
||
printf("[native] 旧实例(端口 %d): 已请求优雅退出(从站回 INIT)\n", p);
|
||
acted = 1;
|
||
}
|
||
{
|
||
ULONGLONG dl = GetTickCount64() + 8000;
|
||
while (GetTickCount64() < dl) {
|
||
int any = 0;
|
||
for (p = 8090; p <= 8095; p++) if (port_taken(p)) any = 1;
|
||
if (!any) break;
|
||
Sleep(300);
|
||
}
|
||
}
|
||
snap = CreateToolhelp32Snapshot(TH32CS_SNAPPROCESS, 0);
|
||
if (snap != INVALID_HANDLE_VALUE) {
|
||
pe.dwSize = sizeof pe;
|
||
if (Process32First(snap, &pe)) do {
|
||
if (!_stricmp(pe.szExeFile, "ecat_native.exe") && pe.th32ProcessID != me) {
|
||
HANDLE h = OpenProcess(PROCESS_TERMINATE, FALSE, pe.th32ProcessID);
|
||
if (h) { TerminateProcess(h, 1); CloseHandle(h); killed++; }
|
||
}
|
||
} while (Process32Next(snap, &pe));
|
||
CloseHandle(snap);
|
||
}
|
||
if (killed) printf("[native] 强杀残留 ecat_native 进程 %d 个\n", killed);
|
||
if (acted) Sleep(1000); /* Npcap 句柄随进程退出由内核回收，给它一拍 */
|
||
}
|
||
|
||
/* ── 分位数（读侧现算，同 _cyc_fresh 口径）── */
|
||
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);
|
||
}
|
||
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 工具 ── */
|
||
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 json_str(const char *body, const char *key, char *out, int osz)
|
||
{
|
||
const char *p = strstr(body, key);
|
||
if (!p) { out[0] = 0; return; }
|
||
p += strlen(key);
|
||
while (*p && *p != ':') p++;
|
||
if (*p != ':') { out[0] = 0; return; }
|
||
p++;
|
||
while (*p == ' ' || *p == '"' || *p == '\t') p++;
|
||
int i = 0;
|
||
while (*p && *p != '"' && i < osz - 1) out[i++] = *p++;
|
||
out[i] = 0;
|
||
}
|
||
static void jesc(char *dst, size_t dsz, const char *src)
|
||
{
|
||
size_t o = 0;
|
||
for (; *src && o + 6 < dsz; src++) {
|
||
unsigned char c = (unsigned char)*src;
|
||
if (c == '"' || c == '\\') { dst[o++] = '\\'; dst[o++] = (char)c; }
|
||
else if (c < 0x20) { o += (size_t)snprintf(dst + o, dsz - o, "\\u%04x", c); }
|
||
else dst[o++] = (char)c;
|
||
}
|
||
dst[o] = 0;
|
||
}
|
||
/* 提取 key 的原始 token（字符串去引号 / 数字原样） */
|
||
static int json_tok(const char *obj, const char *key, char *tok, int tsz)
|
||
{
|
||
const char *p = obj;
|
||
int n = (int)strlen(key);
|
||
for (;;) {
|
||
p = strstr(p, key);
|
||
if (!p) return 0;
|
||
if (p > obj && (isalnum((unsigned char)p[-1]) || p[-1] == '_')) { p += n; continue; }
|
||
p += n;
|
||
/* JSON 键带引号："id" 匹配后下一个字符是闭合引号，必须一并跳过才见 ':' */
|
||
while (*p == ' ' || *p == '\t' || *p == '"') p++;
|
||
if (*p != ':') continue;
|
||
p++;
|
||
while (*p == ' ' || *p == '\t') p++;
|
||
if (*p == '"') {
|
||
int i = 0; p++;
|
||
while (*p && *p != '"' && i < tsz - 1) tok[i++] = *p++;
|
||
tok[i] = 0;
|
||
return 1;
|
||
} else {
|
||
int i = 0;
|
||
while (*p && *p != ',' && *p != '}' && *p != ']' && *p != ' ' && i < tsz - 1) tok[i++] = *p++;
|
||
tok[i] = 0;
|
||
return i > 0;
|
||
}
|
||
}
|
||
}
|
||
static int hexval(char c)
|
||
{
|
||
if (c >= '0' && c <= '9') return c - '0';
|
||
if (c >= 'a' && c <= 'f') return c - 'a' + 10;
|
||
if (c >= 'A' && c <= 'F') return c - 'A' + 10;
|
||
return -1;
|
||
}
|
||
static int hex_decode(const char *s, uint8 *out, int cap)
|
||
{
|
||
int n = 0;
|
||
while (*s && n < cap) {
|
||
int hi, lo;
|
||
while (*s == ' ') s++;
|
||
if (!*s) break;
|
||
hi = hexval(*s++);
|
||
lo = hexval(*s++);
|
||
if (hi < 0 || lo < 0) break;
|
||
out[n++] = (uint8)((hi << 4) | lo);
|
||
}
|
||
return n;
|
||
}
|
||
static void hex_encode(const uint8 *d, int n, char *out)
|
||
{
|
||
static const char hx[] = "0123456789ABCDEF";
|
||
int i;
|
||
for (i = 0; i < n; i++) { out[i * 2] = hx[d[i] >> 4]; out[i * 2 + 1] = hx[d[i] & 0xF]; }
|
||
out[n * 2] = 0;
|
||
}
|
||
static int parse_frame(const char *obj, frame_t *f)
|
||
{
|
||
char tok[160];
|
||
double v;
|
||
memset(f, 0, sizeof *f);
|
||
if (json_tok(obj, "ch", tok, sizeof tok)) f->ch = (int)strtol(tok, NULL, 0);
|
||
if (json_tok(obj, "id", tok, sizeof tok)) {
|
||
if (tok[0] == '0' && (tok[1] == 'x' || tok[1] == 'X')) f->id = (uint32)strtoul(tok + 2, NULL, 16);
|
||
else if (strchr(tok, 'x') || strchr(tok, 'X')) {
|
||
char *xp = strchr(tok, 'x');
|
||
if (!xp) xp = strchr(tok, 'X');
|
||
f->id = (uint32)strtoul(xp + 1, NULL, 16);
|
||
}
|
||
else f->id = (uint32)strtoul(tok, NULL, 10);
|
||
}
|
||
if (json_tok(obj, "data", tok, sizeof tok) && tok[0])
|
||
f->dlc = hex_decode(tok, f->data, CAN_FD_DATA_MAX);
|
||
if (json_num(obj, "dlc", &v) && v >= 0) {
|
||
int want = (int)v;
|
||
if (want > f->dlc) { /* data 短于 dlc：ljust 补 0（memset 已保证） */ }
|
||
f->dlc = want;
|
||
}
|
||
if (f->dlc > CAN_FD_DATA_MAX) return -1;
|
||
return 0;
|
||
}
|
||
|
||
static void send_http(SOCKET cs, int code, const char *ctype, const char *body, int len)
|
||
{
|
||
char hdr[256];
|
||
int hl = snprintf(hdr, sizeof hdr,
|
||
"HTTP/1.1 %d %s\r\nContent-Type: %s\r\nCache-Control: no-store\r\n"
|
||
"Content-Length: %d\r\nConnection: close\r\n\r\n",
|
||
code, code == 200 ? "OK" : "Not Found", ctype, len);
|
||
send(cs, hdr, hl, 0);
|
||
if (len > 0) send(cs, body, len, 0);
|
||
}
|
||
static void send_json(SOCKET cs, const char *body) { send_http(cs, 200, "application/json; charset=utf-8", body, (int)strlen(body)); }
|
||
|
||
static const char *ctype_for(const char *path)
|
||
{
|
||
const char *e = strrchr(path, '.');
|
||
if (!e) return "application/octet-stream";
|
||
if (!strcmp(e, ".html")) return "text/html; charset=utf-8";
|
||
if (!strcmp(e, ".css")) return "text/css; charset=utf-8";
|
||
if (!strcmp(e, ".js")) return "application/javascript; charset=utf-8";
|
||
return "application/octet-stream";
|
||
}
|
||
static void serve_static(SOCKET cs, const char *path)
|
||
{
|
||
const char *rel = "ecat.html";
|
||
char fp[1024];
|
||
FILE *f;
|
||
char *buf;
|
||
long sz;
|
||
if (!strcmp(path, "/motor.html")) rel = "motor.html";
|
||
else if (!strcmp(path, "/ecat.css")) rel = "ecat.css";
|
||
else if (!strcmp(path, "/motor.js")) rel = "motor.js";
|
||
else if (!strcmp(path, "/") || !strcmp(path, "/index.html") || !strcmp(path, "/ecat.html")) rel = "ecat.html";
|
||
else { send_http(cs, 404, "text/plain", "missing", 7); return; }
|
||
_snprintf(fp, sizeof fp, "%s/%s", g_webdir, rel);
|
||
f = fopen(fp, "rb");
|
||
if (!f) { send_http(cs, 404, "text/plain", "missing", 7); return; }
|
||
fseek(f, 0, SEEK_END); sz = ftell(f); fseek(f, 0, SEEK_SET);
|
||
buf = (char *)malloc(sz > 0 ? sz : 1);
|
||
if (buf && fread(buf, 1, sz, f) == (size_t)sz)
|
||
send_http(cs, 200, ctype_for(rel), buf, (int)sz);
|
||
else
|
||
send_http(cs, 500, "text/plain", "read error", 10);
|
||
free(buf);
|
||
fclose(f);
|
||
}
|
||
|
||
/* ── 快照 JSON（motors/bridge/xrx/rx_batch/head + cyc_us）── */
|
||
static void emit_bridge_json(char *resp, int rsz, int *prl)
|
||
{
|
||
char hx[CAN_RX_DATA_MAX * 2 + 2], en[96];
|
||
int rl = *prl, d;
|
||
bridge_t b;
|
||
EnterCriticalSection(&g_state_lock);
|
||
b = g_bridge;
|
||
LeaveCriticalSection(&g_state_lock);
|
||
if (!b.valid) { rl += snprintf(resp + rl, rsz - rl, "{\"valid\":false}"); *prl = rl; return; }
|
||
d = b.dlc > CAN_RX_DATA_MAX ? CAN_RX_DATA_MAX : b.dlc;
|
||
memset(hx, 0, sizeof hx);
|
||
hex_encode(b.data, d, hx);
|
||
jesc(en, sizeof en,
|
||
b.tx_result == 0x00 ? "OK" :
|
||
b.tx_result == 0xE2 ? "BUS_OFF" :
|
||
b.tx_result == 0xE3 ? "TEC 过高" :
|
||
b.tx_result == 0xE4 ? "邮箱满" :
|
||
b.tx_result == 0xE5 ? "参数无效" : "");
|
||
if (en[0] == 0) snprintf(en, sizeof en, "0x%02X", b.tx_result);
|
||
rl += snprintf(resp + rl, rsz - rl,
|
||
"{\"valid\":true,\"ack\":%d,\"rx_valid\":%s,\"truncated\":%s,\"ch\":%d,\"dlc\":%d,"
|
||
"\"id\":%u,\"data\":\"%s\",\"rx_count\":%u,\"tx_result\":%d,\"backlog\":%d,"
|
||
"\"tx_count\":%d,\"drop\":%d,\"tx_result_text\":\"%s\"}",
|
||
b.ack, (b.flags & 0x01) ? "true" : "false", (b.flags & 0x02) ? "true" : "false",
|
||
b.ch, b.dlc, b.id, hx, b.rx_count, b.tx_result, b.backlog,
|
||
b.tx_count, b.drop, en);
|
||
*prl = rl;
|
||
}
|
||
/* Python 的 can/send、can/state 用 {**st} 把桥字段拍平到顶层 —— ecat.html 的
|
||
* renderCanState 直接读 r.rx_count/r.tx_result_text。每键自带前置逗号，
|
||
* 调用方先写完自己的键再接上即可。 */
|
||
/* tx_result 文本（Python CAN_TX_RESULT_DESC 同一张表）。返回内部静态串，只读短用。 */
|
||
static const char *bridge_tx_text(uint8 code)
|
||
{
|
||
switch (code) {
|
||
case 0x00: return "OK";
|
||
case 0xE2: return "BUS_OFF";
|
||
case 0xE3: return "TEC 过高";
|
||
case 0xE4: return "邮箱满";
|
||
case 0xE5: return "参数无效";
|
||
default: return ""; /* 调用方格式化 0x%02X */
|
||
}
|
||
}
|
||
|
||
static void emit_bridge_flat(char *resp, int rsz, int *prl, const bridge_t *b)
|
||
{
|
||
char hx[CAN_RX_DATA_MAX * 2 + 2], en[96];
|
||
int rl = *prl, d;
|
||
if (!b->valid) { rl += snprintf(resp + rl, rsz - rl, ",\"valid\":false"); *prl = rl; return; }
|
||
d = b->dlc > CAN_RX_DATA_MAX ? CAN_RX_DATA_MAX : b->dlc;
|
||
memset(hx, 0, sizeof hx);
|
||
hex_encode(b->data, d, hx);
|
||
jesc(en, sizeof en,
|
||
b->tx_result == 0x00 ? "OK" :
|
||
b->tx_result == 0xE2 ? "BUS_OFF" :
|
||
b->tx_result == 0xE3 ? "TEC 过高" :
|
||
b->tx_result == 0xE4 ? "邮箱满" :
|
||
b->tx_result == 0xE5 ? "参数无效" : "");
|
||
if (en[0] == 0) snprintf(en, sizeof en, "0x%02X", b->tx_result);
|
||
rl += snprintf(resp + rl, rsz - rl,
|
||
",\"valid\":true,\"ack\":%d,\"rx_valid\":%s,\"truncated\":%s,\"ch\":%d,\"dlc\":%d,"
|
||
"\"id\":%u,\"data\":\"%s\",\"rx_count\":%u,\"tx_result\":%d,\"backlog\":%d,"
|
||
"\"tx_count\":%d,\"drop\":%d,\"tx_result_text\":\"%s\"",
|
||
b->ack, (b->flags & 0x01) ? "true" : "false", (b->flags & 0x02) ? "true" : "false",
|
||
b->ch, b->dlc, b->id, hx, b->rx_count, b->tx_result, b->backlog,
|
||
b->tx_count, b->drop, en);
|
||
*prl = rl;
|
||
}
|
||
|
||
static void emit_xrx_json(char *resp, int rsz, int *prl)
|
||
{
|
||
char hx[CAN_XRX_DATA_MAX * 2 + 2]; int rl = *prl;
|
||
xrx_t x;
|
||
EnterCriticalSection(&g_state_lock);
|
||
x = g_xrx;
|
||
LeaveCriticalSection(&g_state_lock);
|
||
if (!x.valid) { rl += snprintf(resp + rl, rsz - rl, "null"); *prl = rl; return; }
|
||
hex_encode(x.data, x.n, hx);
|
||
rl += snprintf(resp + rl, rsz - rl, "{\"ch\":%d,\"dlc\":%d,\"id\":%u,\"data\":\"%s\"}",
|
||
x.ch, x.dlc, x.id, hx);
|
||
*prl = rl;
|
||
}
|
||
static void emit_rxb_json(char *resp, int rsz, int *prl, int with_frames)
|
||
{
|
||
char hx[CAN_FD_DATA_MAX * 2 + 2];
|
||
int rl = *prl, i;
|
||
rxb_t rb;
|
||
EnterCriticalSection(&g_state_lock);
|
||
rb = g_rxb;
|
||
LeaveCriticalSection(&g_state_lock);
|
||
rl += snprintf(resp + rl, rsz - rl, "{\"count\":%d,\"lost\":%d,\"pending\":%d",
|
||
rb.count, rb.lost, rb.pending);
|
||
if (with_frames) {
|
||
rl += snprintf(resp + rl, rsz - rl, ",\"frames\":[");
|
||
for (i = 0; i < rb.n; i++) {
|
||
hex_encode(rb.fr[i].data, rb.fr[i].dlc, hx);
|
||
rl += snprintf(resp + rl, rsz - rl, "%s{\"ch\":%d,\"dlc\":%d,\"valid\":true,\"id\":%u,\"data\":\"%s\"}",
|
||
i ? "," : "", rb.fr[i].ch, rb.fr[i].dlc, rb.fr[i].id, hx);
|
||
}
|
||
rl += snprintf(resp + rl, rsz - rl, "]");
|
||
} else {
|
||
rl += snprintf(resp + rl, rsz - rl, ",\"frames\":[]}");
|
||
*prl = rl;
|
||
return;
|
||
}
|
||
rl += snprintf(resp + rl, rsz - rl, "}");
|
||
*prl = rl;
|
||
}
|
||
static void build_state_json(char *resp, int rsz)
|
||
{
|
||
char extra[4096];
|
||
slot_t slots[MOTOR_SLOT_COUNT];
|
||
int i, j, rl, ord[MOTOR_SLOT_COUNT], no = 0, qdepth;
|
||
int n_bases, bases[4], hv, active, boot, wbase, wcount;
|
||
uint32 gseq;
|
||
double period, slot_period;
|
||
long long cycles, expected;
|
||
int wkc, seq, frames;
|
||
|
||
build_cyc_us(extra, sizeof extra);
|
||
EnterCriticalSection(&g_state_lock);
|
||
memcpy(slots, g_slots, sizeof slots);
|
||
hv = g_head.valid; active = g_head.active; boot = g_head.boot;
|
||
wbase = g_head.wbase; wcount = g_head.wcount; gseq = g_head.gseq;
|
||
n_bases = g_n_bases; if (n_bases > 4) n_bases = 4;
|
||
memcpy(bases, g_bases, n_bases * sizeof(int));
|
||
period = g_period_ms; cycles = g_total; expected = g_expected_wkc; wkc = (int)g_wkc;
|
||
seq = g_sent_seq; frames = g_sent_frames;
|
||
slot_period = period * (n_bases > 0 ? n_bases : 1);
|
||
LeaveCriticalSection(&g_state_lock);
|
||
EnterCriticalSection(&g_cmd_lock); qdepth = g_cmd_len; LeaveCriticalSection(&g_cmd_lock);
|
||
|
||
for (i = 0; i < MOTOR_SLOT_COUNT; i++)
|
||
if (slots[i].node != 0) ord[no++] = i;
|
||
for (i = 1; i < no; i++)
|
||
for (j = i; j > 0; j++) {
|
||
int a = ord[j - 1], c = ord[j];
|
||
if (slots[a].ch > slots[c].ch || (slots[a].ch == slots[c].ch && slots[a].node > slots[c].node)) {
|
||
ord[j - 1] = c; ord[j] = a;
|
||
} else break;
|
||
}
|
||
|
||
rl = snprintf(resp, rsz,
|
||
"{\"ok\":true,\"log\":[],\"cycles\":%lld,\"period_ms\":%.4f,\"spin\":true,"
|
||
"\"wkc\":%d,\"expected_wkc\":%lld,\"slot_period_ms\":%.4f,\"win_bases\":[",
|
||
cycles, period, wkc, expected, slot_period);
|
||
for (i = 0; i < n_bases; i++) rl += snprintf(resp + rl, rsz - rl, "%s%d", i ? "," : "", bases[i]);
|
||
rl += snprintf(resp + rl, rsz - rl, "],\"queue_depth\":%d,\"sent_seq\":%d,\"sent_frames\":%d,\"ts\":0,\"head\":",
|
||
qdepth, seq, frames);
|
||
if (hv)
|
||
rl += snprintf(resp + rl, rsz - rl,
|
||
"{\"active\":%d,\"boot_active\":%s,\"win_base\":%d,\"win_count\":%d,\"global_seq\":%u}",
|
||
active, boot ? "true" : "false", wbase, wcount, gseq);
|
||
else rl += snprintf(resp + rl, rsz - rl, "{}");
|
||
rl += snprintf(resp + rl, rsz - rl, ",\"motors\":[");
|
||
for (i = 0; i < no; i++) {
|
||
slot_t *s = &slots[ord[i]];
|
||
char hx[MOT_STATUS_DLC * 2 + 2];
|
||
hex_encode(s->data, s->dlc > MOT_STATUS_DLC ? MOT_STATUS_DLC : s->dlc, hx);
|
||
rl += snprintf(resp + rl, rsz - rl,
|
||
"%s{\"slot\":%d,\"node\":%d,\"ch\":%d,\"online\":%s,\"dlc\":%d,\"seq\":%u,\"data\":\"%s\"}",
|
||
i ? "," : "", ord[i], s->node, s->ch, s->online ? "true" : "false",
|
||
s->dlc, s->seq, hx);
|
||
}
|
||
rl += snprintf(resp + rl, rsz - rl, "],\"bridge\":");
|
||
emit_bridge_json(resp, rsz, &rl);
|
||
rl += snprintf(resp + rl, rsz - rl, ",\"xrx\":");
|
||
emit_xrx_json(resp, rsz, &rl);
|
||
rl += snprintf(resp + rl, rsz - rl, ",\"rx_batch\":");
|
||
emit_rxb_json(resp, rsz, &rl, 1);
|
||
rl += snprintf(resp + rl, rsz - rl, ",\"wr\":%lld,%s}", g_wedge_recoveries, extra);
|
||
}
|
||
|
||
/* ═════════ 阶段 3 辅助：SDO / EEPROM / sync_diag ═════════ */
|
||
|
||
/* EEPROM 类路由的大缓冲：4096 字全差异时日志上十万字节，放不进栈。做成文件级
|
||
* static 而不是函数内 static，是为了让"靠 g_bus_lock 串行化"这件事在声明处就看得见
|
||
* —— 五条阶段 3 路由全部先 EnterCriticalSection(&g_bus_lock) 再碰它。 */
|
||
static char g_ee_logs[160 * 1024];
|
||
static uint8 g_ee_data[EEPROM_SIZE];
|
||
static char g_ee_hex[2 * EEPROM_SIZE + 8];
|
||
|
||
/* pysoem Slave.eeprom_read(word) → uint32(低 16 位是数据字)。EC_VER1 封装在从站号上
|
||
* 做 eeprom2master 仲裁；单从站总线直接用 slave 1，与 Python 的 slaves[0] 同物。 */
|
||
static uint16 ee_read_word(uint16 word)
|
||
{
|
||
return (uint16)(ec_readeeprom(1, word, EC_TIMEOUTEEP) & 0xFFFF);
|
||
}
|
||
static int ee_write_word(uint16 word, uint16 val)
|
||
{
|
||
return ec_writeeeprom(1, word, val, EC_TIMEOUTEEP) > 0;
|
||
}
|
||
|
||
/* 往 JSON 字符串数组里追加一条(已转义中文/ASCII，无特殊字符)。调用方负责
|
||
* 前后括号。返回新长度。
|
||
* 缓冲是 static(4096 字 EEPROM 的日志放不进栈)且跨请求复用，故每次追加都必须
|
||
* 补 NUL：漏了就会把上一次请求的残留日志一起 %s 出去(实测 verify 返回
|
||
* "OK, 全部一致" 后面粘着上一轮的 449 行差异)。 */
|
||
static int logs_add(char *logs, int cap, int *lp, const char *fmt, ...)
|
||
{
|
||
char tmp[256];
|
||
va_list ap;
|
||
int add, o;
|
||
va_start(ap, fmt);
|
||
add = vsnprintf(tmp, sizeof tmp, fmt, ap);
|
||
va_end(ap);
|
||
if (add < 0) return *lp;
|
||
if (*lp + add + 4 >= cap) return *lp; /* 溢出保护：日志截断可接受 */
|
||
o = *lp;
|
||
if (o) logs[o++] = ',';
|
||
logs[o++] = '"';
|
||
memcpy(logs + o, tmp, add);
|
||
o += add;
|
||
logs[o++] = '"';
|
||
logs[o] = 0;
|
||
*lp = o;
|
||
return o;
|
||
}
|
||
|
||
static void handle_conn(SOCKET cs)
|
||
{
|
||
char *buf = (char *)malloc(65536);
|
||
char *resp = (char *)malloc(512 * 1024);
|
||
char msg[512], substr[64] = "I226";
|
||
int len = 0, i, cl = 0, rl;
|
||
char *hdr_end, *body, method[8] = "", path[128] = "";
|
||
|
||
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;
|
||
{
|
||
char *p;
|
||
for (p = strstr(buf, "\n"); p && p < hdr_end; p = strstr(p + 1, "\n"))
|
||
if (!_strnicmp(p + 1, "content-length:", 15)) { cl = atoi(p + 16); break; }
|
||
while (cl > 0 && (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);
|
||
if (cl >= 0) body[cl] = 0;
|
||
}
|
||
if (sscanf(buf, "%7s %127s", method, path) != 2) goto done;
|
||
|
||
if (!strcmp(method, "GET")) { serve_static(cs, path); goto done; }
|
||
|
||
/* focus：Python 里是"把 GUI 窗口带到前台"的信号(esp32_server._focus_existing
|
||
* 用它探活)。native 无窗口，只需如实应答存在。 */
|
||
if (!strcmp(path, "/api/focus")) { send_json(cs, "{\"ok\":true,\"focused\":true}"); goto done; }
|
||
|
||
if (!strcmp(path, "/api/adapters")) {
|
||
ec_adaptert *ad; int idx = 0; char ed[512], en[512];
|
||
rl = snprintf(resp, 512 * 1024, "{\"adapters\":[");
|
||
for (ad = ec_find_adapters(); ad; ad = ad->next, idx++) {
|
||
jesc(ed, sizeof ed, ad->desc); jesc(en, sizeof en, ad->name);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "%s{\"index\":%d,\"desc\":\"%s\",\"name\":\"%s\"}",
|
||
idx ? "," : "", idx, ed, en);
|
||
}
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "]}");
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/open")) {
|
||
int ok;
|
||
json_str(body, "substr", substr, sizeof substr);
|
||
if (!substr[0]) strcpy(substr, "I226");
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
ok = do_open(substr, msg, sizeof msg);
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":%s,\"log\":[\"%s\"]}", ok ? "true" : "false", msg);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/status")) {
|
||
char en[160], ed[320];
|
||
if (!g_bus_open) { send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done; }
|
||
EnterCriticalSection(&g_bus_lock);
|
||
ec_readstate();
|
||
jesc(en, sizeof en, g_slave_names[0]); jesc(ed, sizeof ed, g_desc);
|
||
rl = snprintf(resp, 512 * 1024,
|
||
"{\"ok\":true,\"slave\":{\"name\":\"%s\",\"state\":%d,\"al_status\":%d,\"desc\":\"%s\"}}",
|
||
en, ec_slave[1].state & 0x0F, ec_slave[1].ALstatuscode, ed);
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/close")) {
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
do_close();
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":true,\"log\":[\"已断开\"]}"); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/shutdown")) {
|
||
send_json(cs, "{\"ok\":true,\"log\":[\"后台服务即将退出; 双击 ecat_web.vbs 可再次启动\"]}");
|
||
Sleep(300); /* 让响应在回环上真正发出去 */
|
||
graceful_exit("shutdown 路由");
|
||
goto done;
|
||
}
|
||
if (!strcmp(path, "/api/op/start")) {
|
||
double v;
|
||
int dc_cycle = 0, dc_shift = 0, ok, lsz = 0;
|
||
if (json_num(body, "dc_cycle_us", &v)) dc_cycle = (int)v;
|
||
if (json_num(body, "dc_shift_us", &v)) dc_shift = (int)v;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
g_ee_logs[0] = 0;
|
||
ok = do_op_start(dc_cycle, dc_shift, g_ee_logs, sizeof g_ee_logs, &lsz);
|
||
if (!ok) {
|
||
/* 失败自动重试一次(api_op_start 同款)：入口预清理=重枚举，重试即"断开重连" */
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "自动重试:重新执行完整序列(入口含重枚举预清理)…");
|
||
ok = do_op_start(dc_cycle, dc_shift, g_ee_logs, sizeof g_ee_logs, &lsz);
|
||
}
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":%s,\"log\":[%s]}", ok ? "true" : "false", g_ee_logs);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/op/stop")) {
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
do_op_stop();
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":true,\"log\":[\"已退回 INIT\"]}"); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/motor/start")) {
|
||
double v;
|
||
int dc_cycle = 0, dc_shift = 0, lsz = 0;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
g_ee_logs[0] = 0;
|
||
loop_stop();
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
if (json_num(body, "dc_cycle_us", &v)) dc_cycle = (int)v;
|
||
if (json_num(body, "dc_shift_us", &v)) dc_shift = (int)v;
|
||
/* g_op 只是"上次进过 OP"的记忆，不是从站真实状态：循环停超 1000ms 过程
|
||
* 数据看门狗就把它踢出 OP(0x001B)，这里必须现读核实(api_motor_start 注释)。 */
|
||
if (g_op) {
|
||
ec_readstate();
|
||
if ((ec_slave[1].state & 0x0F) != EC_STATE_OPERATIONAL) {
|
||
g_op = 0;
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz,
|
||
"检测到从站已掉出 OP(过程数据看门狗超时)，重新走 PREOP→SAFEOP→OP");
|
||
}
|
||
}
|
||
if (!g_op && !do_op_start(dc_cycle, dc_shift, g_ee_logs, sizeof g_ee_logs, &lsz)) {
|
||
g_op = 0;
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[%s]}", g_ee_logs);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (json_num(body, "period_ms", &v) && v >= 0.0625 && v <= 1000.0) {
|
||
EnterCriticalSection(&g_cfg_lock); g_period_ms = v; LeaveCriticalSection(&g_cfg_lock);
|
||
}
|
||
loop_start();
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz,
|
||
"后台 PDO 循环已启动(native): 周期 %gms，窗口 7 槽/周期，"
|
||
"窗口自适应(只覆盖有主的槽，在线电机每周期都刷新)，每 64 周期插一次发现扫描以找新电机",
|
||
g_period_ms);
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz,
|
||
"节拍源: 自旋(native 恒自旋: 周期跑得准，但会占满一个 P 核)");
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"log\":[%s],\"period_ms\":%.4f,\"spin\":true}",
|
||
g_ee_logs, g_period_ms);
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/motor/stop")) {
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":true,\"log\":[\"PDO 循环已停\"]}"); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/motor/spin")) {
|
||
send_json(cs, "{\"ok\":true,\"log\":[\"native 亚毫秒恒自旋，spin 无副作用\"]}"); goto done;
|
||
}
|
||
/* motor/send：循环活着 → 排进队列(worker 同 seq 沿批量下发)；循环没跑 →
|
||
* 同步下发（Python 的 fallback：排队等于让命令烂在队列里）。非独占路由，
|
||
* 但要拿 g_bus_lock 双检 g_loop_running —— 防"检查后、踩周期前"motor/start 插进来。 */
|
||
if (!strcmp(path, "/api/motor/send")) {
|
||
static frame_t fr[64];
|
||
int nf = 0, backlog = 0, ok = 1;
|
||
double v;
|
||
const char *fa, *p;
|
||
char desc[2560];
|
||
int dpos = 0, lsz = 0, dc_cycle = 0, dc_shift = 0;
|
||
if (json_num(body, "dc_cycle_us", &v)) dc_cycle = (int)v;
|
||
if (json_num(body, "dc_shift_us", &v)) dc_shift = (int)v;
|
||
fa = strstr(body, "\"frames\"");
|
||
if (fa && (p = strchr(fa, '[')) != NULL) {
|
||
while (nf < 64 && (p = strchr(p, '{')) != NULL) {
|
||
int depth = 0, j;
|
||
for (j = 0; p[j]; j++) {
|
||
if (p[j] == '{') depth++;
|
||
else if (p[j] == '}') { depth--; if (!depth) break; }
|
||
}
|
||
if (depth) break;
|
||
{
|
||
char obj[2048];
|
||
int on = j + 1;
|
||
if (on >= (int)sizeof obj) on = (int)sizeof obj - 1;
|
||
memcpy(obj, p, on); obj[on] = 0;
|
||
if (parse_frame(obj, &fr[nf]) == 0) nf++;
|
||
else { ok = 0; break; }
|
||
}
|
||
p = p + j + 1;
|
||
}
|
||
} else {
|
||
if (parse_frame(body, &fr[0]) == 0) nf = 1;
|
||
else ok = 0;
|
||
}
|
||
if (!ok) {
|
||
int dlc64 = -1;
|
||
if (json_num(body, "dlc", &v)) dlc64 = (int)v;
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"DLC %d 超出 CANFD 上限 64\"]}", dlc64);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!g_bus_open) { send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done; }
|
||
for (i = 0; i < nf; i++) {
|
||
if (fr[i].ch < 0 || fr[i].ch > 2) {
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"通道必须是 0/1/2，收到 %d\"]}", fr[i].ch);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
dpos += snprintf(desc + dpos, sizeof desc - dpos, "%sCH%d 0x%X DLC%d",
|
||
dpos ? "; " : "", fr[i].ch, fr[i].id, fr[i].dlc);
|
||
}
|
||
EnterCriticalSection(&g_bus_lock);
|
||
/* 镜像 Python api_motor_send 的顺序：先入队，再看循环活着没 —— 同步 fallback
|
||
* 因此还会顺带清掉更早的积压（那些帧本来也该发）。 */
|
||
EnterCriticalSection(&g_cmd_lock);
|
||
for (i = 0; i < nf; i++) {
|
||
if (g_cmd_len >= CMD_Q) break;
|
||
g_cmd_q[(g_cmd_head + g_cmd_len) % CMD_Q] = fr[i];
|
||
g_cmd_len++;
|
||
}
|
||
backlog = g_cmd_len;
|
||
LeaveCriticalSection(&g_cmd_lock);
|
||
if (g_loop_running) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024,
|
||
"{\"ok\":true,\"queued\":%d,\"backlog\":%d,\"batch_max_bytes\":%d,\"log\":[]}",
|
||
nf, backlog, TXB_BYTES);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
/* 循环没在跑：备好 OP 后同步下发（Python 的 _require_op_ready fallback）。
|
||
* 备好 OP 失败时帧留在队列里 —— 与 Python 返回 ok:false 但不丢帧一致。 */
|
||
loop_stop();
|
||
g_ee_logs[0] = 0;
|
||
if (!g_op && !do_op_start(dc_cycle, dc_shift, g_ee_logs, sizeof g_ee_logs, &lsz)) {
|
||
g_op = 0;
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[%s]}", g_ee_logs);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
{
|
||
int n = 0, cycles = 0;
|
||
frame_t first;
|
||
static frame_t bfr[64];
|
||
for (;;) {
|
||
int have, nb, q;
|
||
EnterCriticalSection(&g_cmd_lock);
|
||
have = g_cmd_len;
|
||
if (have == 0) {
|
||
LeaveCriticalSection(&g_cmd_lock);
|
||
break;
|
||
}
|
||
first = g_cmd_q[g_cmd_head];
|
||
g_cmd_head = (g_cmd_head + 1) % CMD_Q;
|
||
g_cmd_len--;
|
||
have--;
|
||
nb = have > 32 ? 32 : have;
|
||
for (q = 0; q < nb; q++) bfr[q] = g_cmd_q[(g_cmd_head + q) % CMD_Q];
|
||
nb = batch_capacity(bfr, nb);
|
||
g_cmd_head = (g_cmd_head + nb) % CMD_Q;
|
||
g_cmd_len -= nb;
|
||
LeaveCriticalSection(&g_cmd_lock);
|
||
fill_can_out(&first, nb ? bfr : NULL, nb, 0);
|
||
pump_cycles(2, NULL);
|
||
n += 1 + nb;
|
||
cycles++;
|
||
}
|
||
rl = snprintf(resp, 512 * 1024,
|
||
"{\"ok\":true,\"queued\":0,\"sent_sync\":%d,\"pdo_cycles\":%d,"
|
||
"\"log\":[\"后台循环未运行，已同步下发 %d 帧（%d 个 PDO 周期）: ", n, cycles, n, cycles);
|
||
if (dpos >= (int)sizeof desc) dpos = (int)sizeof desc - 1;
|
||
memcpy(resp + rl, desc, dpos + 1); rl += dpos;
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "\"]}");
|
||
}
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
/* can/send（ecat.html CAN 调试页）：独占 + 3 拍 pump，**同步回读 ack**，桥字段
|
||
* 拍平进响应（renderCanState 直接读顶层）。与 motor/send 不同，它不走队列。 */
|
||
if (!strcmp(path, "/api/can/send")) {
|
||
frame_t f;
|
||
double v;
|
||
int lsz = 0, dc_cycle = 0, dc_shift = 0, clr = 0, wkc, seq, acked;
|
||
bridge_t b;
|
||
char tok[16], dhs[CAN_TX_DATA_MAX * 2 + 2];
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
g_ee_logs[0] = 0;
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
if (json_num(body, "dc_cycle_us", &v)) dc_cycle = (int)v;
|
||
if (json_num(body, "dc_shift_us", &v)) dc_shift = (int)v;
|
||
if (!g_op && !do_op_start(dc_cycle, dc_shift, g_ee_logs, sizeof g_ee_logs, &lsz)) {
|
||
g_op = 0;
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[%s]}", g_ee_logs);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (json_num(body, "dlc", &v) && (int)v > CAN_TX_DATA_MAX) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"DLC %d 超出 PDO 单帧上限 %d\"]}", (int)v, CAN_TX_DATA_MAX);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (parse_frame(body, &f) < 0) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"DLC 超出 PDO 单帧上限 32\"]}"); goto done;
|
||
}
|
||
if (f.ch < 0 || f.ch > 2) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"通道必须是 0/1/2，收到 %d\"]}", f.ch);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (json_tok(body, "clear_stats", tok, sizeof tok) &&
|
||
(tok[0] == 't' || tok[0] == 'T' || tok[0] == '1')) clr = 1;
|
||
memset(dhs, 0, sizeof dhs);
|
||
hex_encode(f.data, f.dlc > CAN_TX_DATA_MAX ? CAN_TX_DATA_MAX : f.dlc, dhs);
|
||
seq = fill_can_out(&f, NULL, 0, clr);
|
||
wkc = pump_cycles(3, &b);
|
||
acked = (b.valid && b.ack == seq);
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "已下发 CH%d 0x%X DLC%d %s seq=%d",
|
||
f.ch, f.id, f.dlc, f.dlc ? dhs : "(空)", seq);
|
||
if (!acked)
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz,
|
||
"警告: 从站回显 ack=%d != seq=%d，过程数据可能未被应用层处理（确认已进 OP、固件 ≥1.5.55）",
|
||
b.ack, seq);
|
||
else if (b.tx_result != 0) {
|
||
const char *tx = bridge_tx_text(b.tx_result);
|
||
if (tx[0]) logs_add(g_ee_logs, sizeof g_ee_logs, &lsz,
|
||
"从站发送失败: %s （0xE4 邮箱满=总线无应答/未接收器；0xE2 BUS_OFF=接线或终端电阻）", tx);
|
||
else logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "从站发送失败: 0x%02X", b.tx_result);
|
||
}
|
||
rl = snprintf(resp, 512 * 1024,
|
||
"{\"ok\":true,\"log\":[%s],\"wkc\":%d,\"expected_wkc\":%lld,\"seq\":%d,\"acked\":%s",
|
||
g_ee_logs, wkc, g_expected_wkc, seq, acked ? "true" : "false");
|
||
emit_bridge_flat(resp, 512 * 1024, &rl, &b);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "}");
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/motor/rx")) {
|
||
int limit = -1, n;
|
||
double v;
|
||
if (json_num(body, "limit", &v)) limit = (int)v;
|
||
EnterCriticalSection(&g_rx_lock);
|
||
n = limit < 0 ? g_rx_len : (limit < g_rx_len ? limit : g_rx_len);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"log\":[],\"frames\":[");
|
||
for (i = 0; i < n; i++) {
|
||
int t = (g_rx_head + i) % RX_Q;
|
||
char hx[CAN_FD_DATA_MAX * 2 + 2];
|
||
hex_encode(g_rx_q[t].data, g_rx_q[t].dlc, hx);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl,
|
||
"%s{\"ch\":%d,\"dlc\":%d,\"valid\":%s,\"id\":%u,\"data\":\"%s\"}",
|
||
i ? "," : "", g_rx_q[t].ch, g_rx_q[t].dlc,
|
||
g_rx_valid[t] ? "true" : "false", g_rx_q[t].id, hx);
|
||
}
|
||
g_rx_head = (g_rx_head + n) % RX_Q;
|
||
g_rx_len -= n;
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "],\"remain\":%d}", g_rx_len);
|
||
LeaveCriticalSection(&g_rx_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/can/state")) {
|
||
bridge_t b;
|
||
int wkc;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
if (!g_op) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未进 OP —— 先点『进入 OP』或发送一帧\"]}"); goto done;
|
||
}
|
||
if (g_loop_running) {
|
||
/* 循环在跑：给它的最新快照，别再自己踩周期(两个写者抢 outputs，
|
||
* 额外周期还会打乱窗口滚动 —— api_can_state 注释同款) */
|
||
EnterCriticalSection(&g_state_lock);
|
||
b = g_bridge;
|
||
LeaveCriticalSection(&g_state_lock);
|
||
wkc = (int)g_wkc;
|
||
} else {
|
||
wkc = pump_cycles(1, &b);
|
||
}
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"log\":[],\"wkc\":%d,\"expected_wkc\":%lld",
|
||
wkc, g_expected_wkc);
|
||
emit_bridge_flat(resp, 512 * 1024, &rl, &b);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "}");
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/motor/state")) {
|
||
/* 楔死自动恢复：检测在循环线程（窗口回显冻结，见 pdo_worker），恢复在
|
||
* 本路由（UI 每 50ms 轮询，必然及时到达）。检测后留 0.5s 缓冲（给在途
|
||
* 请求收尾），TryEnter 避免与互斥路由互等（拿不到锁就下拍再试）。恢复
|
||
* 与 motor/start 掉 OP 同款：完整重拉 INIT→PREOP→SAFEOP→OP，实测能把
|
||
* 固件 OP 态应用层楔死拉回正常。10s 冷却防反复重拉。 */
|
||
if (g_wedge_at && g_loop_running) {
|
||
double nw = now_s();
|
||
if (nw - (double)g_wedge_at > 0.5
|
||
&& nw >= (double)g_wedge_cooldown
|
||
&& TryEnterCriticalSection(&g_bus_lock)) {
|
||
char wl[1024];
|
||
int wlsz = 0, wok;
|
||
InterlockedExchange(&g_wedge_at, 0);
|
||
g_wedge_cooldown = (LONG)(nw + 10.0);
|
||
loop_stop();
|
||
wok = do_op_start(0, 0, wl, sizeof wl, &wlsz);
|
||
loop_start();
|
||
g_wedge_recoveries++;
|
||
printf("[native][WEDGE] 自动重拉%s：%.*s\n", wok ? "成功" : "失败",
|
||
wlsz > 0 ? wlsz : 0, wl);
|
||
fflush(stdout);
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
}
|
||
}
|
||
build_state_json(resp, 512 * 1024);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
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, 512 * 1024, "{\"ok\":false,\"log\":[\"周期超出允许范围 [62.5us,1000ms]\"]}");
|
||
else {
|
||
EnterCriticalSection(&g_cfg_lock); g_period_ms = v; LeaveCriticalSection(&g_cfg_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"log\":[\"PDO 周期(native) %gms → %gms\"],\"period_ms\":%.4f}", old, v, v);
|
||
}
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/motor/dbg")) {
|
||
static beat_t b[RING];
|
||
int n = recent_beats(b, RING);
|
||
rl = snprintf(resp, 512 * 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, 512 * 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, 512 * 1024 - rl, "],\"gc_events\":[],\"http\":[]}");
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/") || !strcmp(path, "/api/native/health")) {
|
||
int qd;
|
||
EnterCriticalSection(&g_cmd_lock); qd = g_cmd_len; LeaveCriticalSection(&g_cmd_lock);
|
||
rl = snprintf(resp, 512 * 1024,
|
||
"{\"ok\":true,\"name\":\"ecat_native\",\"bus_open\":%d,\"op\":%d,\"loop\":%d,"
|
||
"\"slaves\":%d,\"wkc\":%d,\"cycles\":%lld,\"queue_depth\":%d}",
|
||
g_bus_open, g_op, g_loop_running, g_slavecount, (int)g_wkc, g_total, qd);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
if (!strcmp(path, "/api/native/enctest")) {
|
||
/* 纯编码对照（不碰网卡）：跑 fill_can_out 返回每用例 384B hex，
|
||
与 Python _fill_can_out 逐字节比对。case 顶层键必须先于 batch 出现
|
||
（手搓解析器取首次匹配）。 */
|
||
static frame_t bfr[64];
|
||
const char *p = strstr(body, "\"cases\"");
|
||
char tok[80];
|
||
int nc = 0;
|
||
p = p ? strchr(p, '[') : NULL;
|
||
rl = snprintf(resp, 512 * 1024, "{\"hexes\":[");
|
||
while (p && nc < 32 && (p = strchr(p, '{')) != NULL) {
|
||
char casebuf[4096];
|
||
int depth = 0, j, on, q, nb = 0, win = 0, dlc;
|
||
double v;
|
||
frame_t first;
|
||
for (j = 0; p[j]; j++) {
|
||
if (p[j] == '{') depth++;
|
||
else if (p[j] == '}') { depth--; if (!depth) break; }
|
||
}
|
||
if (depth) break;
|
||
on = j + 1;
|
||
if (on >= (int)sizeof casebuf) on = (int)sizeof casebuf - 1;
|
||
memcpy(casebuf, p, on); casebuf[on] = 0;
|
||
memset(&first, 0, sizeof first);
|
||
if (json_num(casebuf, "win", &v)) win = (int)v;
|
||
g_can_out[TX_OFF_WIN] = (uint8)win;
|
||
if (json_num(casebuf, "ch", &v)) first.ch = (int)v;
|
||
if (json_tok(casebuf, "id", tok, sizeof tok)) {
|
||
if (tok[0] == '0' && (tok[1] == 'x' || tok[1] == 'X')) first.id = (uint32)strtoul(tok + 2, NULL, 16);
|
||
else if (strchr(tok, 'x') || strchr(tok, 'X')) {
|
||
char *xp = strchr(tok, 'x'); if (!xp) xp = strchr(tok, 'X');
|
||
first.id = (uint32)strtoul(xp + 1, NULL, 16);
|
||
} else first.id = (uint32)strtoul(tok, NULL, 10);
|
||
}
|
||
if (json_tok(casebuf, "data", tok, sizeof tok) && tok[0])
|
||
first.dlc = hex_decode(tok, first.data, CAN_FD_DATA_MAX);
|
||
if (json_num(casebuf, "dlc", &v) && v >= 0) first.dlc = (int)v;
|
||
{ /* batch 数组 */
|
||
const char *ba = strstr(casebuf, "\"batch\"");
|
||
if (ba && (ba = strchr(ba, '[')) != NULL) {
|
||
while (nb < 32 && (ba = strchr(ba, '{')) != NULL) {
|
||
char ob[512];
|
||
int d2 = 0, k2, o2;
|
||
for (k2 = 0; ba[k2]; k2++) {
|
||
if (ba[k2] == '{') d2++;
|
||
else if (ba[k2] == '}') { d2--; if (!d2) break; }
|
||
}
|
||
if (d2) break;
|
||
o2 = k2 + 1;
|
||
if (o2 >= (int)sizeof ob) o2 = (int)sizeof ob - 1;
|
||
memcpy(ob, ba, o2); ob[o2] = 0;
|
||
if (parse_frame(ob, &bfr[nb]) == 0) nb++;
|
||
ba = ba + k2 + 1;
|
||
}
|
||
}
|
||
}
|
||
fill_can_out(&first, bfr, nb, 0);
|
||
(void)dlc;
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "%s\"", nc ? "," : "");
|
||
for (q = 0; q < PDO_OUT_LEN; q++) rl += snprintf(resp + rl, 512 * 1024 - rl, "%02X", g_can_out[q]);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "\"");
|
||
nc++;
|
||
p = p + j + 1;
|
||
}
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "],\"seq\":%d}", g_can_seq);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
/* ═════════ 阶段 3：配置/维护面（独占总线，镜像 Python _BUS_EXCLUSIVE_PATHS）═════════
|
||
* 语义逐条对齐 ecat_server.py 的 api_sdo / api_eeprom_* / api_flash / api_sync_diag：
|
||
* 先停 PDO 循环再碰邮箱/EEPROM —— 两者并发是历史上邮箱楔死的诱因。
|
||
* 唯一允许的差异：异常文本（pysoem 的 SdoError/MailboxError 消息）拿不到，
|
||
* 用 ret/wkc 数值近似，JSON 形状不变。 */
|
||
|
||
if (!strcmp(path, "/api/sdo")) {
|
||
char opt[8] = "", tok[32];
|
||
static uint8 payload[EEPROM_SIZE];
|
||
uint8 buf[SDO_BUF_MAX];
|
||
char hexs[2 * sizeof payload + 2];
|
||
double v;
|
||
int idx, sub, sz = 8, wkc;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
json_tok(body, "op", opt, sizeof opt);
|
||
idx = json_num(body, "index", &v) ? (int)v
|
||
: (json_tok(body, "index", tok, sizeof tok) ? (int)strtol(tok, NULL, 0) : 0);
|
||
sub = json_num(body, "sub", &v) ? (int)v
|
||
: (json_tok(body, "sub", tok, sizeof tok) ? (int)strtol(tok, NULL, 0) : 0);
|
||
if (json_num(body, "size", &v) && (int)v > 0) {
|
||
sz = (int)v; if (sz > SDO_BUF_MAX) sz = SDO_BUF_MAX;
|
||
}
|
||
if (!strcmp(opt, "read")) {
|
||
int real = sz, q;
|
||
char hx[SDO_BUF_MAX * 2 + 2];
|
||
wkc = ec_SDOread(1, (uint16)idx, (uint8)sub, FALSE, &real, buf, EC_TIMEOUTRXM);
|
||
if (wkc > 0 && real >= 0) {
|
||
for (q = 0; q < real && q < SDO_BUF_MAX; q++) sprintf(hx + 2 * q, "%02x", buf[q]);
|
||
hx[2 * (real > SDO_BUF_MAX ? SDO_BUF_MAX : real)] = 0;
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"log\":[\"SDO 0x%04X:%d = ", idx, sub);
|
||
for (q = 0; q < real && q < SDO_BUF_MAX; q++) rl += snprintf(resp + rl, 512 * 1024 - rl, "%02X", buf[q]);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "\"],\"data\":\"%s\"}", hx);
|
||
} else {
|
||
rl = snprintf(resp, 512 * 1024,
|
||
"{\"ok\":false,\"log\":[\"SDO 失败: SdoError wkc=%d ret=%d\"]}", wkc, real);
|
||
}
|
||
} else {
|
||
int plen, q;
|
||
json_str(body, "hex", hexs, sizeof hexs);
|
||
plen = hex_decode(hexs, payload, (int)sizeof payload);
|
||
if (plen <= 0) {
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"SDO 失败: hex 缺失或为空\"]}");
|
||
} else {
|
||
wkc = ec_SDOwrite(1, (uint16)idx, (uint8)sub, FALSE, plen, payload, EC_TIMEOUTRXM);
|
||
if (wkc > 0) {
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"log\":[\"SDO 0x%04X:%d <- ", idx, sub);
|
||
for (q = 0; q < plen; q++) rl += snprintf(resp + rl, 512 * 1024 - rl, "%02X", payload[q]);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, " OK\"]}");
|
||
} else {
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"SDO 失败: SdoError wkc=%d\"]}", wkc);
|
||
}
|
||
}
|
||
}
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
|
||
if (!strcmp(path, "/api/sync_diag")) {
|
||
uint32 vals[4];
|
||
int i, okall = 1;
|
||
double now = now_s(), rate = -1.0;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
for (i = 0; i < 4; i++) {
|
||
uint8 b[8];
|
||
int real = 4;
|
||
int wkc = ec_SDOread(1, 0x2000, (uint8)(i + 1), FALSE, &real, b, EC_TIMEOUTRXM);
|
||
if (wkc <= 0 || real < 4) { okall = 0; break; }
|
||
vals[i] = le32get(b);
|
||
}
|
||
if (!okall) {
|
||
g_syncprev_valid = 0; /* 基线失效(固件<1.5.31/掉线)：清，别留下半个样本 */
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"SDO 读 0x2000 失败 —— 从站固件需 ≥1.5.31 且已连接(≥PREOP)\"]}");
|
||
goto done;
|
||
}
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"log\":[],\"sync0\":%u,\"sync1\":%u,"
|
||
"\"escirq\":%u,\"uptime\":%u,\"sync0_rate\":",
|
||
vals[0], vals[1], vals[2], vals[3]);
|
||
if (g_syncprev_valid) {
|
||
double dt = now - g_syncprev_ts;
|
||
if (dt > 0.05) {
|
||
rate = (double)(uint32)(vals[0] - g_syncprev_sync0) / dt;
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "%.1f", rate);
|
||
} else rl += snprintf(resp + rl, 512 * 1024 - rl, "null");
|
||
} else rl += snprintf(resp + rl, 512 * 1024 - rl, "null");
|
||
g_syncprev_valid = 1; g_syncprev_sync0 = vals[0]; g_syncprev_ts = now;
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "}");
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
|
||
/* ESC 寄存器裸读诊断: {"start":544,"count":2} 读从站 1 寄存器(十进制)。
|
||
* 先手发一次 LRW 再读 —— 若输出真进了 SM2，读回的 0x220 bit10 应挂起；
|
||
* lrw_wkc 直接回答"过程数据有没有到达从站"(0x001B 排查用)。 */
|
||
if (!strcmp(path, "/api/esc/read")) {
|
||
uint8 buf[512];
|
||
double v;
|
||
int start = 0, count = 16, i, rwkc, lwkc;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
if (json_num(body, "start", &v)) start = (int)v;
|
||
if (json_num(body, "count", &v)) count = (int)v;
|
||
if (count < 1) count = 1;
|
||
if (count > (int)sizeof buf) count = (int)sizeof buf;
|
||
ec_send_processdata();
|
||
lwkc = ec_receive_processdata(EC_TIMEOUTRET);
|
||
rwkc = ec_FPRD(ec_slave[1].configadr, (uint16)start, (uint16)count, buf, EC_TIMEOUTRET);
|
||
rl = snprintf(resp, 512 * 1024,
|
||
"{\"ok\":%s,\"lrw_wkc\":%d,\"rd_wkc\":%d,\"hex\":\"",
|
||
rwkc > 0 ? "true" : "false", lwkc, rwkc);
|
||
for (i = 0; i < count; i++) rl += snprintf(resp + rl, 512 * 1024 - rl, "%02X", buf[i]);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "\"}");
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
|
||
/* ESC 寄存器裸写诊断: {"start":1056,"val":10000} 写从站 1 的 16-bit 寄存器。 */
|
||
if (!strcmp(path, "/api/esc/write")) {
|
||
double v;
|
||
int start = 0, val = 0, wkc;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
if (json_num(body, "start", &v)) start = (int)v;
|
||
if (json_num(body, "val", &v)) val = (int)v;
|
||
wkc = ec_FPWRw(ec_slave[1].configadr, (uint16)start, (uint16)(val & 0xFFFF), EC_TIMEOUTRET);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":%s,\"wkc\":%d}", wkc > 0 ? "true" : "false", wkc);
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
|
||
if (!strcmp(path, "/api/eeprom/read")) {
|
||
uint16 words[EE_WORDS];
|
||
double v;
|
||
int start = 0, count = 16, wi, base;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
if (json_num(body, "start", &v)) start = (int)v;
|
||
if (json_num(body, "count", &v)) count = (int)v;
|
||
if (count < 1) count = 1;
|
||
if (count > EE_WORDS) count = EE_WORDS;
|
||
if (start < 0) start = 0;
|
||
if (start > EE_WORDS - 1) start = EE_WORDS - 1;
|
||
for (wi = 0; wi < count; wi++) words[wi] = ee_read_word((uint16)(start + wi));
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"rows\":[");
|
||
for (base = 0; base < count; base += 8) {
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "%s{\"addr\":%d,\"hex\":\"", base ? "," : "", start + base);
|
||
for (wi = base; wi < base + 8 && wi < count; wi++)
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "%s%04X", wi > base ? " " : "", words[wi]);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "\"}");
|
||
}
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "],\"raw\":\"");
|
||
for (wi = 0; wi < count; wi++) rl += snprintf(resp + rl, 512 * 1024 - rl, "%04X", words[wi]);
|
||
rl += snprintf(resp + rl, 512 * 1024 - rl, "\"}");
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
|
||
if (!strcmp(path, "/api/eeprom/write")) {
|
||
int dlen, word, n = 0, nfail = 0, lsz = 0, ok;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
g_ee_logs[0] = 0;
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
json_str(body, "data", g_ee_hex, sizeof g_ee_hex);
|
||
dlen = hex_decode(g_ee_hex, g_ee_data, (int)sizeof g_ee_data);
|
||
if (dlen % 2 && dlen < (int)sizeof g_ee_data) g_ee_data[dlen++] = 0xFF;
|
||
if (dlen > EEPROM_SIZE) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"%dB 超过 EEPROM 容量 %dB\"]}", dlen, EEPROM_SIZE);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
for (word = 0; word < dlen / 2; word++) {
|
||
uint16 val = (uint16)(g_ee_data[2 * word] | (g_ee_data[2 * word + 1] << 8));
|
||
if (val == 0xFFFF) continue;
|
||
if (!ee_write_word((uint16)word, val)) { /* pysoem 抛 EepromError → dispatcher 的兜底文案 */
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"服务器异常: EepromError: word 写入失败\"]}"); goto done;
|
||
}
|
||
if (ee_read_word((uint16)word) != val) {
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "word 0x%04X 校验失败", word);
|
||
nfail++;
|
||
}
|
||
n++;
|
||
}
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "写入 %d 字, 失败 %d 字", n, nfail);
|
||
ok = (nfail == 0);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":%s,\"log\":[%s],\"written\":%d,\"failed\":%d}",
|
||
ok ? "true" : "false", g_ee_logs, n, nfail);
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
|
||
if (!strcmp(path, "/api/eeprom/verify")) {
|
||
int dlen, word, ndiff = 0, lsz = 0, ok;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
g_ee_logs[0] = 0;
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
json_str(body, "data", g_ee_hex, sizeof g_ee_hex);
|
||
dlen = hex_decode(g_ee_hex, g_ee_data, (int)sizeof g_ee_data);
|
||
if (dlen % 2 && dlen < (int)sizeof g_ee_data) g_ee_data[dlen++] = 0xFF;
|
||
for (word = 0; word < (dlen > EEPROM_SIZE ? EEPROM_SIZE : dlen) / 2; word++) {
|
||
uint16 expect = (uint16)(g_ee_data[2 * word] | (g_ee_data[2 * word + 1] << 8));
|
||
uint16 actual = ee_read_word((uint16)word);
|
||
if (expect != actual) {
|
||
if (ndiff <= 10) /* Python: len(diffs)<=10 时进 log —— 实际会打第 11 行，原样镜像 */
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "word 0x%04X: 文件 0x%04X != 芯片 0x%04X", word, expect, actual);
|
||
ndiff++;
|
||
}
|
||
}
|
||
ok = (ndiff == 0);
|
||
if (ok) logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "verify: OK, 全部一致");
|
||
else logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "verify: %d 字不符", ndiff);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":%s,\"log\":[%s],\"diffs\":%d}", ok ? "true" : "false", g_ee_logs, ndiff);
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
|
||
if (!strcmp(path, "/api/flash")) {
|
||
const uint8 *img;
|
||
int ilen, word, n = 0, nfail = 0, lsz = 0;
|
||
uint32 rb32;
|
||
EnterCriticalSection(&g_bus_lock);
|
||
loop_stop();
|
||
g_ee_logs[0] = 0;
|
||
if (!g_bus_open) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"未连接\"]}"); goto done;
|
||
}
|
||
json_str(body, "data", g_ee_hex, sizeof g_ee_hex);
|
||
if (g_ee_hex[0]) {
|
||
ilen = hex_decode(g_ee_hex, g_ee_data, (int)sizeof g_ee_data);
|
||
img = g_ee_data;
|
||
if (ilen % 2 && ilen < (int)sizeof g_ee_data) { g_ee_data[ilen++] = 0xFF; }
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "烧录 上传的 bin: %d 字节", ilen);
|
||
} else {
|
||
img = ESI_EEPROM_IMAGE;
|
||
ilen = (int)sizeof(ESI_EEPROM_IMAGE);
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "烧录 gen_eeprom.py 镜像: %d 字节", ilen);
|
||
}
|
||
if (ilen > EEPROM_SIZE) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"%dB 超过 EEPROM 容量 %dB\"]}", ilen, EEPROM_SIZE);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
/* 字节序探针：写 0x1234 看落在读回 4 字节的哪个半字。
|
||
* 探针字必须落在**真**的 0xFF 填充区：SII 类别区已长到 word 0x0281
|
||
* (STRINGS/General/FMMU/SyncM/RxPDO196字/TxPDO324字，END 在 word 0x0282)，
|
||
* 老脚本用的 0x0200 其实在 TxPDO 类别**内部** —— 探针后"恢复"成 0xFFFF
|
||
* 就把那一字真数据擦成空洞（本机实测芯片 word 0x0200 = 0xFFFF 而镜像是
|
||
* 0x401B，正是这么来的；只因烧录紧随其后重写全片才没爆）。改用 0x0400，
|
||
* 在 4096B 容量内、远超 END，且被 0xFF 跳过逻辑天然忽略。 */
|
||
if (!ee_write_word(EE_SCRATCH_WORD, 0x1234)) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"服务器异常: EepromError: word 写入失败\"]}"); goto done;
|
||
}
|
||
rb32 = (uint32)ec_readeeprom(1, EE_SCRATCH_WORD, EC_TIMEOUTEEP);
|
||
if (((rb32 & 0xFFFF) != 0x1234) && ((rb32 >> 16) != 0x1234)) {
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "EEPROM 写探针失败: 读回 %08X", rb32);
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[%s]}", g_ee_logs);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
ee_write_word(EE_SCRATCH_WORD, 0xFFFF);
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "字节序探针 OK");
|
||
for (word = 0; word < ilen / 2; word++) {
|
||
uint16 val = (uint16)(img[2 * word] | (img[2 * word + 1] << 8));
|
||
if (val == 0xFFFF) continue;
|
||
if (!ee_write_word((uint16)word, val)) {
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, "{\"ok\":false,\"log\":[\"服务器异常: EepromError: word 写入失败\"]}"); goto done;
|
||
}
|
||
if (ee_read_word((uint16)word) != val) {
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "word 0x%04X 校验失败", word);
|
||
nfail++;
|
||
}
|
||
n++;
|
||
if (n % 64 == 0) logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, " ... 已写 %d 字", n);
|
||
}
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, "刷写完成: 成功 %d 字, 失败 %d 字", n, nfail);
|
||
if (nfail) {
|
||
/* Python api_flash 失败分支只回 {ok,log}，不带 written —— 原样镜像 */
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[%s]}", g_ee_logs);
|
||
} else {
|
||
logs_add(g_ee_logs, sizeof g_ee_logs, &lsz, ">>> 板子断电重新上电后 ESC 才会重载配置(身份字/PDI/SYNC)");
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":true,\"log\":[%s],\"written\":%d}", g_ee_logs, n);
|
||
}
|
||
LeaveCriticalSection(&g_bus_lock);
|
||
send_json(cs, resp); goto done;
|
||
}
|
||
|
||
rl = snprintf(resp, 512 * 1024, "{\"ok\":false,\"log\":[\"接口不存在(native): %s\"]}", path);
|
||
send_json(cs, resp);
|
||
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 = 8090, i, auto_up = 0, tries;
|
||
int parent_pid = 0;
|
||
LARGE_INTEGER f;
|
||
WSADATA wsa;
|
||
SOCKET ls = INVALID_SOCKET;
|
||
struct sockaddr_in sa;
|
||
|
||
for (i = 1; i < argc; 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);
|
||
else if (!strcmp(argv[i], "--web") && i + 1 < argc) snprintf(g_webdir, sizeof g_webdir, "%s", argv[++i]);
|
||
else if (!strcmp(argv[i], "--parent-pid") && i + 1 < argc) parent_pid = atoi(argv[++i]);
|
||
else if (!strcmp(argv[i], "--no-browser")) { /* 与 Python 参数面兼容：native 本就不弹浏览器 */ }
|
||
else if (!strcmp(argv[i], "--auto")) auto_up = 1;
|
||
}
|
||
|
||
QueryPerformanceFrequency(&f);
|
||
g_qpc2s = 1.0 / (double)f.QuadPart;
|
||
InitializeCriticalSection(&g_ring_lock);
|
||
InitializeCriticalSection(&g_cfg_lock);
|
||
InitializeCriticalSection(&g_bus_lock);
|
||
InitializeCriticalSection(&g_state_lock);
|
||
InitializeCriticalSection(&g_cmd_lock);
|
||
InitializeCriticalSection(&g_rx_lock);
|
||
WSAStartup(MAKEWORD(2, 2), &wsa);
|
||
|
||
if (parent_pid) {
|
||
/* 被 esp32_server 拉起：跳过清场(会把用户开着的实例优雅关掉)，
|
||
* 但父进程消失时自我了断，不留孤儿占 Npcap/端口。 */
|
||
HANDLE t = CreateThread(NULL, 0, parent_watch, (LPVOID)(DWORD_PTR)parent_pid, 0, NULL);
|
||
if (t) CloseHandle(t);
|
||
} else {
|
||
kill_stale_instances();
|
||
}
|
||
|
||
if (auto_up) {
|
||
char m[4096];
|
||
int lsz = 0;
|
||
m[0] = 0;
|
||
if (do_open(substr, m, sizeof m)) {
|
||
printf("[native] %s\n", m); fflush(stdout);
|
||
if (do_op_start(0, 0, m, sizeof m, &lsz)) { printf("[native] %s\n", m); fflush(stdout); loop_start(); }
|
||
} else fprintf(stderr, "[native] auto open 失败: %s\n", m);
|
||
}
|
||
|
||
/* 端口漂移(镜像 _bind_server)：被占就试下一个，最多 6 个。
|
||
* 注意别把 port++ 放进 for 增量段 —— 绑定成功那轮也会走增量，
|
||
* 日志就会报出一个没在听的端口(实测监听 8090、日志写 8091)。
|
||
* SO_REUSEADDR 保留 —— Windows 上它允许重复 bind(双监听抢连接)，Python
|
||
* 同样带着它、靠"先探测+清场"兜底，这里维持同构。 */
|
||
for (tries = 0; tries < 6 && ls == INVALID_SOCKET; tries++) {
|
||
int p = port + tries;
|
||
if (port_taken(p)) { printf("端口 %d 已被占用, 尝试 %d\n", p, p + 1); continue; }
|
||
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)p);
|
||
sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
|
||
if (bind(ls, (struct sockaddr *)&sa, sizeof sa) == SOCKET_ERROR) {
|
||
printf("端口 %d 绑定失败(%d), 尝试 %d\n", p, WSAGetLastError(), p + 1);
|
||
closesocket(ls); ls = INVALID_SOCKET; continue;
|
||
}
|
||
listen(ls, 32);
|
||
port = p;
|
||
}
|
||
if (ls == INVALID_SOCKET) {
|
||
fprintf(stderr, "[native] 无法绑定端口 %d..%d\n", port, port + 5);
|
||
return 6;
|
||
}
|
||
printf("PORT=%d\n", port);
|
||
printf("[native] 监听 http://127.0.0.1:%d (阶段4: 全路由 + 生命周期契约)\n", port);
|
||
fflush(stdout);
|
||
|
||
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;
|
||
}
|