SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
31b2d98625
cmvr-es/docs/motor_service_modbus_tcp.md
xtkuang 28f1dd1bf8 feat: add gRPC motor control over Modbus TCP 
Add synchronous and streaming MotorService APIs backed by the PLC Modbus TCP runtime and protocol driver. Extend AUBO JSON commands and isolate vendor libstdc++ paths while keeping build-tree tests runnable.
2026-07-30 15:09:07 +08:00

46 KiB
Raw Blame History

MotorService 与 Modbus TCP PLC 电机控制

本文说明当前 MotorService、CMVR PLC v1 寄存器协议和 Siemens S7-1215C DC/DC/DC 侧的对接方法。本文只描述当前源码已经存在的接口和 协议；寄存器表中标为“预留”或“当前未发送”的内容，不代表上层已经开放。

1. 当前范围

当前实现是一条刻意保持简单的单轴控制链：

gRPC MotorService
        |
        v
MotorManager
        |
        v
AbstractMotor
        |
        v
CmvrPlcMotorProtocol
        |
        v
ModbusTcpMotorBusRuntime
        |
        v
libmodbus -> PLC MB_SERVER -> 驱动器/电机

各层职责如下：

  • MotorService：解析 MotorManager ID 和电机选择器，执行单电机互斥、 同步等待、gRPC 取消检测、流式 latest-wins 和软件急停抢占。
  • MotorManager：按 motor_id 或 joint_name 返回 AbstractMotor。
  • AbstractMotor：提供统一的置零、Profile、Cyclic、使能和 Quick Stop 接口。
  • CmvrPlcMotorProtocol：执行关节限位检查，把 SI 单位转换为定点整数， 并映射成 CMVR PLC v1 命令。
  • ModbusTcpMotorBusRuntime：管理一个 PLC 连接、握手、心跳、重连、每轴 命令序列、payload/commit 写入和 ACK 轮询。
  • PLC：必须原子接收命令、执行驱动器控制、发布状态，并独立执行通信和 周期指令 watchdog。

当前服务按“一个 RPC 或一个流独占一个电机”仲裁，不是多轴同步控制器。 同一个 MotorManager 中不同电机可以分别被不同调用者控制。需要多轴同一扫描 周期锁存时，应在后续版本增加 manager/runtime 级批量 commit，不应依赖客户端 逐轴调用。

runtime 的 start() 启动连接 supervisor，而不是要求 PLC 当时已经在线。首次 同步握手失败时 MotorManager 仍完成注册，runtime 保持 connected=false，并从 reconnect_min_ms 起按上限退避持续重试。离线期间 状态 RPC 返回 UNAVAILABLE，运动命令在写 mailbox 前安全失败；PLC 后续上线 并完成完整身份/session 握手后无需重启 cmvr_es。

2. 实际 gRPC 接口

服务定义位于：

  • protos/cmvr/api/motor_service.proto
  • protos/cmvr/api/motor_command.proto

MotorTarget.header.device_id 是 MotorManager 的设备 ID，不是单个电机 的 ID。MotorTarget.selector 必须且只能设置一个：

motor_id   uint32，当前服务要求不大于 255
joint_name 非空字符串

2.1 Unary RPC

RPC 当前语义 是否阻塞
setZero 调用 calibrateZeroQ()。PLC 后端发送 SetZero，等待 PLC 进入成功终态后返回 是，PLC 后端安全命令上限当前为 5 s
moveToZero 复用 Profile Position，把目标位置设为 0 rad；不是 setZero，也不发送寄存器图中的 MoveToZero opcode 是，直到到位、取消或超时
profilePosition 设置 Profile Position 的目标位置、最大速度和加速度 是，直到到位、取消或超时
profileVelocity 设置 Profile Velocity 的目标速度和加速度 是，直到实际速度稳定进入容差
emergencyStop 抢占当前阻塞命令/流，设置服务内软件急停锁存并调用 quickStop() 是，等待 PLC Quick Stop 成功终态
getStatus 读取模式、位置、速度、到位标志和 MotorService 仲裁状态 同步快照读取
setEnabled enabled=true 调用 torqueOn()，成功后解除服务内软件急停锁存；false 调用 torqueOff() 是，等待 PLC 成功终态

moveToZero、profilePosition 和 profileVelocity 使用 MotorWaitOptions：

字段 0 时默认值 服务端范围/语义
timeout_ms 30000 ms 1 ms 至 600000 ms
poll_period_ms 10 ms 1 ms 至 1000 ms
position_tolerance_rad 0.001 rad 仅正值覆盖默认值
velocity_tolerance_rad_s 0.01 rad/s 仅正值覆盖默认值
settle_sample_count 3 1 至 1000 个连续样本

setZero 只有在 PLC 返回 Completed + ZeroValid 且 zero_epoch 相比命令前 变化时才成功。如果 RPC 返回失败、取消、断链或超时，而 commit 可能已经写出， session/zero epoch 的最终结果是不确定的；禁止直接盲重试，应先通过 PLC/HMI 或维护诊断确认当前 session、zero_epoch 和实际零位状态。

Profile Position 只有在 PLC/驱动器报告 target_reached，同时实际位置和 速度连续满足容差时才返回成功。Profile Velocity 在实际速度连续满足目标速度 容差时返回成功；RPC 返回后速度命令仍然有效。正常停车应再发送目标速度为 0 rad/s 的 profileVelocity，不能把 RPC 返回理解为“速度控制已经结束”。

Profile Position/Velocity 的 PLC payload 使用 600000 ms command_timeout_ms 上限，与 MotorService 允许的最长等待一致。RPC 使用更短 超时或被取消时，服务端仍会主动发送 Quick Stop；PLC 通信 watchdog 仍是上位机 失联时的权威保护。该 600000 ms 是 PLC 执行上限，不会把 RPC 默认等待从 30000 ms 延长。

成功 ACK 的 Profile Position/Velocity 会绑定到提交时的 connection_epoch。只要 PLC 断线重连使 epoch 改变，该 Profile 的位置、速度 和到位反馈就全部失效：getQ/getQd 返回 NaN，reachedTargetQ 返回 false。 即使新 session 的安全停止速度恰好也是目标 0 rad/s，也不能把数值相等误认为 旧命令成功。成功的新 Profile 会重新绑定当前 epoch；成功的首个 Cyclic sample、Quick Stop 或 Disable 会清除该绑定。Enable/SetZero 本身不恢复旧 Profile 反馈的可信性。

当前 getStatus 会依次调用模式、位置、速度和到位读取。对于 Modbus TCP 后端， 这些读取不是一次原子寄存器快照，字段可能来自相邻的 PLC 扫描周期。 它只暴露 AbstractMotor 的通用状态和 MotorService 仲裁状态，不包含 plc_boot_id、session、fault_code、zero_epoch 或 PLC result code。 结果不确定或需要故障恢复时，应从 PLC/HMI 或维护诊断层读取这些原始字段， 不能只依赖本 RPC。

2.2 双向流 RPC

当前有两个双向流：

rpc streamCyclicPosition(stream CyclicPositionRequest)
    returns (stream CyclicControlResponse);

rpc streamCyclicVelocity(stream CyclicVelocityRequest)
    returns (stream CyclicControlResponse);

首帧必须是 open：

open.target              选择一个 MotorManager 中的单个电机
open.watchdog_timeout_ms gRPC 层输入 watchdog

watchdog 为 0 时默认 500 ms，非零值被限制到 20 ms 至 60000 ms。打开 成功后，服务端先返回：

phase = CYCLIC_STREAM_OPENED
sequence = 0
status = 当前电机状态

CYCLIC_STREAM_OPENED 只表示 MotorService 已解析目标、取得该电机的独占 控制权并选择了 Cyclic 模式。CMVR PLC 后端采用惰性打开：收到第一个合法 setpoint 时才依次发送 OpenCyclic* 和对应的 Cyclic*Sample。因此客户端 必须等到该 setpoint 的 CYCLIC_STREAM_APPLIED，才能确认 PLC 已 ACK 打开 命令并锁存了首个样本。

每次 OpenCyclicPosition/Velocity 都创建新的轴级 stream epoch。PLC 必须在 Open 的同一个原子状态事务中清零 last_applied_cyclic_sequence、旧样本去重 状态和 cyclic watchdog，再设置正确的 CSP/CSV mode 与 StreamActive=1。 runtime 只有确认这些 Open 后置条件才接受 ACK，Protocol 随后才从样本序列 1 重新开始。Quick Stop、Disable 或断链关闭旧 epoch；重开后的首个 1 必须 重新应用，不能被旧流的样本 1 去重。

每次 gRPC cyclic 流首帧触发的 setMode 都建立一个新的上层 cyclic_generation，即使模式值与上一条流相同。该 generation 绑定当时的 connection_epoch。活动流遇到重连后会永久锁存失败：同一流的当前和后续 setpoint 都返回失败，不发送新的 OpenCyclic* 或 Cyclic*Sample；服务端 终止并 Quick Stop 该流。客户端必须创建新的 gRPC 流，由新首帧显式建立新的 generation 后才允许重新 Open。禁止把断链前或断链期间 pending 的 latest setpoint 自动应用到新 PLC session。

后续每帧只能是 setpoint：

  • 周期位置：严格递增且非零的 sequence、target_position_rad，以及可选 target_velocity_rad_s。
  • 周期速度：严格递增且非零的 sequence、target_velocity_rad_s。

序列号允许跳号，但不能为 0、重复或倒退。服务端 reader 只保留一个尚未 处理的最新帧；新帧覆盖旧帧时，dropped_setpoints 累计增加。这是低延迟 latest-wins 语义，不适合必须逐点无损执行的离线轨迹。

每次 AbstractMotor 接受 setpoint 后返回：

phase = CYCLIC_STREAM_APPLIED
sequence = 已应用的客户端序列
dropped_setpoints = 本流累计覆盖数

对于 CMVR PLC 后端，AbstractMotor 返回成功前，runtime 已经看到相同 command sequence 的 PLC ACK；周期样本还要求 last_applied_cyclic_sequence 严格等于本次样本序列。对于其他电机后端， APPLIED 只保证对应后端的同步调用返回 true。 为避免每个周期样本额外触发多次现场总线状态读取，APPLIED 响应刻意不携带 status。OPENED 和终止响应仍携带完整状态；需要连续遥测的客户端应以独立、 较低频率调用 getStatus，不能把 APPLIED 当作状态采样接口。

客户端正常结束请求流后，服务端先同步调用 quickStop()；确认后返回 CYCLIC_STREAM_STOPPED 并释放电机所有权。当前协议没有显式 close 请求帧；客户端 half-close 就是结束信号。若 Quick Stop 未被后端确认， 服务端改发 CYCLIC_STREAM_FAILED 并以 INTERNAL 结束 RPC，不会把失败 误报成 STOPPED。

出现以下情况时，服务端会 Quick Stop：

  • gRPC context 被取消或 deadline 到期；
  • 输入在 watchdog 窗口内没有新帧；
  • 序列号或 payload 非法；
  • PLC/后端拒绝 setpoint；
  • 客户端停止读取响应；
  • emergencyStop 增加抢占 generation。

服务端流使用同步 Write()。慢客户端可能阻塞反馈写入，reader 此时仍会 覆盖 pending setpoint，但上层 watchdog 检查也可能被延迟。因此：

  1. 客户端必须并发、持续读取反馈；
  2. gRPC watchdog 只是第一层保护；
  3. PLC 侧通信 watchdog 和周期样本 watchdog 才是断网、进程卡死时的 权威保护。

2.3 当前没有开放的能力

当前 MotorService 没有以下 RPC：

  • Profile Torque；
  • Cyclic Torque 流；
  • Clear Fault；
  • 普通 Stop/Halt；
  • 多轴原子控制。

寄存器块预留了 target_torque，但 CmvrPlcMotorProtocol 当前明确拒绝 周期力矩。不要因为寄存器存在就让 PLC 项目把该功能标记为已经可用。

2.4 仲裁和 gRPC 状态

同一电机已有阻塞命令或流时，新控制调用返回 RESOURCE_EXHAUSTED。软件急停锁存期间，运动命令返回 FAILED_PRECONDITION；只有成功执行 setEnabled(enabled=true) 才清除 该服务内锁存。

任何取消、超时、异常或周期流结束后的 Quick Stop 如果未被后端确认成功， MotorService 也会按 fail-closed 原则进入同一软件急停锁存。此时 RPC 的 INTERNAL 表示“安全状态尚未确认”，不能继续发送运动命令；应先排查 PLC/ 驱动器状态，再通过成功的 setEnabled(enabled=true) 显式恢复。 显式 emergencyStop 会在调用后端前先锁存；若最终 Quick Stop 未确认，它 返回 FAILED_PRECONDITION，但仍保持锁存。两种返回码都不能解释为已经安全 停机。

常见 gRPC 状态包括：

  • INVALID_ARGUMENT：选择器、数值、首帧或序列号非法；
  • NOT_FOUND：MotorManager 或电机不存在；
  • RESOURCE_EXHAUSTED：同一电机已有 owner；
  • FAILED_PRECONDITION：软件急停锁存或后端拒绝命令；
  • DEADLINE_EXCEEDED：同步等待或流 watchdog 超时；
  • ABORTED：被 emergencyStop 抢占；
  • CANCELLED：客户端取消或停止读取。
  • UNAVAILABLE：后端断链，或状态因故障/watchdog 返回非 finite；
  • INTERNAL：服务异常，或命令失败后的安全清理/Quick Stop 也失败。

3. MotorManager 配置

仓库已提供单 PLC、两轴配置示例 cmvr-es/config/devices/motor/plc_motors.pb.txt，内容如下：

motor {
  id: "plc_motors"

  motor_groups {
    id: "plc_axis_group"
    bus_type: MOTOR_BUS_MODBUS_TCP
    vendor: MOTOR_VENDOR_PLC_GENERIC
    protocol: MOTOR_PROTOCOL_CMVR_PLC_V1

    modbus_tcp {
      host: "192.168.0.10"
      port: 502
      unit_id: 1

      connect_timeout_ms: 500
      io_timeout_ms: 100
      heartbeat_period_ms: 100
      communication_watchdog_ms: 1000
      cyclic_watchdog_ms: 500
      status_poll_period_ms: 20
      reconnect_min_ms: 100
      reconnect_max_ms: 2000
      command_ack_timeout_ms: 500

      protocol_major: 1
      protocol_minor: 0

      axes { motor_id: 1 axis_index: 0 }
      axes { motor_id: 2 axis_index: 1 }
    }

    joint_limits {
      enable: true
      source: JOINT_LIMIT_SOURCE_CUSTOM
      joints {
        joint_name: "PLC_AXIS_1"
        q_lb: -3.141592653589793
        q_ub: 3.141592653589793
        qd: 1.0
        qdd: 2.0
      }
      joints {
        joint_name: "PLC_AXIS_2"
        q_lb: -1.5707963267948966
        q_ub: 1.5707963267948966
        qd: 0.5
        qdd: 1.0
      }
    }

    motors {
      motors { id: 1 joint_name: "PLC_AXIS_1" }
      motors { id: 2 joint_name: "PLC_AXIS_2" }
    }
  }
}

字段行为：

  • host 必须是 IPv4 字面量（例如 192.168.0.10），不做 DNS 解析；这样 TCP 建连和 stop() 不会被无界的名称解析阻塞。
  • port=0 时 runtime 使用 502；unit_id=0 时使用 1。
  • connect_timeout_ms=0 时使用 500 ms；该值为 TCP 建连的独立硬超时。
  • io_timeout_ms=0 时使用 100 ms，并同时用于 libmodbus response timeout 和 byte timeout。
  • 心跳、通信 watchdog、周期 watchdog、状态轮询、重连和 ACK timeout 为 0 时，当前 runtime 分别使用 100、500、500、20、100、2000、500 ms；上面的实体样例将通信 watchdog 显式放宽为 1000 ms。
  • communication_watchdog_ms 必须同时不少于 2 * heartbeat_period_ms 和 3 * io_timeout_ms + 2 * heartbeat_period_ms； command_ack_timeout_ms、cyclic_watchdog_ms 必须不少于 3 * io_timeout_ms + status_poll_period_ms，因为一次可靠状态读取包含 sequence-before、完整状态块、sequence-after 三次 FC3； cyclic_watchdog_ms 必须在 20 ms 至 60000 ms 内； reconnect_max_ms 不得小于 reconnect_min_ms，否则初始化失败。
  • protocol_major=0 时使用当前 major 1。握手严格检查 major； PLC 的 minor 版本不得低于配置要求。
  • axis_index 是 PLC 轴块索引，必须小于 PLC 发布的 axis_count，且同一 group 内不能重复。
  • encoder_counts_per_rev 和 gear_ratio 对 CMVR PLC v1 不生效，因为 PLC 协议交换的是 SI 定点值，而不是编码器 count。

还需要在 DeviceManager 配置中注册该 MotorSystem：

device_manager {
  # 只有未被 RobotArm 选择且仍希望初始化全部电机时才需要 true。
  init_all_motors_when_no_active_joints: true

  devices {
    id: "plc_motors"
    type: DEVICE_TYPE_MOTOR_SYSTEM
    config_file: "devices/motor/plc_motors.pb.txt"
    enable: true
  }
}

id 必须与 motor.id 以及 gRPC MotorTarget.header.device_id 一致。安装产物读取 output/bin/config/；修改源码树配置后需再次执行 cmake --install build。

4. CMVR PLC v1 Holding Register

4.1 地址和 32 位编码

本文和 C++ runtime 中的地址都是 零基 Modbus PDU Holding Register 地址。地址 0 是第一个 Holding Register，PLC/HMI 文档若使用 40001 风格显示，则通常对应本文地址 0。不要把 40001 直接传给 modbus_read_registers()。

每个寄存器是 16 位。所有 32 位有符号或无符号值均使用：

register[offset]     = bits 31..16（高 WORD）
register[offset + 1] = bits 15..0 （低 WORD）

libmodbus 负责单个 16 位寄存器在线路上的字节序；PLC 应显式按“高 WORD 在前、低 WORD 在后”组合 32 位值。TIA 中建议用移位和 OR 的辅助 FC， 不要依赖 AT 视图、BLKMOV 或 CPU 内部字节布局偶然得到相同结果。

4.2 全局区

全局区从地址 0 开始，共 32 个寄存器：

地址 长度 类型 名称 方向与说明
0 1 UINT16 magic_cm PLC -> CMVR，固定 16#434D
1 1 UINT16 magic_vr PLC -> CMVR，固定 16#5652
2 1 UINT16 protocol_major PLC -> CMVR，当前为 1
3 1 UINT16 protocol_minor PLC -> CMVR，当前为 0
4 1 UINT16 axis_count PLC -> CMVR，可用轴块数量
5 1 UINT16 plc_global_state PLC -> CMVR，全局状态
6..7 2 UINT32 plc_boot_id PLC -> CMVR，每次 PLC 程序运行实例重启必须改变
8..9 2 UINT32 cmvr_session_id CMVR -> PLC，每次成功握手/重连生成新的非零随机会话
10..11 2 UINT32 cmvr_heartbeat CMVR -> PLC，周期递增
12..13 2 UINT32 plc_heartbeat PLC -> CMVR，PLC 自己的周期计数
14..15 2 UINT32 communication_watchdog_ms CMVR -> PLC，PLC 侧通信 watchdog
16 1 UINT16 global_error PLC -> CMVR，全局错误
17 1 UINT16 owner_state PLC -> CMVR，当前 owner/握手状态
18..19 2 UINT32 owner_session_id PLC -> CMVR，当前 Accepted/Rejected 决策对应的候选 session
20..31 12 - reserved 写 0，PLC 忽略

当前 runtime 在握手初始快照中检查 magic、版本、axis_count 和非零 plc_boot_id，先写通信 watchdog，最后以新 session + 首个 heartbeat 作为 候选 owner 的发布动作。握手的 每次轮询以及 owner 接受后的最终完整快照都必须保持相同的 magic、版本、 axis_count 和 boot ID；其中任一变化或 boot ID 变为 0 都会立即断线重连。 只有 owner_session_id 等于本次 session，且 owner_state=Accepted，连接才 进入可发命令状态。后续心跳周期同时检查 plc_boot_id、owner/session 以及 plc_heartbeat 是否持续推进；PLC 心跳在通信 watchdog 窗口内不变化会主动 断线并重连。owner_state=Rejected 也只有在 owner_session_id 回显本次 session 时才表示本次握手被拒；旧 session 遗留的 Rejected 状态会被忽略并继续 轮询。

PLC 检测到新 session 后，发布顺序必须是：先把 owner_state 改成 Accepting（此时不能先改 owner_session_id），然后安全停止旧 owner、清除 mailbox/旧 stream，再写候选 owner_session_id，最后发布 Accepted。拒绝时 同样先处于 Accepting/None，写完对应 session 后最后发布 Rejected。 否则新的 session ID 可能与残留的旧 Accepted 短暂组合，令 CMVR 过早发命令。

4.3 每轴布局

轴 i 的基地址：

B(i) = 100 + 128 * i

每轴占 128 个 Holding Registers：

B + 0  .. B + 63   控制区
B + 64 .. B + 127  状态区

控制区

相对地址 长度 类型 名称 当前说明
0..1 2 UINT32 payload_sequence 本次轴命令序列
2 1 UINT16 command_code 见命令码表
3 1 UINT16 command_flags 当前写 0
4..5 2 INT32 target_position micro-rad
6..7 2 INT32 target_velocity micro-rad/s
8..9 2 INT32 acceleration micro-rad/s^2
10..11 2 INT32 target_torque 预留，mN·m
12..13 2 INT32 position_tolerance 预留，micro-rad
14..15 2 INT32 velocity_tolerance 预留，micro-rad/s
16..17 2 UINT32 command_timeout_ms 命令完成时限；Profile 当前写 600000 ms
18..19 2 UINT32 stream_watchdog_ms PLC 侧周期样本 watchdog
20..21 2 UINT32 cyclic_sample_sequence 周期样本序列
22..23 2 UINT32 client_monotonic_time_ms CMVR steady-clock 低 32 位
24..25 2 UINT32 expected_zero_epoch SetZero 写入命令前读到的 zero epoch
26 1 UINT16 disconnect_action 预留；当前驱动写 0
27..28 2 UINT32 command_session_id 必须等于当前已接受 session
29..59 31 - reserved 写 0
60..61 2 UINT32 payload_sequence_mirror 必须等于 payload sequence
62..63 2 UINT32 commit_sequence 单独、最后写入

状态区

下表地址相对于 S = B + 64：

相对地址 长度 类型 名称 说明
0..1 2 UINT32 ack_sequence PLC 已解析的 command sequence
2..3 2 UINT32 active_sequence 当前执行中的 command sequence
4 1 UINT16 command_state 命令状态
5 1 UINT16 result_code 结果码
6 1 UINT16 axis_state PLC/驱动器轴状态
7 1 UINT16 current_mode cmvr.msgs.RunMode 数值
8..9 2 INT32 actual_position micro-rad
10..11 2 INT32 actual_velocity micro-rad/s
12..13 2 INT32 actual_torque mN·m
14..15 2 INT32 target_position PLC 当前目标，micro-rad
16..17 2 INT32 target_velocity PLC 当前目标，micro-rad/s
18 1 bit field status_flags 见状态位表
19 1 UINT16 drive_statusword 原始驱动器状态字
20..21 2 UINT32 fault_code PLC/驱动器故障码
22..23 2 UINT32 zero_epoch 置零版本
24..25 2 UINT32 last_applied_cyclic_sequence 最后实际锁存的周期样本
26..27 2 UINT32 state_sequence 状态 seqlock；非零偶数才是稳定版本
28..29 2 UINT32 plc_monotonic_time_ms PLC 单调时间低 32 位
30..31 2 UINT32 heartbeat_age_ms PLC 计算的 CMVR 心跳年龄
32..33 2 UINT32 ack_session_id ack_sequence 所属 session
34..61 28 - reserved PLC 写 0
62..63 2 UINT32 state_sequence_mirror 稳定版本镜像

PLC 状态区使用 seqlock 发布。稳定版本从非零偶数 2 开始，每次完整更新增加 2；版本回绕到 0 时跳到 2。发布顺序必须是：

  1. 更新任何状态字段前，先把 state_sequence 写成下一奇数， state_sequence_mirror 保持上一个稳定偶数；
  2. 写完 ACK、状态、实际值、flags、heartbeat age 等所有字段；
  3. 先把 state_sequence_mirror 写成新的非零偶数；
  4. 最后把 state_sequence 写成相同偶数。

runtime 在同一 socket 互斥区内执行三段读取：先单独读取 state_sequence，再读取完整 64-word 状态块，最后再次单独读取 state_sequence。只有 before、after、块内 sequence 和 mirror 四者相等， 且为非零偶数时才接受完整块；失败最多重试 3 次。这样不要求 Siemens MB_SERVER 对 64-word FC3 做原子内存快照，也允许不同的完整状态读取之间版本 持续推进。

为保证活性，PLC 不得在每个高速控制扫描都无条件翻转 Holding 状态版本。建议 驱动控制状态先写内部 shadow，再由较低频的对外发布任务在字段有意义变化时复制 到 Holding 状态区，并让每个稳定偶数版本至少覆盖三次连续 FC3 的时间窗口； 也可以使用双缓冲后按上述 seqlock 顺序发布。否则 PLC 每次 FC3 之间都推进版本， runtime 的 3 次有限重试会按设计失败，而不是返回可能撕裂的状态。

4.4 命令码

值 名称 当前上层使用
0 Nop 否
1 SetZero setZero
2 MoveToZero 预留；当前 moveToZero 发送 ProfilePosition(target=0)
3 ProfilePosition moveToZero、profilePosition
4 ProfileVelocity profileVelocity
5 OpenCyclicPosition 第一个周期位置样本前自动发送
6 CyclicPositionSample 周期位置样本
7 OpenCyclicVelocity 第一个周期速度样本前自动发送
8 CyclicVelocitySample 周期速度样本
9 CloseCyclicStream 预留；当前流结束使用 QuickStop
10 QuickStop emergencyStop、流结束和上层超时
11 Enable setEnabled(true)
12 Disable setEnabled(false)

PLC 必须区分：

  • SetZero：按已确认的项目语义设置当前位置基准，不得擅自解释为运动回原点；
  • ProfilePosition(target=0)：运动到已经建立的零位；
  • Homing/寻找原点：当前 gRPC 和寄存器协议没有独立开放。

4.5 命令状态、结果和状态位

command_state 定义：

0 Idle              1 Received          2 Validating
3 Accepted          4 Running           5 TargetReached
6 Completed         7 Rejected          8 Failed
9 TimedOut         10 QuickStopped     11 CommunicationLost

runtime 会先拒绝未知状态，再按命令检查成功后置条件：

  • SetZero：Completed、ZeroValid=1，且 zero_epoch 相比命令前变化；
  • Enable：Completed 且 Enabled=1；
  • Disable：Completed 且 Enabled=0；
  • QuickStop：QuickStopped 或 Completed，且实际速度已接近 0；
  • Profile：Completed 或 TargetReached；
  • 非终态 ACK 只接受 Accepted、Running、TargetReached、Completed。

Rejected、Failed、TimedOut、CommunicationLost 始终视为失败。

result_code 定义：

0  Ok                    1  InvalidCommand
2  InvalidParameter      3  AxisNotReady
4  AxisBusy              5  NotEnabled
6  PositionLimit         7  VelocityLimit
8  AccelerationLimit     9  ZeroNotValid
10 DriveFault           11  CommandTimeout
12 SequenceError        13  SessionMismatch
14 CommunicationWatchdog
15 CyclicWatchdog       16  Unsupported
17 InternalError

status_flags：

bit 名称
0 Enabled
1 Moving
2 TargetReached
3 Fault
4 QuickStopActive
5 CommunicationWatchdogExpired
6 CyclicWatchdogExpired
7 ZeroValid
8 StreamActive
9 CommandBusy

CmvrPlcMotorProtocol 把 Fault、CommunicationWatchdogExpired 和 CyclicWatchdogExpired 都作为致命反馈状态；失败/未知 command_state 或非 Ok result_code 同样无效。此时 getQ/getQd 返回 NaN，reachedTargetQ 返回 false，MotorService 不得把残留的有限位置/速度误判为到位或成功状态。

4.6 缩放和范围

CMVR PLC v1 使用固定缩放：

量 gRPC/C++ 单位 寄存器值
位置 rad round(rad * 1,000,000)，micro-rad
速度 rad/s round(rad/s * 1,000,000)，micro-rad/s
加速度 rad/s^2 round(rad/s^2 * 1,000,000)，micro-rad/s^2
力矩 N·m 预留为 round(N·m * 1,000)，mN·m

前三者当前由 CmvrPlcMotorProtocol 实际使用，并在转换前检查 finite 和 INT32 范围。缩放为 1,000,000 时，理论可表示范围约为 [-2147.483648, 2147.483647] 个对应 SI 单位。PLC 仍必须再次执行软件限位、 驱动器限位和状态检查，不能只依赖上位机检查。

Modbus PLC 电机初始化强制要求每个轴都有有限、有效的 q_lb、q_ub、qd 和 qdd：q_ub > q_lb，且 qd、qdd 均大于 0。缺失、NaN、无穷或非法 限位会令电机初始化失败，不能以“未配置限位”的方式继续带轴运行。

5. payload-first、commit-last 和 ACK

5.1 CMVR 写入顺序

每轴的 command_sequence 独立递增并跳过 0。一次命令严格执行：

  1. 仅 SetZero 先可靠读取 fresh zero_epoch；其他命令不做冗余 baseline 状态读取；
  2. 在本地构造 62 个寄存器的完整 payload；
  3. 同时写入 payload_sequence 和 payload_sequence_mirror；
  4. 使用一次 Holding Register 批量写，把 B+0 .. B+61 写入 PLC；
  5. 再使用第二次写，把同一序列写入 B+62 .. B+63；
  6. 轮询状态区，直到 ack_sequence 等于本次 command sequence；
  7. 检查 command_state 和 result_code；
  8. 周期样本还要检查 last_applied_cyclic_sequence。

PLC 只允许在以下条件全部满足时消费 payload：

commit_sequence != last_processed_commit
payload_sequence == payload_sequence_mirror
payload_sequence == commit_sequence
command_session_id == owner_session_id
当前 cmvr_session_id 是已取得 owner 的有效 session

对于 SetZero，PLC 还必须要求 expected_zero_epoch 等于执行前的当前 zero_epoch；不匹配时以 Rejected + SequenceError ACK，避免陈旧或重复的 置零事务改变新的零位基准。

PLC 应先把完整 payload 复制到内部命令快照，再更新 last_processed_commit。不要一边读取 Holding Register，一边执行驱动器动作。

无论接受还是拒绝，PLC 都应把 ack_sequence 和 ack_session_id 更新为 本次命令的序列和 session，同时填写 command_state 和 result_code。 runtime 只有在两者都精确匹配时才接受 ACK，旧连接残留的相同序列不会被误认。 否则 CMVR 只能得到模糊的 ACK timeout。

如果 commit 已写成功但 ACK 读取失败，电机是否已经执行是不确定的。 CMVR runtime 不会自动重放该运动命令。特别是 SetZero、非零速度和使能命令， 调用方不得在未知结果下盲目重试；应先读取 PLC 状态、boot/session、zero epoch 和实际轴状态。

5.2 boot/session 防重放

PLC 必须把全局 session 和每轴 command sequence 共同作为命令命名空间：

  • cmvr_session_id 在每次成功握手/重连时重新生成非零随机值。检测到新 session 时，PLC 必须先把 owner_state 置为 Accepting，并且这一步必须 早于改写 owner_session_id；随后安全停止旧 owner 的轴、清除旧的 stream-active 状态，并清空各轴旧 mailbox 的 commit/payload 接收状态。 清理全部完成后再写入新的 owner_session_id，最后一步才把 owner_state 发布为 Accepted。这样 runtime 不会把“新 session ID + 旧 Accepted”误认为新 owner 已就绪。拒绝候选 session 时也必须先保持 Accepting、写入对应 owner_session_id，最后一步发布 Rejected。 CMVR 只有看到该 握手确认后才把连接标记为可用。CMVR 会把每轴 command sequence 从 1 重新开始，PLC 必须在新 session 命名空间内接受该序列。runtime 至少保证相邻两次连接的 session 不相同， 避免紧邻重连立即复用旧 mailbox/ACK 命名空间。
  • 每个轴应保存“当前 session 下最后处理的 commit sequence”。相同 commit 只返回原 ACK，不能再次执行。
  • PLC 启动时必须生成新的非零 plc_boot_id，清除 owner 和旧 commit 接受 状态，并要求看到新的有效 session/heartbeat 后才接受命令。若 Holding DB 设置为 retentive，也不能让上次启动遗留的 commit 自动执行。
  • CMVR 周期检查 plc_boot_id。boot ID 变化会断开连接；重连成功后 connection_epoch 改变。Protocol 会锁存并拒绝旧 cyclic generation， 不会自动重新 OpenCyclic*。只有客户端新建 gRPC 流并通过首帧建立新 generation 后才能恢复。
  • Profile 和 Cyclic 命令把 Protocol 已绑定的 connection_epoch 作为 expected_connection_epoch 传给 runtime。runtime 在调用入口、等待每轴 队列之后，以及持有 I/O 锁准备分配 sequence/写 commit 前都要求 invocation/current/expected 三者严格一致。因此即使重连恰好发生在 Protocol 读取 epoch 与 runtime 写 mailbox 之间，旧命令也只会失败，不会 写入新 session。Quick Stop/Disable 不绑定旧 expected epoch，它们作为新调用 只清理当前 session。
  • command sequence 是 32 位并会回绕。PLC 应使用 session 加序列的状态机， 明确处理回绕；不能简单把“任何不相等的值”永远视为新命令。

TCP 自身有序可靠，但不能替代上述应用层规则：PLC DB 可能保留旧值，PLC 和 CMVR 也可能独立重启。

6. S7-1215C DC/DC/DC 与 TIA Portal

6.1 MB_SERVER 数据块

建议创建一个专用、非 retentive 的协议数据块，例如：

HoldingRegister : ARRAY[0 .. 100 + 128 * AXIS_COUNT - 1] OF WORD;

数组下标与本文零基 PDU 地址一致。根据使用的 TIA Portal 和 CPU firmware， MB_HOLD_REG 对数据块访问方式可能有要求；若编译器不允许优化 DB 的 VARIANT/指针映射，应关闭该协议 DB 的 optimized block access。不要把命令 状态机、驱动器实例 DB 与外部可写 Holding Register 直接重叠。

在 OB1 或固定周期 OB 中每个扫描周期调用一个 MB_SERVER 实例。典型参数 包括：

DISCONNECT  = FALSE
CONNECT_ID  = 项目内唯一连接 ID
IP_PORT     = 502
MB_HOLD_REG = 协议 HoldingRegister 数组
NDR/DR/ERROR/STATUS = 诊断输出

不同 TIA Portal 版本的块接口和 VARIANT 写法可能略有差异，应以当前工程中 插入的 MB_SERVER 指令帮助为准。一个 server 实例使用自己的 instance DB； 连接 ID 和 TCP 端口不得与其他 OUC/Modbus 实例冲突。

MB_SERVER 只负责 Modbus TCP 搬运。另建 PLC FB 完成：

  1. 初始化 magic、协议版本、axis count 和 boot ID；
  2. 监视 session 与 heartbeat；
  3. 对每轴执行 payload/commit 原子接收；
  4. 做范围、状态、使能、零位和 command timeout 校验；
  5. 调用 Technology Object、PROFINET 驱动器 telegram 或项目已有驱动器 FB；
  6. 更新 ACK、命令状态、结果码、实际值和状态位；
  7. 执行通信/周期 watchdog 和安全降级。

6.2 32 位辅助函数

PLC 侧应显式实现以下等价逻辑：

DecodeUDInt(high, low) =
    SHL(WORD_TO_DWORD(high), 16) OR WORD_TO_DWORD(low)

EncodeHigh(value) = DWORD_TO_WORD(SHR(value, 16))
EncodeLow(value)  = DWORD_TO_WORD(value AND 16#0000_FFFF)

有符号值先按 DWORD 原样组合，再解释为 DINT。负值使用二进制补码；不要分别 对高、低 WORD 做有符号运算。

6.3 驱动器动作映射

映射必须由实际驱动器/Technology Object 语义决定：

  • SetZero 只执行已确认的“当前位置建立零位”动作，不得自动替换成会运动的 Homing。
  • “回 0 位”当前收到的是 ProfilePosition(target=0)。
  • Profile Position/Velocity 的轨迹生成在 PLC/驱动器侧完成；Modbus TCP 不是驱动器位置环或电流环。
  • Cyclic Position/Velocity 是 CMVR 到 PLC 的软实时 setpoint 更新。PLC 在本地扫描周期锁存最新样本，再由 PLC/驱动器的确定性周期执行。
  • 稳态周期样本通常需要 5 次 Modbus 事务（payload、commit，以及 guard/full/guard 三次 ACK 快照读取）；恰逢心跳到期时增加 1 次。首个样本 还需要先完成一次惰性的 Open。这个事务模型不承诺固定控制频率，也不是硬 实时链路。
  • Quick Stop 的减速度、抱闸时序和重力轴保持策略必须在 PLC/驱动器中配置。
  • Enable/Disable 必须检查故障、STO、抱闸和轴 ready 状态；不能只翻转一个 普通布尔位。

PLC 必须在执行前再次检查位置、速度、加速度、驱动器状态和项目级互锁，并把 拒绝原因写入 result_code。

当前测试只在 x86 loopback fake PLC 上验证功能和协议一致性，尚未给出 S7-1215C 实机可持续频率。投产前必须在目标 TIA Portal 程序、真实 PLC 扫描 周期和现场交换网络下阶梯增加 CSP/CSV 发送频率，记录 ACK 延迟的 P50/P99/最大值、dropped_setpoints、Modbus 异常与 watchdog 触发次数。 cyclic_watchdog_ms 应依据实测最坏延迟并保留工程余量设置；在完成这项台架 测试前，不能宣称支持某个固定 Hz。

CMVR 中的 QuickStop 和 Disable 走 safety-priority 通道：它们会增加该轴 的取消 generation，令正在等待的普通命令失败，并绕过普通轴命令互斥锁。安全 命令不先读取状态，也不在 mailbox 前插入心跳写；拿到 socket 后首先发送安全 payload/commit。它仍与单个 Modbus socket 的一次事务互斥，不会把两条报文 交错写入；普通命令在 commit 前会再次检查 generation，避免急停完成后补发 旧运动命令。

同一轴的 safety 命令使用独立 safety mutex 串行。每个 safety 调用在等待该锁 之前就提升普通命令的取消 generation，因此抢占不会被前一个 Quick Stop 阻塞； 但后来的 safety 调用不会取消前一个 safety 调用的 ACK 等待，多个并发 Quick Stop/Disable 都能得到各自确定的执行结果。

6.4 心跳、watchdog 和断链

CMVR 默认每 100 ms 写一次 cmvr_session_id + cmvr_heartbeat。本仓库实体 样例要求 PLC 在 1000 ms 通信 watchdog 内看到 heartbeat 发生变化 （字段为 0 时 runtime 默认 500 ms）。PLC 应使用自己的 单调时间测量“最后一次变化”的年龄，不能只检查 TCP socket 仍连接，也不能把 重复读到同一个 counter 当作有效心跳。

命令提交不会为每个周期样本强制写 heartbeat；runtime 记录最后一次成功写入 时刻，仅在 heartbeat_period_ms 已到期时由当前命令顺带补写。supervisor worker 仍按周期写心跳并检查 PLC boot/session/heartbeat，因此高频 setpoint 既不会产生一倍额外 Modbus 写流量，也不会饿死通信 watchdog。

推荐 PLC 状态机：

无 owner
  -> 收到有效非零 session 且 heartbeat 开始变化
  -> owner active
  -> 接受该 session 的 commit

owner active
  -> heartbeat age 超过 communication_watchdog_ms
  -> 对所有 owner 轴执行受控停止/Quick Stop
  -> 设置 CommunicationWatchdogExpired
  -> command_state = CommunicationLost
  -> 释放 owner

周期流还需要每轴独立 watchdog。只有新的 Cyclic*Sample.cyclic_sample_sequence 才刷新它；普通 CMVR 心跳不能让旧的 非零速度无限保持。周期 watchdog 超时后应停止该轴、清除 StreamActive， 设置 CyclicWatchdogExpired，并要求重新 OpenCyclic*。网络恢复后不得 自动恢复断链前的速度或 setpoint；旧 gRPC 流必须失败关闭，由客户端新建流。

CMVR runtime 遇到 Modbus 读写错误会关闭 socket，并按 reconnect_min_ms 到 reconnect_max_ms 指数退避重连。它不会自动重放上一 条运动命令。PLC 侧安全动作必须在没有 CMVR 参与的情况下独立完成。

7. 安全边界

标准 S7-1215C DC/DC/DC 不是 failsafe PLC。MB_SERVER、普通 OB/FB、 普通数字输出以及本服务的 emergencyStop 都只是功能性控制，不能提供 安全等级的急停、STO 或防护门联锁。

实际设备至少应按风险评估使用：

  • 硬接线急停回路；
  • 合规的安全继电器，或 F-CPU + F-I/O；
  • 驱动器 STO 双通道或经认证的安全功能；
  • 接触器/抱闸反馈和必要的 EDM；
  • 与机械负载、重力轴和制动距离匹配的安全设计。

软件 emergencyStop 和 Modbus Quick Stop 可以作为操作层的快速停止，但 不能替代硬接线安全回路。标准 CPU 程序卡死、以太网交换机故障、普通输出粘连 或软件错误时，硬件安全链仍必须独立切断危险能量。

8. 构建、依赖和测试

8.1 x86-64

仓库已有 x86-64 的 libmodbus 3.1.11：

dependency/x86/third_party/modbus/3.1.11/include/modbus
dependency/x86/third_party/modbus/3.1.11/lib/libmodbus.so
dependency/x86/third_party/modbus/3.1.11/lib/libmodbus.so.5

request.txt 已包含 third_party/modbus/3.1.11。标准构建流程：

cmake -S . -B build -DBUILD_TESTING=ON
cmake --build build -j"$(nproc)"
ctest --test-dir build --output-on-failure
cmake --install build
ldd -r output/bin/cmvr_es | grep -E 'modbus|not found'

只验证本次 MotorService/PLC 电机链路时，可执行：

cmake --build build --target \
  modbus_tcp_motor_bus_runtime_test \
  grpc_motor_service_test \
  grpc_motor_service_modbus_e2e_test \
  -j4
ctest --test-dir build \
  -R '^(modbus_tcp_motor_bus_runtime_test|grpc_motor_service_test|grpc_motor_service_modbus_e2e_test)$' \
  --output-on-failure

当前这 3 个 CTest 目标共包含 63 个 GoogleTest 用例：Modbus runtime 25 个、 MotorService 36 个、gRPC–Modbus 端到端 2 个。

测试分别覆盖：

  • Modbus runtime 的命令、离线启动后上线、重连/stop-start session 隔离、 stale owner 决策、握手身份漂移、三段 seqlock 撕裂重试、旧 cyclic generation 跨 epoch 的 fail-closed 锁存、Protocol-to-runtime epoch TOCTOU 拒绝、Profile 反馈 session 绑定、CSP/CSV 样本 ACK、冗余 状态/心跳事务抑制、并发 safety 串行、故障反馈拒绝、非法轴/样本，以及 仓库真实样例配置的解析和 runtime 初始化；
  • MotorService 的同步 Profile Position/Velocity 等待、取消/超时、急停竞争、 异常边界、CSP/CSV 流、流背压和停止失败；
  • 单进程真实链路 gRPC stub → DeviceManager → MotorManager → AbstractMotor → CMVR PLC protocol → Modbus TCP fake PLC，包括使能、Profile Position/Velocity、CSP、half-close Quick Stop、急停锁存、活动 cyclic 流 跨重连失败关闭、新流恢复，以及断线重连不重放。

fake PLC 用例会在 loopback 地址启动本地 server，运行环境必须允许本地 TCP bind/listen。

运行安装产物：

./output/bin/cmvr_es

默认 gRPC 端口由 cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt 配置，当前为 50052。启动前应先用禁能或脱载轴验证 PLC 寄存器和方向。

8.2 启用配置和 gRPC 调用

首次联调前：

  1. 把 cmvr-es/config/devices/motor/plc_motors.pb.txt 中的 host、轴映射 和关节限位改成现场值；
  2. 完成 PLC watchdog、驱动器 Quick Stop 和硬件安全链检查；
  3. 将 cmvr-es/config/manager/device_manager.pb.txt 中 plc_motors 的 enable 改为 true；
  4. 重新执行 cmake --install build，再启动 ./output/bin/cmvr_es。

启用 reflection 后，可先确认服务和状态：

grpcurl -plaintext 127.0.0.1:50052 list cmvr.api.MotorService

grpcurl -plaintext \
  -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"}}' \
  127.0.0.1:50052 cmvr.api.MotorService/getStatus

在轴已安全脱载、PLC/驱动器允许使能后，显式使能并执行一个同步位置命令：

grpcurl -plaintext \
  -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"},"enabled":true}' \
  127.0.0.1:50052 cmvr.api.MotorService/setEnabled

grpcurl -plaintext \
  -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"},"targetPositionRad":0.1,"maxVelocityRadS":0.2,"accelerationRadS2":0.5,"wait":{"timeoutMs":30000}}' \
  127.0.0.1:50052 cmvr.api.MotorService/profilePosition

软件 Quick Stop：

grpcurl -plaintext \
  -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"}}' \
  127.0.0.1:50052 cmvr.api.MotorService/emergencyStop

双向周期流应使用生成的 gRPC client stub，并发写 setpoint、持续读反馈；不要 用只发送一次 JSON 的 unary 调用方式模拟。客户端必须先收到 OPENED，发送 首个样本，再等待相同 sequence 的 APPLIED。正常退出时 half-close 写端并 继续读取，直到收到 STOPPED 和最终 OK status。

当前服务默认监听 0.0.0.0:50052，使用 insecure gRPC，任何可达客户端都能 发控制命令。现场至少应绑定可信控制网接口或回环地址并配置防火墙；不得直接 暴露到办公网或公网。若需要跨不可信网络访问，应在进入设备前增加认证、TLS 和工业安全网关。

8.3 ARM 当前缺失

dependency/arm/third_party/ 当前没有 libmodbus。现有 libmodbus.so.5.1.0 是 x86-64 ELF，不能复制到 ARM 设备使用。

ARM 支持前需要：

  1. 为目标 ARM ABI 编译 libmodbus 3.1.11；
  2. 按相同布局放入 dependency/arm/third_party/modbus/3.1.11/{include,lib}；
  3. 确认 ARM toolchain/顶层 CMake 选择 dependency/arm。当前顶层 CMakeLists.txt 仍把 ARCH 设为 x86；
  4. 在目标设备执行 file、readelf -h 和 ldd -r 验证架构、SONAME 和 运行时依赖；
  5. 重新执行无硬件测试和 PLC 台架测试。

在这些步骤完成前，ARM 构建应视为不支持 Modbus PLC 电机后端。

8.4 PLC 台架检查

建议按以下顺序验证：

  1. PLC 上电后检查 magic、版本、axis count、boot ID。
  2. 只连接 Modbus，确认 session、CMVR heartbeat 和 PLC heartbeat。
  3. 禁能状态验证错误参数、重复 commit、旧 session 和 ACK/result。
  4. 验证 SetZero 的项目语义，确认没有意外运动。
  5. 低速、低加速度验证 Profile Position 和 Profile Velocity。
  6. 验证周期流正常结束、gRPC watchdog 和 PLC cyclic watchdog。
  7. 分别拔网线、停止 cmvr_es、重启交换机、重启 PLC，确认不会恢复旧速度。
  8. 验证 emergencyStop 后必须显式 enable 才能再次运动。
  9. 在硬件安全回路测试合格后，才允许带载运行。

9. 当前已知约束

  • MotorService 是单轴 API，没有多轴同扫描周期 commit。
  • DeviceManager/MotorManager 拓扑在 gRPC 服务运行期间必须保持不变； 当前管理器只支持启动期注册，不支持在仍有 RPC 或流持有电机时热移除、 热替换同 ID 的 MotorManager。需要换配置时，应先停止 gRPC 服务和设备， 再重建运行时。
  • Profile Torque、Cyclic Torque、Clear Fault 和独立 Homing 未开放。
  • gRPC 流的同步 Write() 可能受慢客户端背压；PLC watchdog 必须独立。
  • getStatus 不是一次原子 Modbus 快照。
  • Modbus TCP 不提供认证、加密或安全完整性。控制网络应隔离，并在需要时通过 防火墙/VPN/工业安全网关限制访问。
  • Modbus TCP 的普通软件停止不具备功能安全等级。