首页 / 驱动算法 / EtherCAT专题

EtherCAT 伺服控制实战:CiA402 周期模式 CSP/CSV 与使能时序

把 CANopen 板块的 CiA402 控制字/状态字放到 EtherCAT 上跑,就是本文:CoE 用两套通道承载 6040h/6041h(邮箱 SDO 配置 + 过程数据 PDO 周期交换)、三种周期同步模式(CSP 位置 / CSV 速度 / CMT 转矩)怎么选、6060h 模式切换怎么确认、CSP 必配对象和 Rx/TxPDO 映射全表、EtherCAT 下的使能时序特殊点(进 OP 前 6040h 预置、快捷使能)、一周期内主站和从站各自干什么、位置到达怎么确认、多轴为什么必须 DC 同帧触发,配 CSP 使能与点位运动交互演示和基于 SOEM 的可编译 C 代码。

1. EtherCAT 里的 CiA402:CoE 承载与两套通道

CiA402(DS402)是"伺服应用层协议"——它定义的对象字典(6040h 控制字、6041h 状态字、6060h 操作模式、607Ah 目标位置……)和状态机与总线无关,CANopen 和 EtherCAT 都承载它。EtherCAT 用 CoE(CANopen over EtherCAT)承载,靠两套通道分工:

通道协议干什么用典型对象
邮箱(配置)CoE SDO(0x1000 系列邮箱 + SDO 服务)低频配置:选模式、设参数、读故障6060h 模式、6091h 齿轮比、603Fh 故障码
过程数据(周期)PDO(SM2/SM3 经 FMMU 映射)每周期高频交换:控制字/目标值 vs 状态字/实际值6040h、607Ah、6041h、6064h、606Ch

规则一句话:配置走邮箱、控制走 PDO。进 OP 后邮箱基本闲置(只在改模式/读故障时用),每一周期都在走的只有过程数据——这就是 EtherCAT 伺服"μs~ms 级控制周期"的来源。你刚看完的 SOEM 篇里 ec_config_map 生成的 IOmap,装的就是下面这张 PDO 映射表的数据。

2. 三种周期同步模式:CSP / CSV / CMT 一张表

EtherCAT 伺服几乎都用"周期同步模式"(Cyclic Synchronous),与 CANopen 传统的位置/速度/转矩模式(PP/PV/PT)最大的区别:运动规划在主站做,从站只执行。主站每周期下发"这一拍的目标值",从站按内部增益闭环跟踪:

模式6060h 值主站每周期下发从站回报适用
CSP(周期同步位置)8607Ah 目标位置(int32)6064h 实际位置点动/轨迹/机器人(最常用)
CSV(周期同步速度)960FFh 目标速度(int32)606Ch 实际速度卷绕/张力/速度同步
CMT(周期同步转矩)1060B2h 目标转矩(int16)6077h 实际转矩力控/对拖/恒力

工程真相:真正跑起来用的是 CSP 和 CSV 的混合——运动规划(梯形/S 曲线)在主站实时算目标位置,从站 CSP 闭环;特殊工况(卷绕张力、机器人碰撞检测)临时切 CSV/CMT。模式值 8/9/10 是 CiA402 规范的,与总线无关,CANopen 上同样是这组值。

3. 模式切换:写 6060h,读 6061h 确认

模式切换经邮箱 SDO 完成,两条铁律:先停再切、切完确认。

// SOEM 下切换 CSP(邮箱 SDO 写 6060h = 8)*/ int mode = 8; // CSP */ ec_coe_write(slave_no, 0x6060, 0x00, TRUE, sizeof(mode), &mode, EC_TIMEOUTSAFE, NULL); /* 读回 6061h 确认(从站内部应用切换完成后才返回新值)*/ int cur = 0; ec_coe_read(slave_no, 0x6061, 0x00, FALSE, &cur, EC_TIMEOUTSAFE, NULL); if (cur != 8) { /* 切换未完成,等待重试 */ }

要点:

  • 切换时机:在电机停止(或按厂商要求)时切,运行中切模式多数从站直接报错或拒切;
  • 确认机制:6060h 是"请求",6061h 是"实际"——从站应用完成模式内部初始化后才回写 6061h,所以别只写不读,切完必须轮询 6061h 等于目标值;
  • 切换后清目标:进新模式后第一拍把目标值清 0 或保持当前位置,避免上电残留目标引起突跳。

4. CSP 必配对象与 PDO 映射全表

CSP 模式下,最少只需要两组 PDO 就能跑——一组下行(主站→从站)、一组上行(从站→主站)。这是本专题 PDO 篇"映射三元组"的具体实例:

方向映射对象对象字典类型/字节用途
RxPDO(0x1600,下行)6040h 控制字sub0uint16 / 2B使能/停机/快停(CiA402 篇)
607Ah 目标位置sub1int32 / 4B每周期目标位置(用户单位 counts)
TxPDO(0x1A00,上行)6041h 状态字sub0uint16 / 2B状态机反馈(到位/故障位)
6064h 实际位置sub1int32 / 4B每周期实际位置反馈

这条映射的位打包 = 2B + 4B = 6 字节(PDO 篇的 ⌈Σbitlen/8⌉ 规则),所以 RxPDO 长度 6B、TxPDO 长度 6B。常用扩展:下行加 60FFh(速度前馈)、上行加 606Ch 实际速度(做速度观测)或 60F4h 跟随误差(做监控)。映射一旦确定,IOmap 偏移就固定了——SOEM 篇演示里伺服 O4+I4 就是这类映射的典型。

CSP 运行还常用这几个对象(邮箱配置,不走 PDO):

对象含义典型值
6091h 齿轮比电机圈数 : 负载圈数(用户单位换算)如 10:1 减速器配 17bit 磁编
6065h / 6066h 跟随误差窗口/超时实际与目标偏差超窗且持续超时 → 报跟随误差故障如 ±1mm / 100ms
6081h / 6082h / 6083h 轮廓速度/加速度/减速度CSP 下部分从站用于内部限速/限加(看厂商实现)限速安全阀
60C6h / 60C7h 报警/警告掩码把某类故障映射到 6041h bit3/bit7按需

5. PDO 映射怎么配:主站侧改写 0x1600 / 0x1A00

映射有两个来源:ESI 默认映射(从站 EEPROM 里出厂带,SOEM 的 ec_config_map 直接用)和主站改写。EtherCAT 伺服通常允许主站在 PREOP 阶段改 0x1600/0x1A00 再映射——这正是"主站覆盖来源链"的落地(PDO 篇):

// 改写 RxPDO 0x1600:控制字 + 目标位置(2 项)*/ uint32 map[2]; map[0] = 0x60400010; // index 6040h, sub 00, 16bit */ map[1] = 0x607A0020; // index 607Ah, sub 00, 32bit */ uint16 n = 2; ec_coe_write(slave_no, 0x1600, 0x00, TRUE, sizeof(n), &n, EC_TIMEOUTSAFE, NULL); // 先清 0 项数 */ ec_coe_write(slave_no, 0x1600, 0x01, TRUE, 4, &map[0], EC_TIMEOUTSAFE, NULL); ec_coe_write(slave_no, 0x1600, 0x02, TRUE, 4, &map[1], EC_TIMEOUTSAFE, NULL); ec_coe_write(slave_no, 0x1600, 0x00, TRUE, sizeof(n), &n, EC_TIMEOUTSAFE, NULL); // 再写 2 项生效 */ // 必须:改写发生在 ec_config_map 之前;改完再映射,SOEM 才会按新映射算 IOmap */

三个坑提醒:

  • 顺序:先清 0x00(项数=0)→ 逐项写 → 最后写 0x00=项数 收尾,否则从站不认;
  • 时机:必须在 ec_config_map 之前(SOEM 篇第 6 步之前),映射之后改无效——IOmap 已经按旧映射分好偏移了;
  • 对齐:映射项字节数最好按 2/4 对齐(CANopen 惯例),EtherCAT 从站一般也按 4 字节对齐分组。

6. 使能时序:EtherCAT 下的特殊点

状态机本身照搬 CiA402 篇(0x0006→0x0007→0x000F),但 EtherCAT 有两个特殊点:进 OP 前 6040h 预置和快捷使能。

特殊点 ①:进 OP 前 6040h 必须预置

EtherCAT 从站切到 OP 时,过程数据立即开始"每周期生效"——如果 6040h 还是上电初值(0x0000),从站一进 OP 就收到"Disable voltage"命令,多数驱动器会直接报"控制字无效/配置错误"或拒使能。标准做法:进 OP 前先把 6040h 预置为 0x0006(Shutdown),进 OP 后再逐级 0x0007 → 0x000F 使能。

特殊点 ②:快捷使能(0x000F 一步到位)

不少 EtherCAT 驱动器(如汇川、台达、雷赛的主流型号)支持在 OP 下直接写 0x000F,从站固件内部自动走完迁移(相当于发 0x0006 → 0x0007 → 0x000F 三连)。能用但别依赖——它省时间,但跳过了逐级确认,排查问题时少一个观测点;严格场合仍建议逐级。

// SOEM 下完整使能时序(CSP)*/ /* 0. 进 OP 前预置控制字 = 0x0006 */ *(uint16 *)&IOmap[0] = 0x0006; ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); /* 1. 请求 OP(SOEM 篇第 5 步)*/ ec_slave[0].state = EC_STATE_OPERATIONAL; ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); ec_writestate(0); /* 等 OP 确认(轮询 ec_statecheck)*/ /* 2. 逐级使能:0x0007 → 0x000F,每级等状态字 */ *(uint16 *)&IOmap[0] = 0x0007; ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); if ((*(uint16 *)&IOmap[4] & 0x006F) != 0x0223) { /* 等 Switched on */ } *(uint16 *)&IOmap[0] = 0x000F; ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); if ((*(uint16 *)&IOmap[4] & 0x006F) != 0x0227) { /* 等 Operation enabled */ }

注意读状态字用 0x006F 掩码(bit0-6 状态位,跳过 bit7-15 的警告/远程/厂商位),这是 CiA402 篇避坑表里"看错 bit9 远程位"的实战应用。

7. 周期运行:一周期内主站和从站各自干什么

进 OP 后,每一控制周期(如 1ms)的节奏是这样的——结合同步模式篇的 DC Sync0:

时序主站(SOEM)从站(伺服)
周期起点算好本拍目标位置(运动规划),写 6040h=0x000F + 607Ah=目标—
发送ec_send_processdata(LRW 帧带上 RxPDO 数据)帧过站,SM2 接收、FMMU 把数据落到应用区
Sync0 触发—(DC 由从站本地时钟触发)Sync0 中断:应用同时采样新目标,执行 CSP 闭环(位置环→速度环→电流环)
回报ec_receive_processdata(帧返回带上 TxPDO)应用把 6041h/6064h 写入 SM3,帧回程捎回
主站处理读 6041h 查状态/到位,读 6064h 做位置闭环外环或记录—

核心理解:从站不是在"收到帧"那一刻执行,而是在 Sync0 触发那一刻执行——帧早到几 µs、晚到几 µs 都不影响执行时刻,这正是 DC 把抖动压到 ns 级的意义。主站侧只要保证"数据在 Sync0 之前就位",控制质量就与主站软件抖动基本解耦。

8. 到达与跟随确认:bit10 / 位置窗口

"到位"怎么判断?CSP 下目标每周期都在变,不能用"目标=实际"(那是 PP 模式终点概念)。两个机制:

  • 跟随误差(真正该盯的):6065h 窗口(如 ±100 counts)+ 6066h 超时(如 100ms)——|目标−实际| 超窗持续超时,从站报跟随误差故障(AL 状态码/6041h bit3),这是 CSP 下电机堵转、卡死的检测通道;
  • 位置窗口到位(可选的到位信号):6067h 位置窗口 + 6068h 时间——目标停止不变且实际进入窗口并保持时间后,6041h bit10(Target reached)置 1;目标一动 bit10 立即清 0。
bit10 的常见误判

很多项目用"bit10=1"判断定位完成,结果在连续轨迹(目标一直变)里 bit10 永远不置位——轨迹运动看跟随误差,定位停止看 bit10,别混。到位判定在主站做:|6064h − 目标停点| < 窗口并保持若干周期,比依赖 bit10 更可控。

9. 多轴同步:为什么必须 DC 同帧触发

单轴 CSP 用 SM 同步也能跑(帧到达即执行),但多轴机器人/龙门/电子凸轮必须 DC,原因在同步模式篇讲透了:SM 同步的执行时刻 = 帧到达时刻,受主站软件抖动影响(μs 级随机),轴间相位差不可控;DC 让所有从站在同一个 Sync0 时刻采样执行,轴间同步误差只剩时钟漂移补偿残余(ns 级)。

工程落地点:

  • 所有伺服从站 DC 能力必须在 SII 里声明(EEPROM 篇),SOEM 的 ec_configdc 才会给它们配时钟;
  • Sync0 周期 = 控制周期,相移留足数据就位时间(同步模式篇:相移 > 帧到达 + ESC 处理时间);
  • 多轴联动时主站侧还要做轴间位置同步补偿(各轴实际位置同时采样后,用同一拍数据算插补),这是机器人控制器的活。

10. 交互演示:CSP 使能 + 点位运动模拟器

点"使能"看状态机逐步到位,再给目标位置看 CSP 周期跟随:顶部状态字实时变化,右侧目标/实际位置曲线一周期一拍地走。

CSP 使能 + 点位运动(模拟 1ms 周期)
Switch on disabled · 0x0240
  • ① 使能:0x0006 → 0x0007 → 0x000F 逐级走,状态字 0x0240 → 0x0221 → 0x0223 → 0x0227;
  • ② 运动:目标阶跃 100000,实际按梯形速度规划跟随(模拟从站 CSP 位置环),到位后 6041h bit10 置 1;
  • RxPDO/TxPDO 内容在信息栏实时显示——6 字节映射的现场感。

11. 可编译 C 代码 + 避坑清单

下面是基于 SOEM 的 CSP 控制完整骨架(承接第 12 篇 minimal_master.c),覆盖:模式切换 → 预置 6040h → 进 OP → 逐级使能 → 周期下发目标 + 到位判定。

/* csp_drive.c —— SOEM 下 CSP 使能 + 点位运动 编译: gcc -O2 -o csp_drive csp_drive.c -I/usr/local/include -L/usr/local/lib -lsoem -lpthread 运行: sudo ./csp_drive eth0 前提: 从站 RxPDO=6040h+607Ah(6B), TxPDO=6041h+6064h(6B)(本文第 4 节映射)*/ #include <stdio.h> #include <unistd.h> #include <ethercat.h> #define EC_TIMEOUTRET 100 #define SLAVE 1 static char IOmap[4096]; int main(int argc, char **argv) { const char *ifname = (argc > 1) ? argv[1] : "eth0"; uint16 cw, sw, mode; int32 target = 0, actual; int t = 0, iolen; if (!ec_init(ifname)) { printf("ec_init failed\n"); return -1; } if (ec_config_init(FALSE) <= 0) { printf("no slave\n"); return -2; } iolen = ec_config_map(&IOmap); /* 模式切 CSP:邮箱写 6060h=8,读 6061h 确认 */ mode = 8; ec_coe_write(SLAVE, 0x6060, 0, TRUE, 2, &mode, EC_TIMEOUTSAFE, NULL); do { ec_coe_read(SLAVE, 0x6061, 0, FALSE, &mode, EC_TIMEOUTSAFE, NULL); } while (mode != 8); /* 进 OP 前预置控制字 = 0x0006 */ *(uint16 *)&IOmap[0] = 0x0006; ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); /* 请求 OP 并确认 */ ec_slave[0].state = EC_STATE_OPERATIONAL; ec_writestate(0); int chk = 0; while (chk++ < 40 && ec_slave[0].state != EC_STATE_OPERATIONAL) { ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); ec_statecheck(0, EC_STATE_OPERATIONAL, 5000); } if (ec_slave[0].state != EC_STATE_OPERATIONAL) return -3; /* 逐级使能:0x0007 → 0x000F,每级等状态字(掩码 0x006F)*/ cw = 0x0007; *(uint16 *)&IOmap[0] = cw; ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); do { ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); } while ((*(uint16 *)&IOmap[4] & 0x006F) != 0x0223); cw = 0x000F; *(uint16 *)&IOmap[0] = cw; ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); do { ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); } while ((*(uint16 *)&IOmap[4] & 0x006F) != 0x0227); printf("OPERATIONAL + enabled. running...\n"); /* 周期下发:目标位置斜坡 100 counts/ms → 100000 停;到位判定用跟随窗口 */ for (;;) { if (target < 100000) target += 100; *(int32 *)&IOmap[2] = target; /* RxPDO 偏移 2:607Ah(2B 控制字后)*/ ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); sw = *(uint16 *)&IOmap[4]; actual = *(int32 *)&IOmap[6]; /* TxPDO 偏移 6:6064h(2B 状态字后)*/ if (target >= 100000 && (target - actual) < 100) { /* 跟随误差窗口内即到位 */ printf("target reached: actual=%d\n", actual); break; } ec_readstate(); if (ec_slave[0].state != EC_STATE_OPERATIONAL) break; usleep(1000); } cw = 0x0000; *(uint16 *)&IOmap[0] = cw; /* 断压收尾 */ ec_send_processdata(); ec_receive_processdata(EC_TIMEOUTRET); return 0; }

避坑清单:

坑现象对策
进 OP 后控制字报错从站拒使能 / 报配置错误进 OP 前 6040h 必须预置(第 6 节特殊点①)
模式切了没确认6061h 还是旧模式,运动行为不对写 6060h 后必须轮询 6061h 等于目标值
映射改在 ec_config_map 之后IOmap 偏移对不上,数据错位0x1600/0x1A00 改写必须在映射之前(第 5 节)
轨迹里等 bit10到位位永不置 1,逻辑卡死轨迹看跟随误差(6065h/6066h),停点看 bit10(第 8 节)
目标值突跳电机猛冲 / 跟随误差报警主站做斜坡/梯形规划;切换模式后第一拍清目标(第 3 节)
多轴相位差大轨迹走形、龙门扭全轴 DC + Sync0 相移留足数据就位时间(第 9 节 + 同步模式篇)

一句话收尾:EtherCAT 伺服 = CiA402 状态机(CANopen 篇)+ PDO 映射(PDO 篇)+ DC 同步(同步模式篇)+ SOEM 骨架(SOEM 篇)——本文把这四块拧成一根可运行的轴;照第 6 节的使能时序和第 11 节的代码走,第一根轴就能转起来。