From 60098ee01e837a6598aeee187fe5916219bf4f17 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 4 Aug 2026 10:36:23 +0800 Subject: [PATCH] refactor: remove PLC motor control backend --- README.md | 5 +- .../config/devices/motor/plc_motors.pb.txt | 56 - cmvr-es/config/manager/device_manager.pb.txt | 8 - cmvr-es/devices/README.md | 2 +- cmvr-es/devices/motor/CMakeLists.txt | 47 - cmvr-es/devices/motor/README.md | 71 +- .../devices/motor/bus_runtime/CMakeLists.txt | 13 +- .../motor/bus_runtime/modbus_tcp/README.md | 93 - .../include/cmvr_plc_register_map.h | 209 --- .../modbus_tcp/include/modbus_tcp_client.h | 47 - .../include/modbus_tcp_motor_bus_runtime.h | 173 -- .../modbus_tcp/src/modbus_tcp_client.cpp | 188 -- .../src/modbus_tcp_motor_bus_runtime.cpp | 1013 ----------- .../modbus_tcp_motor_bus_runtime_test.cpp | 1586 ----------------- .../drivers/modbus_plc_motor/CMakeLists.txt | 21 - .../include/cmvr_plc_motor_protocol.h | 89 - .../include/modbus_plc_motor.h | 21 - .../src/cmvr_plc_motor_protocol.cpp | 582 ------ .../modbus_plc_motor/src/modbus_plc_motor.cpp | 55 - cmvr-es/devices/motor/manager/CMakeLists.txt | 1 - .../motor/manager/include/motor_manager.h | 4 - .../motor/manager/src/motor_manager.cpp | 61 - cmvr-es/service/CMakeLists.txt | 38 - cmvr-es/service/README.md | 9 +- .../service/grpc/src/grpc_motor_service.cpp | 4 +- .../grpc_motor_service_modbus_e2e_test.cpp | 907 ---------- .../grpc/tests/grpc_motor_service_test.cpp | 4 +- docs/motor_service_modbus_tcp.md | 1022 ----------- protos/cmvr/api/motor_command.proto | 2 +- .../config/motor_config/motor_config.proto | 36 +- 30 files changed, 36 insertions(+), 6331 deletions(-) delete mode 100644 cmvr-es/config/devices/motor/plc_motors.pb.txt delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp delete mode 100644 cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp delete mode 100644 docs/motor_service_modbus_tcp.md diff --git a/README.md b/README.md index 9da1699d..5913f0fd 100644 --- a/README.md +++ b/README.md @@ -145,10 +145,7 @@ sudo script/ethercat/stop_ethercat.sh eno1 --restore-network - [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice) - [电机设备模块](cmvr-es/devices/motor/README.md) -- [Modbus TCP PLC runtime](cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md) -- [MotorService 与 CMVR PLC v1 完整协议](docs/motor_service_modbus_tcp.md) - [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md) - [配置与部署规则](cmvr-es/config/README.md) -Modbus Quick Stop 只是功能性停止，不能替代硬接线急停或驱动器 STO。AUBO -JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。 +AUBO JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。 diff --git a/cmvr-es/config/devices/motor/plc_motors.pb.txt b/cmvr-es/config/devices/motor/plc_motors.pb.txt deleted file mode 100644 index 0a1581c2..00000000 --- a/cmvr-es/config/devices/motor/plc_motors.pb.txt +++ /dev/null @@ -1,56 +0,0 @@ -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" } - } - } -} diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 07204d9f..8c8c691d 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -82,14 +82,6 @@ device_manager { enable: false } - devices { - id: "plc_motors" - type: DEVICE_TYPE_MOTOR_SYSTEM - config_file: "devices/motor/plc_motors.pb.txt" - # Configure the PLC endpoint and complete the safety checkout before enabling. - enable: false - } - devices { id: "right_arm" type: DEVICE_TYPE_ROBOT_ARM diff --git a/cmvr-es/devices/README.md b/cmvr-es/devices/README.md index 2642c4f6..b979284f 100644 --- a/cmvr-es/devices/README.md +++ b/cmvr-es/devices/README.md @@ -42,7 +42,7 @@ config/cmvr_es.pb.txt | Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg | | Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg | | BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot | -| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT、Modbus TCP PLC | +| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT | 代码目录存在不等于已经接入配置创建链： diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 08fa5f13..a7399736 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -17,52 +17,5 @@ add_subdirectory(drivers/ti5_canopen) add_subdirectory(drivers/mujoco) add_subdirectory(bus_runtime) add_subdirectory(drivers/ethercat_motor) -add_subdirectory(drivers/modbus_plc_motor) - -if(BUILD_TESTING) - enable_testing() - set(CMVR_MOTOR_LIBMODBUS_ROOT - ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11) - add_executable(modbus_tcp_motor_bus_runtime_test - bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp - ) - target_include_directories(modbus_tcp_motor_bus_runtime_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMVR_MOTOR_LIBMODBUS_ROOT}/include - ) - target_link_directories(modbus_tcp_motor_bus_runtime_test - PRIVATE - ${CMVR_MOTOR_LIBMODBUS_ROOT}/lib - ) - target_link_libraries(modbus_tcp_motor_bus_runtime_test - PRIVATE - cmvr_es::device::motor_bus_runtime - cmvr_es::device::modbus_plc_motor_driver - cmvr_es::proto - modbus - gtest - gtest_main - pthread - glog - ) - target_compile_definitions(modbus_tcp_motor_bus_runtime_test - PRIVATE - CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH="${CMAKE_SOURCE_DIR}/cmvr-es/config/devices/motor/plc_motors.pb.txt" - ) - add_test(NAME modbus_tcp_motor_bus_runtime_test - COMMAND modbus_tcp_motor_bus_runtime_test) - set(_modbus_motor_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _modbus_motor_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(modbus_tcp_motor_bus_runtime_test PROPERTIES - TIMEOUT 15 - ENVIRONMENT "${_modbus_motor_test_environment}" - ) -endif() add_subdirectory(manager) diff --git a/cmvr-es/devices/motor/README.md b/cmvr-es/devices/motor/README.md index a10ba7fc..46949387 100644 --- a/cmvr-es/devices/motor/README.md +++ b/cmvr-es/devices/motor/README.md @@ -2,7 +2,7 @@ `devices/motor/` 提供电机管理、协议适配、总线 runtime 和厂商驱动。Service、 RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口，不应 -直接访问 libmodbus、CAN、EtherCAT 或厂商 SDK。 +直接访问 CAN、EtherCAT、MuJoCo 或厂商 SDK。 返回 [Devices 模块指南](../README.md) 或 [项目总览](../../../README.md)。 @@ -11,72 +11,37 @@ RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口 | 目录 | 职责 | | --- | --- | | `manager/` | 创建 MotorGroup，按 `motor_id`/`joint_name` 暴露 `AbstractMotor` | -| `bus_runtime/` | 连接、收发、重连、watchdog 和总线生命周期 | -| `drivers/` | CANopen、EtherCAT、MuJoCo、Modbus PLC 等具体后端 | -| `drivers/modbus_plc_motor/` | 关节限位、SI 单位和 CMVR PLC v1 命令映射 | +| `bus_runtime/` | 连接、收发和总线生命周期 | +| `drivers/` | CANopen、EtherCAT 和 MuJoCo 等具体后端 | -PLC 电机链路为： +## 当前后端 -```text -gRPC MotorService - | - v -MotorManager -> AbstractMotor - | - v -CmvrPlcMotorProtocol - | - v -ModbusTcpMotorBusRuntime - | - v -libmodbus -> PLC -> 驱动器/电机 -``` +- CAN + TI5 CANopen； +- EtherCAT + EYOU CiA 402； +- MuJoCo 仿真电机。 -各层边界： +配置示例： -- `MotorService` 负责 API 校验、单电机控制权、deadline/cancellation 和 - fail-closed Quick Stop； -- `MotorManager` 负责电机查找与统一抽象； -- `CmvrPlcMotorProtocol` 负责关节限位、SI 单位和 CMVR PLC v1 命令映射； -- `ModbusTcpMotorBusRuntime` 负责 PLC session、mailbox、ACK、状态和重连； -- PLC/驱动器必须独立实现通信 watchdog、周期 watchdog 和硬件安全动作。 +- [`ti5_motors.pb.txt`](../../config/devices/motor/ti5_motors.pb.txt) +- [`ethercat_motors.pb.txt`](../../config/devices/motor/ethercat_motors.pb.txt) +- [`mujoco_motors.pb.txt`](../../config/devices/motor/mujoco_motors.pb.txt) -## Modbus TCP PLC - -实现、配置、依赖和测试入口见： - -- [Modbus TCP runtime README](bus_runtime/modbus_tcp/README.md) -- [完整 MotorService/CMVR PLC v1 协议](../../../docs/motor_service_modbus_tcp.md) -- [MotorService 文档](../../service/README.md#motorservice) -- [`plc_motors.pb.txt`](../../config/devices/motor/plc_motors.pb.txt) - -当前 Modbus 后端只提供 x86-64 的 libmodbus 3.1.11。ARM 目录没有对应库， -不能把 x86 ELF 复制到 ARM 设备使用。 +对外接口与控制权语义见 [MotorService 文档](../../service/README.md#motorservice)。 ## 安全边界 - MotorService 是单轴 API，不提供多轴同扫描周期的原子 commit； -- PLC/Modbus 的 Profile 或 cyclic 能力不能直接当成毫秒级机械臂组伺服； -- 软件 `emergencyStop`、Quick Stop 和普通 PLC 输出都不是安全急停； -- 真实设备必须具有独立的硬接线急停、安全继电器或 F-CPU/F-I/O，以及驱动器 - STO 等经风险评估确定的安全链； -- 新硬件配置保持 `enable: false`，完成方向、限位、watchdog 和故障注入验证后 - 才能启用。 +- 软件 `emergencyStop` 和 Quick Stop 不具备功能安全等级； +- 真实设备必须具有经风险评估确定的硬接线急停、安全继电器和驱动器安全链； +- 新硬件配置保持 `enable: false`，完成方向、限位和故障注入验证后才能启用。 ## 测试 ```bash -cmake --build build --target \ - modbus_tcp_motor_bus_runtime_test \ - grpc_motor_service_test \ - grpc_motor_service_modbus_e2e_test \ - -j4 - +cmake --build build --target grpc_motor_service_test -j4 ctest --test-dir build \ - -R '^(modbus_tcp_motor_bus_runtime_test|grpc_motor_service_test|grpc_motor_service_modbus_e2e_test)$' \ + -R '^grpc_motor_service_test$' \ --output-on-failure ``` -端到端 fake PLC 测试需要本地 TCP bind/listen 权限。软件测试不能代替真实 -S7-1215C、驱动器、STO 和断网故障台架。 +该测试使用 fake motor，不替代真实总线、驱动器或安全链验证。 diff --git a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt index e2a9c6f2..458b8e46 100644 --- a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt +++ b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt @@ -2,29 +2,19 @@ add_library(motor_bus_runtime SHARED can/src/can_motor_bus_runtime.cpp mujoco/src/mujoco_motor_bus_runtime.cpp ethercat/src/ethercat_motor_bus_runtime.cpp - modbus_tcp/src/modbus_tcp_client.cpp - modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp ) set(IGH_ETHERCAT_ROOT ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0 ) -set(CMVR_LIBMODBUS_ROOT - ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11 -) - target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PRIVATE ${IGH_ETHERCAT_ROOT}/include - ${CMVR_LIBMODBUS_ROOT}/include ) -target_link_directories(motor_bus_runtime PRIVATE - ${IGH_ETHERCAT_ROOT}/lib - ${CMVR_LIBMODBUS_ROOT}/lib -) +target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib) target_link_libraries(motor_bus_runtime PUBLIC @@ -33,7 +23,6 @@ target_link_libraries(motor_bus_runtime cmvr_es::mujoco_world PRIVATE ethercat - modbus cmvr_es::device::canbus glog ) diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md deleted file mode 100644 index f1ac4f18..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md +++ /dev/null @@ -1,93 +0,0 @@ -# Modbus TCP PLC Runtime - -本目录实现 CMVR PLC v1 的 Modbus TCP 总线 runtime。它负责 PLC 连接、身份 -握手、session、心跳、重连、命令 mailbox、ACK 轮询和状态快照，不负责 gRPC -请求解析，也不提供功能安全急停。 - -返回 [Motor 设备模块](../../README.md) 或 [Devices 模块指南](../../../README.md)。 - -## 代码与配置 - -- [`include/modbus_tcp_client.h`](include/modbus_tcp_client.h)：有界超时的 - libmodbus client -- [`include/modbus_tcp_motor_bus_runtime.h`](include/modbus_tcp_motor_bus_runtime.h)： - 连接 supervisor、命令和状态 runtime -- [`include/cmvr_plc_register_map.h`](include/cmvr_plc_register_map.h)： - CMVR PLC v1 寄存器常量 -- [`src/modbus_tcp_client.cpp`](src/modbus_tcp_client.cpp) -- [`src/modbus_tcp_motor_bus_runtime.cpp`](src/modbus_tcp_motor_bus_runtime.cpp) -- [`tests/modbus_tcp_motor_bus_runtime_test.cpp`](tests/modbus_tcp_motor_bus_runtime_test.cpp) -- [`../../../../config/devices/motor/plc_motors.pb.txt`](../../../../config/devices/motor/plc_motors.pb.txt) - -完整寄存器表、TIA Portal 数据块、gRPC 语义和台架步骤由 -[MotorService 与 CMVR PLC v1 完整协议](../../../../../docs/motor_service_modbus_tcp.md) -统一维护。 - -## 连接与协议约束 - -- `host` 必须是 IPv4 字面量，避免 DNS 让建连或停止出现无界等待； -- PLC boot ID 必须非零且每次 PLC 重启变化； -- owner 决策和命令 ACK 必须回显当前 session，旧 session 的 mailbox 不得执行； -- runtime `start()` 启动连接 supervisor，PLC 可以在进程启动时离线； -- 离线期间状态不可用，运动命令必须在写 mailbox 前失败； -- 重连必须重新完成身份、版本、boot ID 和 session 握手，不得重放旧命令； -- 状态区按 odd/even seqlock 发布，CMVR 使用 - sequence-before → 64-word block → sequence-after 三段读取； -- payload 必须先完整写入，commit sequence 最后写入，PLC 只原子消费新的 - commit； -- Quick Stop、Disable、故障和通信 watchdog 的状态必须通过当前 session - 的 ACK/状态确认，不能把本地写成功解释为驱动器已经安全停止。 - -## Cyclic stream - -每次 `OpenCyclicPosition/Velocity` 都创建新的轴级 stream epoch。PLC 必须在 -同一个原子状态事务中： - -1. 清零 `last_applied_cyclic_sequence`； -2. 清理旧样本去重状态； -3. 重置 cyclic watchdog； -4. 设置正确的 CSP/CSV mode； -5. 发布 `StreamActive=1` 后再 ACK。 - -重开后的首个样本序列从 `1` 开始，必须重新应用。活动 cyclic 流跨 -`connection_epoch` 后不会自动重开；旧流的当前和后续 setpoint 都被拒绝， -Quick Stop 后客户端必须建立新的 gRPC 流。断链前或断链期间 pending 的 -setpoint 不得进入新 session。 - -## 依赖与平台 - -x86-64 的 libmodbus 3.1.11 位于： - -```text -dependency/x86/third_party/modbus/3.1.11 -``` - -`dependency/arm/third_party/` 当前没有对应 libmodbus，因此 ARM 构建不支持 -该后端。支持 ARM 前必须为目标 ABI 单独编译并验证库，不能复用 x86 二进制。 - -## 安全边界 - -标准 S7-1215C DC/DC/DC 不是 failsafe PLC。Modbus Quick Stop、 -MotorService `emergencyStop`、普通 OB/FB 和普通数字输出都只是功能性控制， -不能替代： - -- 硬接线急停； -- 安全继电器或 F-CPU/F-I/O； -- 驱动器双通道 STO； -- 接触器、抱闸反馈与必要的 EDM。 - -PLC 侧通信 watchdog 和 cyclic watchdog 必须在没有 CMVR 进程参与时独立停止 -危险运动。真实启用前必须在禁能或脱载轴上验证寄存器、方向、限位、断网、PLC -重启、交换机故障和 Quick Stop 失败。 - -## 测试 - -```bash -cmake --build build --target modbus_tcp_motor_bus_runtime_test -j4 -ctest --test-dir build \ - -R '^modbus_tcp_motor_bus_runtime_test$' \ - --output-on-failure -``` - -fake PLC 测试需要本地 TCP bind/listen 权限。测试通过只证明软件协议和故障注入 -路径，不代表真实 PLC、驱动器或硬件安全链已经验收。 diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h deleted file mode 100644 index 4c09afa9..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h +++ /dev/null @@ -1,209 +0,0 @@ -#ifndef CMVR_ES_CMVR_PLC_REGISTER_MAP_H -#define CMVR_ES_CMVR_PLC_REGISTER_MAP_H - -#include -#include -#include - -namespace cmvr::device::cmvr_plc { - -constexpr std::uint16_t kMagicCm = 0x434d; -constexpr std::uint16_t kMagicVr = 0x5652; -constexpr std::uint16_t kProtocolMajor = 1; -constexpr std::uint16_t kProtocolMinor = 0; -constexpr double kPositionScale = 1000000.0; -constexpr double kVelocityScale = 1000000.0; -constexpr double kAccelerationScale = 1000000.0; -constexpr double kTorqueScale = 1000.0; - -constexpr int kGlobalRegisterCount = 32; -constexpr int kMagicCmOffset = 0; -constexpr int kMagicVrOffset = 1; -constexpr int kProtocolMajorOffset = 2; -constexpr int kProtocolMinorOffset = 3; -constexpr int kAxisCountOffset = 4; -constexpr int kPlcGlobalStateOffset = 5; -constexpr int kPlcBootIdOffset = 6; -constexpr int kCmvrSessionIdOffset = 8; -constexpr int kCmvrHeartbeatOffset = 10; -constexpr int kPlcHeartbeatOffset = 12; -constexpr int kCommunicationWatchdogOffset = 14; -constexpr int kGlobalErrorOffset = 16; -constexpr int kOwnerStateOffset = 17; -constexpr int kOwnerSessionIdOffset = 18; - -constexpr int kAxisFirstOffset = 100; -constexpr int kAxisRegisterStride = 128; -constexpr int kAxisControlRegisterCount = 64; -constexpr int kAxisStatusRelativeOffset = 64; -constexpr int kAxisStatusRegisterCount = 64; - -constexpr int axisBase(const std::uint32_t axis_index) -{ - return kAxisFirstOffset + static_cast(axis_index) * kAxisRegisterStride; -} - -enum class CommandCode : std::uint16_t { - Nop = 0, - SetZero = 1, - MoveToZero = 2, - ProfilePosition = 3, - ProfileVelocity = 4, - OpenCyclicPosition = 5, - CyclicPositionSample = 6, - OpenCyclicVelocity = 7, - CyclicVelocitySample = 8, - CloseCyclicStream = 9, - QuickStop = 10, - Enable = 11, - Disable = 12, -}; - -enum class CommandState : std::uint16_t { - Idle = 0, - Received = 1, - Validating = 2, - Accepted = 3, - Running = 4, - TargetReached = 5, - Completed = 6, - Rejected = 7, - Failed = 8, - TimedOut = 9, - QuickStopped = 10, - CommunicationLost = 11, -}; - -enum class ResultCode : std::uint16_t { - Ok = 0, - InvalidCommand = 1, - InvalidParameter = 2, - AxisNotReady = 3, - AxisBusy = 4, - NotEnabled = 5, - PositionLimit = 6, - VelocityLimit = 7, - AccelerationLimit = 8, - ZeroNotValid = 9, - DriveFault = 10, - CommandTimeout = 11, - SequenceError = 12, - SessionMismatch = 13, - CommunicationWatchdog = 14, - CyclicWatchdog = 15, - Unsupported = 16, - InternalError = 17, -}; - -enum StatusFlag : std::uint16_t { - Enabled = 1U << 0U, - Moving = 1U << 1U, - TargetReached = 1U << 2U, - Fault = 1U << 3U, - QuickStopActive = 1U << 4U, - CommunicationWatchdogExpired = 1U << 5U, - CyclicWatchdogExpired = 1U << 6U, - ZeroValid = 1U << 7U, - StreamActive = 1U << 8U, - CommandBusy = 1U << 9U, -}; - -constexpr std::size_t kCommandPayloadRegisterCount = 62; -constexpr int kCommitSequenceRelativeOffset = 62; - -constexpr std::size_t kPayloadSequence = 0; -constexpr std::size_t kCommandCode = 2; -constexpr std::size_t kCommandFlags = 3; -constexpr std::size_t kTargetPosition = 4; -constexpr std::size_t kTargetVelocity = 6; -constexpr std::size_t kAcceleration = 8; -constexpr std::size_t kTargetTorque = 10; -constexpr std::size_t kPositionTolerance = 12; -constexpr std::size_t kVelocityTolerance = 14; -constexpr std::size_t kCommandTimeout = 16; -constexpr std::size_t kStreamWatchdog = 18; -constexpr std::size_t kCyclicSampleSequence = 20; -constexpr std::size_t kClientMonotonicTime = 22; -constexpr std::size_t kExpectedZeroEpoch = 24; -constexpr std::size_t kDisconnectAction = 26; -constexpr std::size_t kCommandSessionId = 27; -constexpr std::size_t kPayloadSequenceMirror = 60; - -constexpr std::size_t kAckSequence = 0; -constexpr std::size_t kActiveSequence = 2; -constexpr std::size_t kCommandState = 4; -constexpr std::size_t kResultCode = 5; -constexpr std::size_t kAxisState = 6; -constexpr std::size_t kCurrentMode = 7; -constexpr std::size_t kActualPosition = 8; -constexpr std::size_t kActualVelocity = 10; -constexpr std::size_t kActualTorque = 12; -constexpr std::size_t kTargetPositionStatus = 14; -constexpr std::size_t kTargetVelocityStatus = 16; -constexpr std::size_t kStatusFlags = 18; -constexpr std::size_t kDriveStatusword = 19; -constexpr std::size_t kFaultCode = 20; -constexpr std::size_t kZeroEpoch = 22; -constexpr std::size_t kLastAppliedCyclicSequence = 24; -constexpr std::size_t kStateSequence = 26; -constexpr std::size_t kPlcMonotonicTime = 28; -constexpr std::size_t kHeartbeatAge = 30; -constexpr std::size_t kAckSessionId = 32; -constexpr std::size_t kStateSequenceMirror = 62; - -enum class OwnerState : std::uint16_t { - None = 0, - Accepting = 1, - Accepted = 2, - Rejected = 3, -}; - -inline void encodeUint32(std::uint16_t* registers, - const std::size_t offset, - const std::uint32_t value) -{ - registers[offset] = static_cast(value >> 16U); - registers[offset + 1] = static_cast(value & 0xffffU); -} - -inline void encodeInt32(std::uint16_t* registers, - const std::size_t offset, - const std::int32_t value) -{ - encodeUint32(registers, offset, static_cast(value)); -} - -inline std::uint32_t decodeUint32(const std::uint16_t* registers, - const std::size_t offset) -{ - return (static_cast(registers[offset]) << 16U) | - static_cast(registers[offset + 1]); -} - -inline std::int32_t decodeInt32(const std::uint16_t* registers, - const std::size_t offset) -{ - return static_cast(decodeUint32(registers, offset)); -} - -inline bool isTerminal(const CommandState state) -{ - return state == CommandState::Completed || - state == CommandState::Rejected || - state == CommandState::Failed || - state == CommandState::TimedOut || - state == CommandState::QuickStopped || - state == CommandState::CommunicationLost; -} - -inline bool isFailure(const CommandState state) -{ - return state == CommandState::Rejected || - state == CommandState::Failed || - state == CommandState::TimedOut || - state == CommandState::CommunicationLost; -} - -} // namespace cmvr::device::cmvr_plc - -#endif // CMVR_ES_CMVR_PLC_REGISTER_MAP_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h deleted file mode 100644 index 2a8015bc..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef CMVR_ES_MODBUS_TCP_CLIENT_H -#define CMVR_ES_MODBUS_TCP_CLIENT_H - -#include -#include -#include -#include - -struct _modbus; -using modbus_t = struct _modbus; - -namespace cmvr::device { - -class ModbusTcpClient { -public: - ModbusTcpClient() = default; - ~ModbusTcpClient(); - - ModbusTcpClient(const ModbusTcpClient&) = delete; - ModbusTcpClient& operator=(const ModbusTcpClient&) = delete; - - bool open(const std::string& host, - std::uint16_t port, - std::uint8_t unit_id, - std::uint32_t connect_timeout_ms, - std::uint32_t response_timeout_ms); - void close(); - void shutdown(); - bool connected() const { return context_ != nullptr; } - - bool readHoldingRegisters(int address, int count, std::vector& values); - bool writeHoldingRegisters(int address, const std::uint16_t* values, int count); - bool writeHoldingRegisters(int address, const std::vector& values); - - const std::string& lastError() const { return last_error_; } - -private: - void setLastErrnoError_(const char* operation); - - modbus_t* context_{nullptr}; - std::atomic socket_fd_{-1}; - std::string last_error_; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_MODBUS_TCP_CLIENT_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h deleted file mode 100644 index 110dbd3f..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h +++ /dev/null @@ -1,173 +0,0 @@ -#ifndef CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H -#define CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h" - -namespace cmvr::device { - -struct ModbusTcpMotorBusRuntimeTestAccess; - -struct CmvrPlcAxisCommand { - cmvr_plc::CommandCode code{cmvr_plc::CommandCode::Nop}; - std::uint16_t flags{0}; - std::int32_t target_position{0}; - std::int32_t target_velocity{0}; - std::int32_t acceleration{0}; - std::int32_t target_torque{0}; - std::int32_t position_tolerance{0}; - std::int32_t velocity_tolerance{0}; - std::uint32_t command_timeout_ms{0}; - std::uint32_t stream_watchdog_ms{0}; - std::uint32_t cyclic_sample_sequence{0}; - std::uint32_t client_monotonic_time_ms{0}; - std::uint32_t expected_zero_epoch{0}; - std::uint16_t disconnect_action{0}; -}; - -struct CmvrPlcAxisStatus { - std::uint32_t ack_sequence{0}; - std::uint32_t active_sequence{0}; - cmvr_plc::CommandState command_state{cmvr_plc::CommandState::Idle}; - cmvr_plc::ResultCode result_code{cmvr_plc::ResultCode::Ok}; - std::uint16_t axis_state{0}; - std::uint16_t current_mode{0}; - std::int32_t actual_position{0}; - std::int32_t actual_velocity{0}; - std::int32_t actual_torque{0}; - std::int32_t target_position{0}; - std::int32_t target_velocity{0}; - std::uint16_t status_flags{0}; - std::uint16_t drive_statusword{0}; - std::uint32_t fault_code{0}; - std::uint32_t zero_epoch{0}; - std::uint32_t last_applied_cyclic_sequence{0}; - std::uint32_t state_sequence{0}; - std::uint32_t plc_monotonic_time_ms{0}; - std::uint32_t heartbeat_age_ms{0}; - std::uint32_t ack_session_id{0}; -}; - -class ModbusTcpMotorBusRuntime final : public AbstractMotorBusRuntime { -public: - ModbusTcpMotorBusRuntime(); - ~ModbusTcpMotorBusRuntime() override; - - bool init(const config::MotorGroupConfig& group_cfg) override; - bool start() override; - void stop() override; - config::MotorBusType busType() const override { return config::MOTOR_BUS_MODBUS_TCP; } - - bool hasMotor(std::uint8_t motor_id) const; - bool axisForMotor(std::uint8_t motor_id, std::uint32_t& axis_index) const; - bool connected() const { return connected_.load(); } - std::uint32_t sessionId() const { return session_id_.load(); } - std::uint32_t plcBootId() const { return plc_boot_id_.load(); } - std::uint64_t connectionEpoch() const { return connection_epoch_.load(); } - std::uint32_t streamWatchdogMs() const { return stream_watchdog_ms_; } - std::uint32_t commandAckTimeoutMs() const { return command_ack_timeout_ms_; } - const std::string& id() const { return id_; } - std::string lastError() const; - - bool readAxisStatus(std::uint8_t motor_id, CmvrPlcAxisStatus& status); - bool submitAxisCommand(std::uint8_t motor_id, - const CmvrPlcAxisCommand& command, - bool wait_for_terminal_state = false, - std::optional - expected_connection_epoch = std::nullopt); - bool submitAxisSafetyCommand(std::uint8_t motor_id, - const CmvrPlcAxisCommand& command); - -private: - friend struct ModbusTcpMotorBusRuntimeTestAccess; - - bool connectAndHandshakeLocked_(); - void markDisconnectedLocked_(const std::string& error); - bool readRegistersLocked_(int address, int count, std::vector& values); - bool writeRegistersLocked_(int address, const std::uint16_t* values, int count); - bool writeHeartbeatLocked_(); - bool readAxisStatusByIndex_(std::uint32_t axis_index, CmvrPlcAxisStatus& status); - bool waitForCommand_(std::uint32_t axis_index, - std::uint32_t command_sequence, - std::uint32_t command_session_id, - std::uint64_t connection_epoch, - std::uint64_t cancel_generation, - bool cancel_on_safety_preemption, - const CmvrPlcAxisCommand& command, - bool wait_for_terminal_state, - const CmvrPlcAxisStatus& initial_status); - bool submitAxisCommandImpl_(std::uint8_t motor_id, - const CmvrPlcAxisCommand& command, - bool wait_for_terminal_state, - bool safety_priority, - std::optional - expected_connection_epoch); - bool writeClientHeartbeatLocked_(); - std::shared_ptr axisMutex_(std::uint8_t motor_id) const; - std::shared_ptr safetyMutex_(std::uint8_t motor_id) const; - void workerLoop_(); - static std::uint32_t randomNonZeroSessionId_(); - static std::uint32_t monotonicMilliseconds_(); - - std::string id_; - config::ModbusTcpConfig config_; - std::unordered_map motor_axes_; - mutable std::unordered_map> axis_mutexes_; - mutable std::unordered_map> safety_mutexes_; - std::unordered_map command_sequences_; - std::unordered_map cancel_generations_; - // Monotonic admission counter protected by state_mutex_. Besides being - // useful when diagnosing queueing, it gives concurrency tests an exact - // synchronization point after an invocation has captured its safety - // generation and connection epoch, but before it waits on a command - // serialization mutex. - std::uint64_t command_admission_count_{0}; - - mutable std::mutex io_mutex_; - mutable std::mutex state_mutex_; - mutable std::mutex lifecycle_mutex_; - ModbusTcpClient client_; - std::string last_error_; - std::thread worker_; - std::mutex worker_wait_mutex_; - std::condition_variable worker_wait_cv_; - std::atomic running_{false}; - std::atomic connected_{false}; - std::atomic plc_boot_id_{0}; - std::atomic connection_epoch_{0}; - std::atomic session_id_{0}; - std::uint32_t last_session_id_{0}; - std::uint32_t heartbeat_counter_{0}; - std::chrono::steady_clock::time_point last_client_heartbeat_write_at_{}; - std::uint32_t plc_heartbeat_counter_{0}; - std::chrono::steady_clock::time_point plc_heartbeat_changed_at_{}; - std::uint32_t connect_timeout_ms_{500}; - std::uint32_t io_timeout_ms_{100}; - std::uint32_t heartbeat_period_ms_{100}; - std::uint32_t communication_watchdog_ms_{500}; - std::uint32_t status_poll_period_ms_{20}; - std::uint32_t reconnect_min_ms_{100}; - std::uint32_t reconnect_max_ms_{2000}; - std::uint32_t command_ack_timeout_ms_{500}; - std::uint32_t stream_watchdog_ms_{500}; - std::uint16_t protocol_major_{cmvr_plc::kProtocolMajor}; - std::uint16_t protocol_minor_{cmvr_plc::kProtocolMinor}; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp deleted file mode 100644 index c2905f5e..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h" - -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace cmvr::device { - -ModbusTcpClient::~ModbusTcpClient() -{ - close(); -} - -bool ModbusTcpClient::open(const std::string& host, - const std::uint16_t port, - const std::uint8_t unit_id, - const std::uint32_t connect_timeout_ms, - const std::uint32_t response_timeout_ms) -{ - close(); - context_ = modbus_new_tcp(host.c_str(), static_cast(port)); - if (!context_) { - last_error_ = "modbus_new_tcp failed"; - return false; - } - if (modbus_set_slave(context_, static_cast(unit_id)) == -1) { - setLastErrnoError_("modbus_set_slave"); - close(); - return false; - } - - const auto timeout = response_timeout_ms == 0 ? 200U : response_timeout_ms; - if (modbus_set_response_timeout(context_, timeout / 1000U, - (timeout % 1000U) * 1000U) == -1 || - modbus_set_byte_timeout(context_, timeout / 1000U, - (timeout % 1000U) * 1000U) == -1) { - setLastErrnoError_("modbus_set_timeout"); - close(); - return false; - } - addrinfo hints{}; - hints.ai_family = AF_INET; - hints.ai_socktype = SOCK_STREAM; - hints.ai_flags = AI_NUMERICHOST; - addrinfo* addresses = nullptr; - const auto service = std::to_string(port); - const auto resolve_result = - getaddrinfo(host.c_str(), service.c_str(), &hints, &addresses); - if (resolve_result != 0) { - last_error_ = std::string("host must be a numeric IPv4 address: ") + - gai_strerror(resolve_result); - close(); - return false; - } - int connected_fd = -1; - for (auto* address = addresses; address; address = address->ai_next) { - const int fd = ::socket(address->ai_family, address->ai_socktype, address->ai_protocol); - if (fd < 0) { - continue; - } - const int old_flags = fcntl(fd, F_GETFL, 0); - if (old_flags < 0 || fcntl(fd, F_SETFL, old_flags | O_NONBLOCK) < 0) { - ::close(fd); - continue; - } - socket_fd_.store(fd); - const int rc = ::connect(fd, address->ai_addr, address->ai_addrlen); - if (rc == 0 || errno == EINPROGRESS) { - pollfd descriptor{fd, POLLOUT, 0}; - const auto timeout = static_cast( - connect_timeout_ms == 0 ? 1000U : connect_timeout_ms); - if (rc == 0 || poll(&descriptor, 1, timeout) > 0) { - int socket_error = 0; - socklen_t error_size = sizeof(socket_error); - if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &socket_error, &error_size) == 0 && - socket_error == 0) { - if (fcntl(fd, F_SETFL, old_flags) == 0) { - connected_fd = fd; - break; - } - } - } - } - socket_fd_.store(-1); - ::close(fd); - } - freeaddrinfo(addresses); - if (connected_fd < 0 || modbus_set_socket(context_, connected_fd) == -1) { - if (connected_fd >= 0) { - ::close(connected_fd); - } - last_error_ = "Modbus TCP connect timed out or failed"; - close(); - return false; - } - socket_fd_.store(connected_fd); - last_error_.clear(); - return true; -} - -void ModbusTcpClient::close() -{ - socket_fd_.store(-1); - if (context_) { - modbus_close(context_); - modbus_free(context_); - context_ = nullptr; - } -} - -void ModbusTcpClient::shutdown() -{ - const auto fd = socket_fd_.load(); - if (fd >= 0) { - ::shutdown(fd, SHUT_RDWR); - } -} - -bool ModbusTcpClient::readHoldingRegisters( - const int address, - const int count, - std::vector& values) -{ - if (!context_) { - last_error_ = "Modbus TCP connection is closed"; - return false; - } - if (address < 0 || count <= 0 || count > MODBUS_MAX_READ_REGISTERS) { - last_error_ = "invalid holding-register read range"; - return false; - } - - values.assign(static_cast(count), 0); - const auto rc = modbus_read_registers(context_, address, count, values.data()); - if (rc != count) { - setLastErrnoError_("modbus_read_registers"); - return false; - } - last_error_.clear(); - return true; -} - -bool ModbusTcpClient::writeHoldingRegisters( - const int address, - const std::uint16_t* values, - const int count) -{ - if (!context_) { - last_error_ = "Modbus TCP connection is closed"; - return false; - } - if (address < 0 || !values || count <= 0 || count > MODBUS_MAX_WRITE_REGISTERS) { - last_error_ = "invalid holding-register write range"; - return false; - } - - const auto rc = modbus_write_registers(context_, address, count, values); - if (rc != count) { - setLastErrnoError_("modbus_write_registers"); - return false; - } - last_error_.clear(); - return true; -} - -bool ModbusTcpClient::writeHoldingRegisters( - const int address, - const std::vector& values) -{ - if (values.size() > static_cast(std::numeric_limits::max())) { - last_error_ = "holding-register write is too large"; - return false; - } - return writeHoldingRegisters(address, values.data(), static_cast(values.size())); -} - -void ModbusTcpClient::setLastErrnoError_(const char* operation) -{ - last_error_ = std::string(operation) + ": " + modbus_strerror(errno); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp deleted file mode 100644 index 849a6358..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp +++ /dev/null @@ -1,1013 +0,0 @@ -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cmvr/msgs/motor.pb.h" -#include "common/base/logging/logger.h" - -namespace cmvr::device { -namespace { - -std::uint32_t valueOrDefault(const std::uint32_t value, const std::uint32_t fallback) -{ - return value == 0 ? fallback : value; -} - -bool validCommandState(const cmvr_plc::CommandState state) -{ - return static_cast(state) <= - static_cast(cmvr_plc::CommandState::CommunicationLost); -} - -} // namespace - -ModbusTcpMotorBusRuntime::ModbusTcpMotorBusRuntime() = default; - -ModbusTcpMotorBusRuntime::~ModbusTcpMotorBusRuntime() -{ - stop(); -} - -bool ModbusTcpMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) -{ - stop(); - id_ = group_cfg.id(); - if (id_.empty() || group_cfg.bus_type() != config::MOTOR_BUS_MODBUS_TCP || - !group_cfg.has_modbus_tcp()) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid group configuration"; - return false; - } - if (group_cfg.protocol() != config::MOTOR_PROTOCOL_CMVR_PLC_V1) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] unsupported protocol for group: " << id_; - return false; - } - - config_ = group_cfg.modbus_tcp(); - in_addr ipv4_address{}; - if (config_.host().empty() || - inet_pton(AF_INET, config_.host().c_str(), &ipv4_address) != 1 || - config_.port() > 65535U || config_.unit_id() > 247U) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid endpoint for group: " << id_; - return false; - } - if (config_.axes().empty()) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] no PLC axes configured for group: " << id_; - return false; - } - - motor_axes_.clear(); - axis_mutexes_.clear(); - safety_mutexes_.clear(); - command_sequences_.clear(); - cancel_generations_.clear(); - command_admission_count_ = 0; - std::unordered_map axes_to_motors; - for (const auto& axis : config_.axes()) { - if (axis.motor_id() < 0 || axis.motor_id() > 255) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] motor id is outside uint8 range"; - return false; - } - const auto motor_id = static_cast(axis.motor_id()); - const auto last_register = - static_cast(cmvr_plc::kAxisFirstOffset) + - static_cast(axis.axis_index()) * - static_cast(cmvr_plc::kAxisRegisterStride) + - static_cast(cmvr_plc::kAxisRegisterStride - 1); - if (last_register > 65535U || - last_register > static_cast(std::numeric_limits::max())) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] axis_index maps outside the " - "Modbus address space: " - << axis.axis_index(); - return false; - } - if (!motor_axes_.emplace(motor_id, axis.axis_index()).second || - !axes_to_motors.emplace(axis.axis_index(), motor_id).second) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] duplicate motor or axis mapping in " - << id_; - return false; - } - axis_mutexes_.emplace(motor_id, std::make_shared()); - safety_mutexes_.emplace(motor_id, std::make_shared()); - command_sequences_.emplace(motor_id, 0U); - cancel_generations_.emplace(motor_id, 0U); - } - - connect_timeout_ms_ = valueOrDefault(config_.connect_timeout_ms(), 500U); - io_timeout_ms_ = valueOrDefault(config_.io_timeout_ms(), 100U); - heartbeat_period_ms_ = valueOrDefault(config_.heartbeat_period_ms(), 100U); - communication_watchdog_ms_ = - valueOrDefault(config_.communication_watchdog_ms(), 500U); - status_poll_period_ms_ = valueOrDefault(config_.status_poll_period_ms(), 20U); - reconnect_min_ms_ = valueOrDefault(config_.reconnect_min_ms(), 100U); - reconnect_max_ms_ = valueOrDefault(config_.reconnect_max_ms(), 2000U); - command_ack_timeout_ms_ = valueOrDefault(config_.command_ack_timeout_ms(), 500U); - protocol_major_ = static_cast( - valueOrDefault(config_.protocol_major(), cmvr_plc::kProtocolMajor)); - protocol_minor_ = static_cast(config_.protocol_minor()); - stream_watchdog_ms_ = valueOrDefault(config_.cyclic_watchdog_ms(), 500U); - if (config_.protocol_major() > std::numeric_limits::max() || - config_.protocol_minor() > std::numeric_limits::max() || - connect_timeout_ms_ < 10U || connect_timeout_ms_ > 10000U || - io_timeout_ms_ < 10U || io_timeout_ms_ > 2000U || - heartbeat_period_ms_ < 10U || heartbeat_period_ms_ > 60000U || - communication_watchdog_ms_ > 120000U || - static_cast(communication_watchdog_ms_) < - static_cast(heartbeat_period_ms_) * 2U || - status_poll_period_ms_ < 1U || status_poll_period_ms_ > 1000U || - reconnect_min_ms_ < 10U || reconnect_min_ms_ > 60000U || - reconnect_max_ms_ > 120000U || - command_ack_timeout_ms_ < io_timeout_ms_ * 3U + status_poll_period_ms_ || - command_ack_timeout_ms_ > 60000U || - stream_watchdog_ms_ < 20U || stream_watchdog_ms_ > 60000U || - communication_watchdog_ms_ < - io_timeout_ms_ * 3U + heartbeat_period_ms_ * 2U || - stream_watchdog_ms_ < io_timeout_ms_ * 3U + status_poll_period_ms_ || - reconnect_max_ms_ < reconnect_min_ms_) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid watchdog/reconnect timing for " - << id_; - return false; - } - session_id_ = 0; - heartbeat_counter_ = 0; - return true; -} - -bool ModbusTcpMotorBusRuntime::start() -{ - std::lock_guard lifecycle_lock(lifecycle_mutex_); - if (running_.load()) { - return true; - } - if (id_.empty() || motor_axes_.empty()) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] runtime is not initialized"; - return false; - } - - running_.store(true); - { - std::lock_guard lock(io_mutex_); - if (!connectAndHandshakeLocked_()) { - CMVR_LOG(WARNING) - << "[ModbusTcpMotorBusRuntime] PLC is offline or incompatible at " - << "startup; supervisor will retry group=" << id_ - << ", error=" << lastError(); - } - } - if (running_.load()) { - worker_ = std::thread(&ModbusTcpMotorBusRuntime::workerLoop_, this); - } - return true; -} - -void ModbusTcpMotorBusRuntime::stop() -{ - running_.store(false); - connected_.store(false); - session_id_.store(0); - connection_epoch_.fetch_add(1); - { - std::lock_guard lock(state_mutex_); - for (auto& [motor, generation] : cancel_generations_) { - (void)motor; - ++generation; - } - } - worker_wait_cv_.notify_all(); - client_.shutdown(); - std::lock_guard lifecycle_lock(lifecycle_mutex_); - const bool restarted_during_stop = running_.exchange(false); - connected_.store(false); - session_id_.store(0); - if (restarted_during_stop) { - connection_epoch_.fetch_add(1); - std::lock_guard state_lock(state_mutex_); - for (auto& [motor, generation] : cancel_generations_) { - (void)motor; - ++generation; - } - } - worker_wait_cv_.notify_all(); - client_.shutdown(); - if (worker_.joinable()) { - worker_.join(); - } - std::lock_guard lock(io_mutex_); - client_.close(); - connected_.store(false); - session_id_.store(0); -} - -bool ModbusTcpMotorBusRuntime::hasMotor(const std::uint8_t motor_id) const -{ - return motor_axes_.count(motor_id) != 0; -} - -bool ModbusTcpMotorBusRuntime::axisForMotor( - const std::uint8_t motor_id, - std::uint32_t& axis_index) const -{ - const auto it = motor_axes_.find(motor_id); - if (it == motor_axes_.end()) { - return false; - } - axis_index = it->second; - return true; -} - -std::string ModbusTcpMotorBusRuntime::lastError() const -{ - std::lock_guard lock(state_mutex_); - return last_error_; -} - -bool ModbusTcpMotorBusRuntime::readAxisStatus( - const std::uint8_t motor_id, - CmvrPlcAxisStatus& status) -{ - std::uint32_t axis_index = 0; - if (!axisForMotor(motor_id, axis_index)) { - std::lock_guard lock(state_mutex_); - last_error_ = "motor id has no PLC axis mapping"; - return false; - } - return readAxisStatusByIndex_(axis_index, status); -} - -bool ModbusTcpMotorBusRuntime::submitAxisCommand( - const std::uint8_t motor_id, - const CmvrPlcAxisCommand& command, - const bool wait_for_terminal_state, - const std::optional expected_connection_epoch) -{ - return submitAxisCommandImpl_( - motor_id, command, wait_for_terminal_state, false, - expected_connection_epoch); -} - -bool ModbusTcpMotorBusRuntime::submitAxisSafetyCommand( - const std::uint8_t motor_id, - const CmvrPlcAxisCommand& command) -{ - return submitAxisCommandImpl_( - motor_id, command, true, true, std::nullopt); -} - -bool ModbusTcpMotorBusRuntime::submitAxisCommandImpl_( - const std::uint8_t motor_id, - const CmvrPlcAxisCommand& command, - const bool wait_for_terminal_state, - const bool safety_priority, - const std::optional expected_connection_epoch) -{ - // Snapshot the connection generation at invocation entry. In particular, - // do not let a caller that arrived while the runtime was offline, or one - // that later waits behind a queue/handshake, silently attach itself to a - // newer PLC ownership session. - const auto invocation_connection_epoch = - connection_epoch_.load(std::memory_order_acquire); - if (!running_.load(std::memory_order_acquire) || - !connected_.load(std::memory_order_acquire) || - connection_epoch_.load(std::memory_order_acquire) != - invocation_connection_epoch || - (expected_connection_epoch.has_value() && - invocation_connection_epoch != *expected_connection_epoch)) { - std::lock_guard lock(state_mutex_); - last_error_ = expected_connection_epoch.has_value() && - invocation_connection_epoch != - *expected_connection_epoch - ? "command expected a different connection epoch" - : "Modbus TCP connection is unavailable at command entry"; - return false; - } - - std::uint32_t axis_index = 0; - auto axis_mutex = axisMutex_(motor_id); - auto safety_mutex = safetyMutex_(motor_id); - if (!axis_mutex || !safety_mutex || !axisForMotor(motor_id, axis_index)) { - std::lock_guard lock(state_mutex_); - last_error_ = "motor id has no PLC axis mapping"; - return false; - } - std::unique_lock axis_lock; - std::unique_lock safety_lock; - std::uint64_t cancel_generation = 0; - { - std::lock_guard lock(state_mutex_); - if (safety_priority) { - ++cancel_generations_[motor_id]; - } - cancel_generation = cancel_generations_[motor_id]; - ++command_admission_count_; - } - if (safety_priority) { - // Cancel ordinary motion immediately, then serialize only with other - // safety commands. A later safety command must not cancel this one's - // result while it is awaiting its ACK. - safety_lock = std::unique_lock(*safety_mutex); - } else { - // Capture above before waiting for the per-axis queue. A safety - // command that arrives while this normal command is queued must make - // the queued command stale rather than becoming its new baseline. - axis_lock = std::unique_lock(*axis_mutex); - std::lock_guard lock(state_mutex_); - if (cancel_generations_[motor_id] != cancel_generation) { - last_error_ = - "PLC command was preempted while waiting for the axis queue"; - return false; - } - } - - if (!running_.load(std::memory_order_acquire) || - !connected_.load(std::memory_order_acquire) || - connection_epoch_.load(std::memory_order_acquire) != - invocation_connection_epoch || - (expected_connection_epoch.has_value() && - invocation_connection_epoch != *expected_connection_epoch)) { - std::lock_guard lock(state_mutex_); - last_error_ = "connection epoch changed while waiting for command queue"; - return false; - } - - CmvrPlcAxisStatus initial_status; - // Only SetZero needs a fresh pre-command zero_epoch. Other commands avoid - // a three-FC3 baseline read; QuickStop/Disable therefore spend the first - // available transaction on the safety mailbox itself. - if (!safety_priority && - command.code == cmvr_plc::CommandCode::SetZero && - !readAxisStatusByIndex_(axis_index, initial_status)) { - return false; - } - - std::uint32_t command_sequence = 0; - std::uint32_t command_session_id = 0; - std::uint64_t command_connection_epoch = 0; - std::array payload{}; - payload[cmvr_plc::kCommandCode] = static_cast(command.code); - payload[cmvr_plc::kCommandFlags] = command.flags; - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetPosition, command.target_position); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetVelocity, command.target_velocity); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kAcceleration, command.acceleration); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetTorque, command.target_torque); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kPositionTolerance, - command.position_tolerance); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kVelocityTolerance, - command.velocity_tolerance); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCommandTimeout, - command.command_timeout_ms); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kStreamWatchdog, - command.stream_watchdog_ms); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCyclicSampleSequence, - command.cyclic_sample_sequence); - cmvr_plc::encodeUint32( - payload.data(), cmvr_plc::kClientMonotonicTime, - command.client_monotonic_time_ms == 0 ? monotonicMilliseconds_() - : command.client_monotonic_time_ms); - const auto expected_zero_epoch = - command.code == cmvr_plc::CommandCode::SetZero - ? initial_status.zero_epoch - : command.expected_zero_epoch; - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kExpectedZeroEpoch, - expected_zero_epoch); - payload[cmvr_plc::kDisconnectAction] = command.disconnect_action; - - const auto base = cmvr_plc::axisBase(axis_index); - { - std::lock_guard lock(io_mutex_); - if (!running_.load(std::memory_order_acquire) || - !connected_.load(std::memory_order_acquire) || - connection_epoch_.load(std::memory_order_acquire) != - invocation_connection_epoch || - (expected_connection_epoch.has_value() && - invocation_connection_epoch != *expected_connection_epoch)) { - std::lock_guard state_lock(state_mutex_); - last_error_ = - "Modbus TCP connection changed before mailbox commit"; - return false; - } - { - std::lock_guard state_lock(state_mutex_); - if (!safety_priority && - cancel_generations_[motor_id] != cancel_generation) { - last_error_ = "PLC command was preempted before mailbox commit"; - return false; - } - command_sequence = ++command_sequences_[motor_id]; - if (command_sequence == 0) { - command_sequence = ++command_sequences_[motor_id]; - } - } - command_session_id = session_id_.load(); - command_connection_epoch = invocation_connection_epoch; - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kPayloadSequence, - command_sequence); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kPayloadSequenceMirror, - command_sequence); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCommandSessionId, - command_session_id); - const auto heartbeat_due = - std::chrono::steady_clock::now() - - last_client_heartbeat_write_at_ >= - std::chrono::milliseconds(heartbeat_period_ms_); - if (!safety_priority && heartbeat_due && - !writeClientHeartbeatLocked_()) { - return false; - } - if (!writeRegistersLocked_(base, payload.data(), static_cast(payload.size()))) { - return false; - } - std::array commit{}; - cmvr_plc::encodeUint32(commit.data(), 0, command_sequence); - if (!writeRegistersLocked_(base + cmvr_plc::kCommitSequenceRelativeOffset, - commit.data(), static_cast(commit.size()))) { - return false; - } - } - return waitForCommand_(axis_index, command_sequence, command_session_id, - command_connection_epoch, cancel_generation, - !safety_priority, command, - wait_for_terminal_state, initial_status); -} - -bool ModbusTcpMotorBusRuntime::connectAndHandshakeLocked_() -{ - client_.close(); - connected_.store(false); - const auto port = static_cast( - config_.port() == 0 ? 502U : config_.port()); - const auto unit_id = static_cast( - config_.unit_id() == 0 ? 1U : config_.unit_id()); - if (!client_.open(config_.host(), port, unit_id, - connect_timeout_ms_, io_timeout_ms_)) { - std::lock_guard state_lock(state_mutex_); - last_error_ = client_.lastError(); - return false; - } - - std::vector global; - if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { - return false; - } - const auto initial_protocol_major = - global[cmvr_plc::kProtocolMajorOffset]; - const auto initial_protocol_minor = - global[cmvr_plc::kProtocolMinorOffset]; - const auto initial_axis_count = global[cmvr_plc::kAxisCountOffset]; - const auto initial_boot_id = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kPlcBootIdOffset); - if (global[cmvr_plc::kMagicCmOffset] != cmvr_plc::kMagicCm || - global[cmvr_plc::kMagicVrOffset] != cmvr_plc::kMagicVr) { - markDisconnectedLocked_("PLC register map magic does not match CMVR"); - return false; - } - if (initial_protocol_major != protocol_major_) { - markDisconnectedLocked_("PLC protocol major version mismatch"); - return false; - } - if (initial_protocol_minor < protocol_minor_) { - markDisconnectedLocked_("PLC protocol minor version is older than required"); - return false; - } - if (initial_boot_id == 0U) { - markDisconnectedLocked_("PLC boot_id must be non-zero"); - return false; - } - for (const auto& [motor_id, axis_index] : motor_axes_) { - (void)motor_id; - if (axis_index >= initial_axis_count) { - markDisconnectedLocked_("configured PLC axis is outside PLC axis_count"); - return false; - } - } - const auto validate_handshake_identity = - [&](const std::vector& snapshot) { - if (snapshot[cmvr_plc::kMagicCmOffset] != cmvr_plc::kMagicCm || - snapshot[cmvr_plc::kMagicVrOffset] != cmvr_plc::kMagicVr || - snapshot[cmvr_plc::kProtocolMajorOffset] != initial_protocol_major || - snapshot[cmvr_plc::kProtocolMinorOffset] != initial_protocol_minor || - snapshot[cmvr_plc::kAxisCountOffset] != initial_axis_count) { - markDisconnectedLocked_( - "PLC register-map identity changed during session handshake"); - return false; - } - const auto boot_id = cmvr_plc::decodeUint32( - snapshot.data(), cmvr_plc::kPlcBootIdOffset); - if (boot_id == 0U || boot_id != initial_boot_id) { - markDisconnectedLocked_( - "PLC boot_id changed or became zero during session handshake"); - return false; - } - return true; - }; - - std::uint32_t next_session_id = 0; - do { - next_session_id = randomNonZeroSessionId_(); - } while (next_session_id == last_session_id_); - // A reconnect must not immediately reuse its preceding command namespace. - // This does not claim global lifetime uniqueness, only adjacent-session - // separation, which is what stale mailbox/ACK rejection requires. - session_id_ = next_session_id; - last_session_id_ = next_session_id; - heartbeat_counter_ = 0; - { - std::lock_guard state_lock(state_mutex_); - for (auto& [motor_id, sequence] : command_sequences_) { - (void)motor_id; - sequence = 0; - } - } - - std::array watchdog{}; - cmvr_plc::encodeUint32(watchdog.data(), 0, communication_watchdog_ms_); - if (!writeRegistersLocked_(cmvr_plc::kCommunicationWatchdogOffset, - watchdog.data(), static_cast(watchdog.size()))) { - return false; - } - // Publish the new owner candidate only after all parameters it depends on - // are visible to the PLC. - std::array session_and_heartbeat{}; - cmvr_plc::encodeUint32(session_and_heartbeat.data(), 0, session_id_.load()); - cmvr_plc::encodeUint32(session_and_heartbeat.data(), 2, ++heartbeat_counter_); - if (!writeRegistersLocked_(cmvr_plc::kCmvrSessionIdOffset, - session_and_heartbeat.data(), - static_cast(session_and_heartbeat.size()))) { - return false; - } - last_client_heartbeat_write_at_ = std::chrono::steady_clock::now(); - - const auto handshake_deadline = std::chrono::steady_clock::now() + - std::chrono::milliseconds(command_ack_timeout_ms_); - do { - if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { - return false; - } - if (!validate_handshake_identity(global)) { - return false; - } - const auto owner_session = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kOwnerSessionIdOffset); - const auto owner_state = - static_cast(global[cmvr_plc::kOwnerStateOffset]); - if (owner_session == session_id_.load() && - owner_state == cmvr_plc::OwnerState::Accepted) { - break; - } - if (owner_session == session_id_.load() && - owner_state == cmvr_plc::OwnerState::Rejected) { - markDisconnectedLocked_("PLC rejected CMVR session ownership"); - return false; - } - std::this_thread::sleep_for(std::chrono::milliseconds(status_poll_period_ms_)); - } while (std::chrono::steady_clock::now() < handshake_deadline); - if (cmvr_plc::decodeUint32(global.data(), cmvr_plc::kOwnerSessionIdOffset) != - session_id_.load() || - static_cast(global[cmvr_plc::kOwnerStateOffset]) != - cmvr_plc::OwnerState::Accepted) { - markDisconnectedLocked_("PLC session ownership handshake timed out"); - return false; - } - - // Confirm one complete immutable snapshot after owner acceptance. This - // closes the race where the PLC restarts or swaps register-map versions - // between the accepted-session observation and connected_=true. - if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global) || - !validate_handshake_identity(global)) { - return false; - } - if (cmvr_plc::decodeUint32(global.data(), - cmvr_plc::kOwnerSessionIdOffset) != - session_id_.load() || - static_cast( - global[cmvr_plc::kOwnerStateOffset]) != - cmvr_plc::OwnerState::Accepted) { - markDisconnectedLocked_( - "PLC session ownership changed during final handshake confirmation"); - return false; - } - - if (!running_.load()) { - markDisconnectedLocked_( - "runtime stopped during PLC session handshake"); - return false; - } - plc_boot_id_.store(initial_boot_id); - plc_heartbeat_counter_ = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kPlcHeartbeatOffset); - plc_heartbeat_changed_at_ = std::chrono::steady_clock::now(); - connected_.store(true); - connection_epoch_.fetch_add(1); - { - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - } - CMVR_LOG(INFO) << "[ModbusTcpMotorBusRuntime] connected group=" << id_ - << ", endpoint=" << config_.host() << ":" << port - << ", boot_id=" << plc_boot_id_.load() - << ", protocol=" << global[cmvr_plc::kProtocolMajorOffset] - << "." << global[cmvr_plc::kProtocolMinorOffset]; - return true; -} - -void ModbusTcpMotorBusRuntime::markDisconnectedLocked_(const std::string& error) -{ - connected_.store(false); - client_.close(); - std::lock_guard state_lock(state_mutex_); - last_error_ = error; -} - -bool ModbusTcpMotorBusRuntime::readRegistersLocked_( - const int address, - const int count, - std::vector& values) -{ - if (!client_.readHoldingRegisters(address, count, values)) { - markDisconnectedLocked_(client_.lastError()); - return false; - } - return true; -} - -bool ModbusTcpMotorBusRuntime::writeRegistersLocked_( - const int address, - const std::uint16_t* values, - const int count) -{ - if (!client_.writeHoldingRegisters(address, values, count)) { - markDisconnectedLocked_(client_.lastError()); - return false; - } - return true; -} - -bool ModbusTcpMotorBusRuntime::writeClientHeartbeatLocked_() -{ - std::array values{}; - cmvr_plc::encodeUint32(values.data(), 0, session_id_.load()); - cmvr_plc::encodeUint32(values.data(), 2, ++heartbeat_counter_); - if (!writeRegistersLocked_(cmvr_plc::kCmvrSessionIdOffset, - values.data(), static_cast(values.size()))) { - return false; - } - last_client_heartbeat_write_at_ = std::chrono::steady_clock::now(); - return true; -} - -bool ModbusTcpMotorBusRuntime::writeHeartbeatLocked_() -{ - if (!writeClientHeartbeatLocked_()) { - return false; - } - std::vector global; - if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { - return false; - } - const auto boot_id = - cmvr_plc::decodeUint32(global.data(), cmvr_plc::kPlcBootIdOffset); - if (boot_id != plc_boot_id_.load()) { - markDisconnectedLocked_("PLC boot_id changed"); - return false; - } - const auto owner_session = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kOwnerSessionIdOffset); - if (owner_session != session_id_.load() || - static_cast(global[cmvr_plc::kOwnerStateOffset]) != - cmvr_plc::OwnerState::Accepted) { - markDisconnectedLocked_("PLC session ownership was lost"); - return false; - } - const auto plc_heartbeat = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kPlcHeartbeatOffset); - const auto now = std::chrono::steady_clock::now(); - if (plc_heartbeat != plc_heartbeat_counter_) { - plc_heartbeat_counter_ = plc_heartbeat; - plc_heartbeat_changed_at_ = now; - } else if (now - plc_heartbeat_changed_at_ >= - std::chrono::milliseconds(communication_watchdog_ms_)) { - markDisconnectedLocked_("PLC heartbeat stopped advancing"); - return false; - } - return true; -} - -bool ModbusTcpMotorBusRuntime::readAxisStatusByIndex_( - const std::uint32_t axis_index, - CmvrPlcAxisStatus& status) -{ - std::vector values; - { - std::lock_guard lock(io_mutex_); - if (!connected_.load()) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "Modbus TCP connection is unavailable"; - return false; - } - const auto status_base = cmvr_plc::axisBase(axis_index) + - cmvr_plc::kAxisStatusRelativeOffset; - bool stable_snapshot = false; - for (int attempt = 0; attempt < 3; ++attempt) { - std::vector before; - std::vector candidate; - std::vector after; - if (!readRegistersLocked_( - status_base + static_cast(cmvr_plc::kStateSequence), - 2, before) || - !readRegistersLocked_( - status_base, cmvr_plc::kAxisStatusRegisterCount, - candidate) || - !readRegistersLocked_( - status_base + static_cast(cmvr_plc::kStateSequence), - 2, after)) { - return false; - } - const auto before_sequence = - cmvr_plc::decodeUint32(before.data(), 0); - const auto after_sequence = - cmvr_plc::decodeUint32(after.data(), 0); - const auto block_sequence = cmvr_plc::decodeUint32( - candidate.data(), cmvr_plc::kStateSequence); - const auto block_mirror = cmvr_plc::decodeUint32( - candidate.data(), cmvr_plc::kStateSequenceMirror); - if (before_sequence != 0U && - (before_sequence & 1U) == 0U && - before_sequence == after_sequence && - before_sequence == block_sequence && - before_sequence == block_mirror) { - values = std::move(candidate); - stable_snapshot = true; - break; - } - } - if (!stable_snapshot) { - std::lock_guard state_lock(state_mutex_); - last_error_ = - "PLC axis status changed during bounded seqlock read"; - return false; - } - } - - status.ack_sequence = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kAckSequence); - status.active_sequence = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kActiveSequence); - status.command_state = - static_cast(values[cmvr_plc::kCommandState]); - status.result_code = - static_cast(values[cmvr_plc::kResultCode]); - status.axis_state = values[cmvr_plc::kAxisState]; - status.current_mode = values[cmvr_plc::kCurrentMode]; - status.actual_position = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualPosition); - status.actual_velocity = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualVelocity); - status.actual_torque = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualTorque); - status.target_position = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kTargetPositionStatus); - status.target_velocity = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kTargetVelocityStatus); - status.status_flags = values[cmvr_plc::kStatusFlags]; - status.drive_statusword = values[cmvr_plc::kDriveStatusword]; - status.fault_code = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kFaultCode); - status.zero_epoch = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kZeroEpoch); - status.last_applied_cyclic_sequence = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kLastAppliedCyclicSequence); - status.state_sequence = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kStateSequence); - status.plc_monotonic_time_ms = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kPlcMonotonicTime); - status.heartbeat_age_ms = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kHeartbeatAge); - status.ack_session_id = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kAckSessionId); - if (status.heartbeat_age_ms > communication_watchdog_ms_) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "PLC axis status is stale"; - return false; - } - return true; -} - -bool ModbusTcpMotorBusRuntime::waitForCommand_( - const std::uint32_t axis_index, - const std::uint32_t command_sequence, - const std::uint32_t command_session_id, - const std::uint64_t connection_epoch, - const std::uint64_t cancel_generation, - const bool cancel_on_safety_preemption, - const CmvrPlcAxisCommand& command, - const bool wait_for_terminal_state, - const CmvrPlcAxisStatus& initial_status) -{ - const auto timeout_ms = wait_for_terminal_state - ? valueOrDefault(command.command_timeout_ms, command_ack_timeout_ms_) - : command_ack_timeout_ms_; - const auto deadline = std::chrono::steady_clock::now() + - std::chrono::milliseconds(timeout_ms); - do { - if (cancel_on_safety_preemption) { - std::lock_guard state_lock(state_mutex_); - for (const auto& [motor_id, axis] : motor_axes_) { - if (axis == axis_index && - cancel_generations_[motor_id] != cancel_generation) { - last_error_ = "PLC command was preempted by a safety command"; - return false; - } - } - } - if (connection_epoch_.load() != connection_epoch) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "connection session changed while awaiting PLC command"; - return false; - } - CmvrPlcAxisStatus status; - if (!readAxisStatusByIndex_(axis_index, status)) { - return false; - } - if (std::chrono::steady_clock::now() >= deadline) { - break; - } - if (status.ack_session_id == command_session_id && - status.ack_sequence == command_sequence) { - if (!validCommandState(status.command_state)) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "PLC returned an unknown command state"; - return false; - } - if (cmvr_plc::isFailure(status.command_state) || - status.result_code != cmvr_plc::ResultCode::Ok) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "PLC rejected or failed command, result=" + - std::to_string(static_cast(status.result_code)); - return false; - } - if (!wait_for_terminal_state) { - const bool accepted_state = - status.command_state == cmvr_plc::CommandState::Accepted || - status.command_state == cmvr_plc::CommandState::Running || - status.command_state == cmvr_plc::CommandState::TargetReached || - status.command_state == cmvr_plc::CommandState::Completed; - if (!accepted_state) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "PLC ACK did not report an accepted command state"; - return false; - } - if (command.code == cmvr_plc::CommandCode::CyclicPositionSample || - command.code == cmvr_plc::CommandCode::CyclicVelocitySample) { - if (status.last_applied_cyclic_sequence == - command.cyclic_sample_sequence) { - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - return true; - } - } else if ( - command.code == cmvr_plc::CommandCode::OpenCyclicPosition || - command.code == cmvr_plc::CommandCode::OpenCyclicVelocity) { - const auto expected_mode = - command.code == - cmvr_plc::CommandCode::OpenCyclicPosition - ? msgs::RUN_MODE_CYCLIC_SYNC_POSITION - : msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; - if (status.last_applied_cyclic_sequence != 0U || - (status.status_flags & - cmvr_plc::StatusFlag::StreamActive) == 0U || - status.current_mode != - static_cast(expected_mode)) { - std::lock_guard state_lock(state_mutex_); - last_error_ = - "PLC OpenCyclic ACK did not establish a fresh active stream"; - return false; - } - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - return true; - } else { - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - return true; - } - } else { - bool completed = false; - switch (command.code) { - case cmvr_plc::CommandCode::SetZero: - completed = - status.command_state == cmvr_plc::CommandState::Completed && - (status.status_flags & cmvr_plc::StatusFlag::ZeroValid) != 0U && - status.zero_epoch != initial_status.zero_epoch; - break; - case cmvr_plc::CommandCode::Enable: - completed = - status.command_state == cmvr_plc::CommandState::Completed && - (status.status_flags & cmvr_plc::StatusFlag::Enabled) != 0U; - break; - case cmvr_plc::CommandCode::Disable: - completed = - status.command_state == cmvr_plc::CommandState::Completed && - (status.status_flags & cmvr_plc::StatusFlag::Enabled) == 0U; - break; - case cmvr_plc::CommandCode::QuickStop: - completed = - (status.command_state == cmvr_plc::CommandState::QuickStopped || - status.command_state == cmvr_plc::CommandState::Completed) && - std::abs(static_cast(status.actual_velocity)) <= 10000; - break; - default: - completed = - status.command_state == cmvr_plc::CommandState::Completed || - status.command_state == cmvr_plc::CommandState::TargetReached; - break; - } - if (completed) { - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - return true; - } - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(status_poll_period_ms_)); - } while (std::chrono::steady_clock::now() < deadline && connected_.load()); - - std::lock_guard state_lock(state_mutex_); - last_error_ = wait_for_terminal_state ? "PLC command completion timeout" - : "PLC command acknowledgement timeout"; - return false; -} - -std::shared_ptr ModbusTcpMotorBusRuntime::axisMutex_( - const std::uint8_t motor_id) const -{ - const auto it = axis_mutexes_.find(motor_id); - return it == axis_mutexes_.end() ? nullptr : it->second; -} - -std::shared_ptr ModbusTcpMotorBusRuntime::safetyMutex_( - const std::uint8_t motor_id) const -{ - const auto it = safety_mutexes_.find(motor_id); - return it == safety_mutexes_.end() ? nullptr : it->second; -} - -void ModbusTcpMotorBusRuntime::workerLoop_() -{ - auto reconnect_delay = reconnect_min_ms_; - const auto interrupted = [this](const std::uint32_t wait_ms) { - std::unique_lock lock(worker_wait_mutex_); - return worker_wait_cv_.wait_for( - lock, std::chrono::milliseconds(wait_ms), - [this] { return !running_.load(); }); - }; - while (running_.load()) { - if (connected_.load()) { - if (interrupted(heartbeat_period_ms_)) { - break; - } - { - std::lock_guard lock(io_mutex_); - if (connected_.load()) { - writeHeartbeatLocked_(); - } - } - continue; - } - - if (interrupted(reconnect_delay)) { - break; - } - if (!running_.load()) { - break; - } - { - std::lock_guard lock(io_mutex_); - if (connectAndHandshakeLocked_()) { - reconnect_delay = reconnect_min_ms_; - continue; - } - } - reconnect_delay = std::min(reconnect_max_ms_, reconnect_delay * 2U); - } -} - -std::uint32_t ModbusTcpMotorBusRuntime::randomNonZeroSessionId_() -{ - std::random_device random_device; - std::mt19937 generator(random_device()); - std::uniform_int_distribution distribution( - 1U, std::numeric_limits::max()); - return distribution(generator); -} - -std::uint32_t ModbusTcpMotorBusRuntime::monotonicMilliseconds_() -{ - return static_cast( - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()) - .count()); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp deleted file mode 100644 index a18fb2c9..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp +++ /dev/null @@ -1,1586 +0,0 @@ -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" -#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" - -namespace cmvr::device { - -struct ModbusTcpMotorBusRuntimeTestAccess { - static std::shared_ptr axisMutex( - const ModbusTcpMotorBusRuntime& runtime, - const std::uint8_t motor_id) - { - return runtime.axisMutex_(motor_id); - } - - static std::shared_ptr safetyMutex( - const ModbusTcpMotorBusRuntime& runtime, - const std::uint8_t motor_id) - { - return runtime.safetyMutex_(motor_id); - } - - static std::uint64_t commandAdmissionCount( - const ModbusTcpMotorBusRuntime& runtime) - { - std::lock_guard lock(runtime.state_mutex_); - return runtime.command_admission_count_; - } -}; - -namespace { - -using namespace std::chrono_literals; - -std::uint16_t reserveLoopbackPort() -{ - const int socket_fd = ::socket(AF_INET, SOCK_STREAM, 0); - if (socket_fd < 0) { - return 0; - } - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); - address.sin_port = 0; - if (::bind(socket_fd, reinterpret_cast(&address), sizeof(address)) != 0) { - ::close(socket_fd); - return 0; - } - socklen_t length = sizeof(address); - if (::getsockname(socket_fd, reinterpret_cast(&address), &length) != 0) { - ::close(socket_fd); - return 0; - } - const auto port = ntohs(address.sin_port); - ::close(socket_fd); - return port; -} - -class FakeCmvrPlc { -public: - enum class HandshakeMutation { - None, - BootId, - ProtocolMajor, - }; - - ~FakeCmvrPlc() { stop(); } - - bool start(const std::uint16_t requested_port = 0) - { - port_ = requested_port == 0 ? reserveLoopbackPort() : requested_port; - if (port_ == 0) { - std::fprintf(stderr, "reserveLoopbackPort failed: %s\n", std::strerror(errno)); - return false; - } - context_ = modbus_new_tcp("127.0.0.1", port_); - mapping_ = modbus_mapping_new(1, 1, - cmvr_plc::kAxisFirstOffset + - cmvr_plc::kAxisRegisterStride, - 1); - if (!context_ || !mapping_) { - std::fprintf(stderr, "fake PLC allocation failed: %s\n", modbus_strerror(errno)); - stop(); - return false; - } - auto* registers = mapping_->tab_registers; - registers[cmvr_plc::kMagicCmOffset] = cmvr_plc::kMagicCm; - registers[cmvr_plc::kMagicVrOffset] = cmvr_plc::kMagicVr; - registers[cmvr_plc::kProtocolMajorOffset] = cmvr_plc::kProtocolMajor; - registers[cmvr_plc::kProtocolMinorOffset] = cmvr_plc::kProtocolMinor; - registers[cmvr_plc::kAxisCountOffset] = 1; - registers[cmvr_plc::kOwnerStateOffset] = - static_cast( - preserve_stale_rejected_owner_ - ? cmvr_plc::OwnerState::Rejected - : cmvr_plc::OwnerState::None); - if (preserve_stale_rejected_owner_) { - cmvr_plc::encodeUint32( - registers, cmvr_plc::kOwnerSessionIdOffset, 0x55667788U); - } - cmvr_plc::encodeUint32(registers, cmvr_plc::kPlcBootIdOffset, - initial_boot_id_); - auto* status = registers + cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset; - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, - state_sequence_); - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, - state_sequence_); - registers[cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset + - cmvr_plc::kCommandState] = - static_cast(cmvr_plc::CommandState::Idle); - registers[cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset + - cmvr_plc::kResultCode] = - static_cast(cmvr_plc::ResultCode::Ok); - - listen_socket_.store(modbus_tcp_listen(context_, 2)); - if (listen_socket_.load() < 0) { - std::fprintf(stderr, "modbus_tcp_listen failed on %u: %s\n", - port_, modbus_strerror(errno)); - stop(); - return false; - } - running_.store(true); - worker_ = std::thread(&FakeCmvrPlc::loop_, this); - return true; - } - - void stop() - { - running_.store(false); - const auto client = client_socket_.exchange(-1); - if (client >= 0) { - ::shutdown(client, SHUT_RDWR); - ::close(client); - } - const auto listener = listen_socket_.exchange(-1); - if (listener >= 0) { - ::shutdown(listener, SHUT_RDWR); - ::close(listener); - } - if (worker_.joinable()) { - worker_.join(); - } - if (mapping_) { - modbus_mapping_free(mapping_); - mapping_ = nullptr; - } - if (context_) { - modbus_free(context_); - context_ = nullptr; - } - } - - void disconnectClient() - { - const auto client = client_socket_.load(); - if (client >= 0) { - ::shutdown(client, SHUT_RDWR); - } - } - - std::uint16_t port() const { return port_; } - std::uint32_t commandCount() const { return command_count_.load(); } - std::uint32_t lastExpectedZeroEpoch() const - { - return last_expected_zero_epoch_.load(); - } - std::uint32_t lastProfileCommandTimeoutMs() const - { - return last_profile_command_timeout_ms_.load(); - } - std::uint32_t cyclicOpenCount() const - { - return cyclic_open_count_.load(); - } - std::uint32_t cyclicApplyCount() const - { - return cyclic_apply_count_.load(); - } - std::uint32_t currentStreamFirstSampleSequence() const - { - return current_stream_first_sample_sequence_.load(); - } - std::uint32_t clientHeartbeatWriteCount() const - { - return client_heartbeat_write_count_.load(); - } - std::uint32_t fullStatusReadCount() const - { - return full_status_read_count_.load(); - } - std::uint32_t observedProfileMailboxCount() const - { - return observed_profile_mailbox_count_.load(); - } - std::uint32_t observedMailboxCommitCount() const - { - return observed_mailbox_commit_count_.load(); - } - void setInitialBootId(const std::uint32_t boot_id) - { - initial_boot_id_ = boot_id; - } - void setHandshakeMutation(const HandshakeMutation mutation) - { - handshake_mutation_ = mutation; - } - void preserveStaleRejectedOwnerDuringHandshake(const bool preserve) - { - preserve_stale_rejected_owner_ = preserve; - } - void setHandshakeAcceptanceDelay(const std::uint32_t delay_ms) - { - handshake_acceptance_delay_ms_ = delay_ms; - } - void holdStatusSnapshotInProgress(const bool hold) - { - hold_status_snapshot_in_progress_.store(hold); - } - void publishOddEqualStatusSnapshot(const bool publish) - { - publish_odd_equal_status_snapshot_.store(publish); - } - void setQuickStopProcessingDelay(const std::uint32_t delay_ms) - { - quick_stop_processing_delay_ms_.store(delay_ms); - } - void injectMixedStablePositionOnce(const std::int32_t mixed_position, - const std::int32_t corrected_position) - { - std::lock_guard lock(mapping_mutex_); - auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset; - // Deliberately emulate a non-atomic FC3 copy whose markers still show - // the old stable version while one field comes from the next scan. - cmvr_plc::encodeInt32(status, cmvr_plc::kActualPosition, - mixed_position); - corrected_mixed_position_.store(corrected_position); - correct_mixed_snapshot_after_next_read_.store(true); - } - void publishPosition(const std::int32_t position) - { - std::lock_guard lock(mapping_mutex_); - auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset; - beginSnapshotUpdate_(status); - cmvr_plc::encodeInt32(status, cmvr_plc::kActualPosition, position); - publishSnapshot_(status); - } - void publishFatalFlags() - { - std::lock_guard lock(mapping_mutex_); - auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset; - beginSnapshotUpdate_(status); - status[cmvr_plc::kStatusFlags] |= - cmvr_plc::StatusFlag::Fault | - cmvr_plc::StatusFlag::CommunicationWatchdogExpired | - cmvr_plc::StatusFlag::CyclicWatchdogExpired; - publishSnapshot_(status); - } - void delayProfileAcks(const bool delay) { delay_profile_acks_.store(delay); } - -private: - void loop_() - { - std::array request{}; - while (running_.load()) { - int listener = listen_socket_.load(); - const int accepted = - listener < 0 ? -1 : modbus_tcp_accept(context_, &listener); - if (accepted < 0) { - if (running_.load()) { - std::this_thread::sleep_for(5ms); - } - continue; - } - client_socket_.store(accepted); - while (running_.load()) { - const auto request_length = modbus_receive(context_, request.data()); - if (request_length <= 0) { - break; - } - { - std::lock_guard mapping_lock(mapping_mutex_); - if (modbus_reply(context_, request.data(), request_length, - mapping_) < 0) { - break; - } - processMailbox_(request.data(), request_length); - } - } - const auto client = client_socket_.exchange(-1); - if (client >= 0) { - ::close(client); - } - } - } - - void processMailbox_(const std::uint8_t* request, - const int request_length) - { - auto* registers = mapping_->tab_registers; - const auto base = cmvr_plc::axisBase(0); - const auto status_base = base + cmvr_plc::kAxisStatusRelativeOffset; - int request_address = -1; - int request_count = 0; - const auto function = - request && request_length >= 8 ? request[7] : 0U; - if (request && request_length >= 12 && - (function == 0x03U || function == 0x10U)) { - request_address = - (static_cast(request[8]) << 8) | - static_cast(request[9]); - request_count = - (static_cast(request[10]) << 8) | - static_cast(request[11]); - } - if (function == 0x10U && - request_address == cmvr_plc::kCmvrSessionIdOffset && - request_count == 4) { - client_heartbeat_write_count_.fetch_add(1); - } - const bool status_read = - function == 0x03U && - request_address >= status_base && - request_address < status_base + - cmvr_plc::kAxisStatusRegisterCount; - const bool full_status_read = - function == 0x03U && - request_address == status_base && - request_count == cmvr_plc::kAxisStatusRegisterCount; - if (full_status_read) { - full_status_read_count_.fetch_add(1); - } - cmvr_plc::encodeUint32( - registers, cmvr_plc::kPlcHeartbeatOffset, ++plc_heartbeat_); - const auto session = - cmvr_plc::decodeUint32(registers, cmvr_plc::kCmvrSessionIdOffset); - const auto now = std::chrono::steady_clock::now(); - if (session != observed_session_) { - observed_session_ = session; - session_observed_at_ = now; - if (!handshake_identity_mutated_) { - if (handshake_mutation_ == HandshakeMutation::BootId) { - cmvr_plc::encodeUint32( - registers, cmvr_plc::kPlcBootIdOffset, - initial_boot_id_ + 1U); - } else if (handshake_mutation_ == - HandshakeMutation::ProtocolMajor) { - registers[cmvr_plc::kProtocolMajorOffset] = - cmvr_plc::kProtocolMajor + 1U; - } - handshake_identity_mutated_ = - handshake_mutation_ != HandshakeMutation::None; - } - if (!preserve_stale_rejected_owner_) { - registers[cmvr_plc::kOwnerStateOffset] = - static_cast( - cmvr_plc::OwnerState::Accepting); - } - last_sequence_ = 0; - last_observed_mailbox_sequence_ = 0; - last_observed_profile_sequence_ = 0; - cmvr_plc::encodeUint32(registers + base, - cmvr_plc::kPayloadSequence, 0); - cmvr_plc::encodeUint32(registers + base, - cmvr_plc::kPayloadSequenceMirror, 0); - cmvr_plc::encodeUint32(registers + base, - cmvr_plc::kCommitSequenceRelativeOffset, 0); - beginSnapshotUpdate_(registers + status_base); - cyclic_stream_open_ = false; - last_cyclic_sample_sequence_ = 0; - current_stream_first_sample_sequence_.store(0); - registers[status_base + cmvr_plc::kStatusFlags] &= - static_cast( - ~cmvr_plc::StatusFlag::StreamActive); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, 0); - publishSnapshot_(registers + status_base); - } - if (active_session_ != observed_session_ && - now - session_observed_at_ >= - std::chrono::milliseconds(handshake_acceptance_delay_ms_)) { - active_session_ = observed_session_; - cmvr_plc::encodeUint32( - registers, cmvr_plc::kOwnerSessionIdOffset, active_session_); - registers[cmvr_plc::kOwnerStateOffset] = - static_cast(cmvr_plc::OwnerState::Accepted); - } - if (active_session_ == 0 || active_session_ != session) { - processStatusInjection_(registers + status_base, status_read, - full_status_read); - return; - } - const auto sequence = - cmvr_plc::decodeUint32(registers + base, cmvr_plc::kPayloadSequence); - const auto mirror = - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kPayloadSequenceMirror); - const auto commit = - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kCommitSequenceRelativeOffset); - if (sequence == 0 || sequence == last_sequence_ || - sequence != mirror || sequence != commit || - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kCommandSessionId) != active_session_) { - processStatusInjection_(registers + status_base, status_read, - full_status_read); - return; - } - const auto code = static_cast( - registers[base + cmvr_plc::kCommandCode]); - if (sequence != last_observed_mailbox_sequence_) { - last_observed_mailbox_sequence_ = sequence; - observed_mailbox_commit_count_.fetch_add(1); - } - const bool profile_command = - code == cmvr_plc::CommandCode::ProfilePosition || - code == cmvr_plc::CommandCode::ProfileVelocity; - if (profile_command) { - if (sequence != last_observed_profile_sequence_) { - last_observed_profile_sequence_ = sequence; - observed_profile_mailbox_count_.fetch_add(1); - } - last_profile_command_timeout_ms_.store( - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kCommandTimeout)); - } - if (delay_profile_acks_.load() && profile_command) { - // Leave the last published status snapshot unchanged. Rewriting - // its seqlock marker on every FC3 poll would make the fake report - // a torn snapshot and release the caller's axis mutex instead of - // deterministically keeping the command pending. - return; - } - beginSnapshotUpdate_(registers + status_base); - if (code == cmvr_plc::CommandCode::QuickStop && - quick_stop_processing_delay_ms_.load() > 0U) { - std::this_thread::sleep_for(std::chrono::milliseconds( - quick_stop_processing_delay_ms_.load())); - } - const bool cyclic_sample = - code == cmvr_plc::CommandCode::CyclicPositionSample || - code == cmvr_plc::CommandCode::CyclicVelocitySample; - const auto cyclic_sample_sequence = cmvr_plc::decodeUint32( - registers + base, cmvr_plc::kCyclicSampleSequence); - if (cyclic_sample && - (!cyclic_stream_open_ || cyclic_sample_sequence == 0U || - cyclic_sample_sequence == last_cyclic_sample_sequence_)) { - last_sequence_ = sequence; - cmvr_plc::encodeUint32( - registers + status_base, cmvr_plc::kAckSequence, sequence); - cmvr_plc::encodeUint32( - registers + status_base, cmvr_plc::kAckSessionId, - active_session_); - registers[status_base + cmvr_plc::kCommandState] = - static_cast( - cmvr_plc::CommandState::Rejected); - registers[status_base + cmvr_plc::kResultCode] = - static_cast( - cmvr_plc::ResultCode::SequenceError); - publishSnapshot_(registers + status_base); - return; - } - if (code == cmvr_plc::CommandCode::SetZero) { - last_expected_zero_epoch_.store( - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kExpectedZeroEpoch)); - } - if (code == cmvr_plc::CommandCode::SetZero && - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kExpectedZeroEpoch) != - cmvr_plc::decodeUint32(registers + status_base, - cmvr_plc::kZeroEpoch)) { - last_sequence_ = sequence; - cmvr_plc::encodeUint32(registers + status_base, - cmvr_plc::kAckSequence, sequence); - cmvr_plc::encodeUint32(registers + status_base, - cmvr_plc::kAckSessionId, active_session_); - registers[status_base + cmvr_plc::kCommandState] = - static_cast(cmvr_plc::CommandState::Rejected); - registers[status_base + cmvr_plc::kResultCode] = - static_cast(cmvr_plc::ResultCode::SequenceError); - publishSnapshot_(registers + status_base); - return; - } - last_sequence_ = sequence; - command_count_.fetch_add(1); - cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kAckSequence, - sequence); - cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kActiveSequence, - sequence); - cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kAckSessionId, - active_session_); - registers[status_base + cmvr_plc::kResultCode] = - static_cast(cmvr_plc::ResultCode::Ok); - - const auto target_position = - cmvr_plc::decodeInt32(registers + base, cmvr_plc::kTargetPosition); - const auto target_velocity = - cmvr_plc::decodeInt32(registers + base, cmvr_plc::kTargetVelocity); - auto& flags = registers[status_base + cmvr_plc::kStatusFlags]; - auto state = cmvr_plc::CommandState::Completed; - switch (code) { - case cmvr_plc::CommandCode::SetZero: - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualPosition, 0); - flags |= cmvr_plc::StatusFlag::ZeroValid; - cmvr_plc::encodeUint32( - registers + status_base, cmvr_plc::kZeroEpoch, - cmvr_plc::decodeUint32(registers + status_base, - cmvr_plc::kZeroEpoch) + - 1U); - break; - case cmvr_plc::CommandCode::ProfilePosition: - registers[status_base + cmvr_plc::kCurrentMode] = - msgs::RUN_MODE_PROFILE_POSITION; - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualPosition, target_position); - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kTargetPositionStatus, target_position); - flags |= cmvr_plc::StatusFlag::TargetReached; - state = cmvr_plc::CommandState::TargetReached; - break; - case cmvr_plc::CommandCode::ProfileVelocity: - registers[status_base + cmvr_plc::kCurrentMode] = - msgs::RUN_MODE_PROFILE_VELOCITY; - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualVelocity, target_velocity); - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kTargetVelocityStatus, target_velocity); - flags |= cmvr_plc::StatusFlag::TargetReached; - state = cmvr_plc::CommandState::TargetReached; - break; - case cmvr_plc::CommandCode::OpenCyclicPosition: - registers[status_base + cmvr_plc::kCurrentMode] = - msgs::RUN_MODE_CYCLIC_SYNC_POSITION; - flags |= cmvr_plc::StatusFlag::StreamActive; - cyclic_stream_open_ = true; - last_cyclic_sample_sequence_ = 0; - current_stream_first_sample_sequence_.store(0); - cyclic_open_count_.fetch_add(1); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, 0); - state = cmvr_plc::CommandState::Accepted; - break; - case cmvr_plc::CommandCode::OpenCyclicVelocity: - registers[status_base + cmvr_plc::kCurrentMode] = - msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; - flags |= cmvr_plc::StatusFlag::StreamActive; - cyclic_stream_open_ = true; - last_cyclic_sample_sequence_ = 0; - current_stream_first_sample_sequence_.store(0); - cyclic_open_count_.fetch_add(1); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, 0); - state = cmvr_plc::CommandState::Accepted; - break; - case cmvr_plc::CommandCode::CyclicPositionSample: - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualPosition, target_position); - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualVelocity, target_velocity); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, - cyclic_sample_sequence); - last_cyclic_sample_sequence_ = cyclic_sample_sequence; - if (current_stream_first_sample_sequence_.load() == 0U) { - current_stream_first_sample_sequence_.store( - cyclic_sample_sequence); - } - cyclic_apply_count_.fetch_add(1); - state = cmvr_plc::CommandState::Accepted; - break; - case cmvr_plc::CommandCode::CyclicVelocitySample: - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualVelocity, target_velocity); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, - cyclic_sample_sequence); - last_cyclic_sample_sequence_ = cyclic_sample_sequence; - if (current_stream_first_sample_sequence_.load() == 0U) { - current_stream_first_sample_sequence_.store( - cyclic_sample_sequence); - } - cyclic_apply_count_.fetch_add(1); - state = cmvr_plc::CommandState::Accepted; - break; - case cmvr_plc::CommandCode::QuickStop: - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualVelocity, 0); - flags &= static_cast(~cmvr_plc::StatusFlag::StreamActive); - cyclic_stream_open_ = false; - state = cmvr_plc::CommandState::QuickStopped; - break; - case cmvr_plc::CommandCode::Enable: - flags |= cmvr_plc::StatusFlag::Enabled; - break; - case cmvr_plc::CommandCode::Disable: - flags &= static_cast(~cmvr_plc::StatusFlag::Enabled); - flags &= static_cast(~cmvr_plc::StatusFlag::StreamActive); - cyclic_stream_open_ = false; - break; - default: - break; - } - registers[status_base + cmvr_plc::kCommandState] = - static_cast(state); - publishSnapshot_(registers + status_base); - } - - void processStatusInjection_(std::uint16_t* status, - const bool status_read, - const bool full_status_read) - { - if (full_status_read && - correct_mixed_snapshot_after_next_read_.exchange(false)) { - beginSnapshotUpdate_(status); - cmvr_plc::encodeInt32( - status, cmvr_plc::kActualPosition, - corrected_mixed_position_.load()); - publishSnapshot_(status); - return; - } - if (status_read && - (hold_status_snapshot_in_progress_.load() || - publish_odd_equal_status_snapshot_.load())) { - beginSnapshotUpdate_(status); - publishSnapshot_(status); - } - } - - void publishSnapshot_(std::uint16_t* status) - { - cmvr_plc::encodeUint32(status, cmvr_plc::kHeartbeatAge, 0); - const auto in_progress_sequence = state_sequence_ + 1U; - if (publish_odd_equal_status_snapshot_.load()) { - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, - in_progress_sequence); - return; - } - if (hold_status_snapshot_in_progress_.load()) { - return; - } - auto next_stable_sequence = state_sequence_ + 2U; - if (next_stable_sequence == 0U) { - next_stable_sequence = 2U; - } - // Seqlock publication order: mirror first, sequence last. - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, - next_stable_sequence); - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, - next_stable_sequence); - state_sequence_ = next_stable_sequence; - } - - void beginSnapshotUpdate_(std::uint16_t* status) - { - // Mark the snapshot in progress before changing any status field. - // The mirror intentionally remains at the previous stable even value. - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, - state_sequence_ + 1U); - } - - modbus_t* context_{nullptr}; - modbus_mapping_t* mapping_{nullptr}; - std::mutex mapping_mutex_; - std::thread worker_; - std::atomic running_{false}; - std::atomic listen_socket_{-1}; - std::atomic client_socket_{-1}; - std::atomic command_count_{0}; - std::atomic last_expected_zero_epoch_{0}; - std::atomic last_profile_command_timeout_ms_{0}; - std::atomic cyclic_open_count_{0}; - std::atomic cyclic_apply_count_{0}; - std::atomic current_stream_first_sample_sequence_{0}; - std::atomic client_heartbeat_write_count_{0}; - std::atomic full_status_read_count_{0}; - std::atomic observed_profile_mailbox_count_{0}; - std::atomic observed_mailbox_commit_count_{0}; - std::atomic quick_stop_processing_delay_ms_{0}; - std::atomic corrected_mixed_position_{0}; - std::atomic correct_mixed_snapshot_after_next_read_{false}; - std::atomic delay_profile_acks_{false}; - std::atomic hold_status_snapshot_in_progress_{false}; - std::atomic publish_odd_equal_status_snapshot_{false}; - std::uint32_t active_session_{0}; - std::uint32_t observed_session_{0}; - std::uint32_t plc_heartbeat_{0}; - std::uint32_t state_sequence_{2}; - std::chrono::steady_clock::time_point session_observed_at_{}; - std::uint32_t last_sequence_{0}; - std::uint32_t last_observed_mailbox_sequence_{0}; - std::uint32_t last_observed_profile_sequence_{0}; - std::uint32_t initial_boot_id_{0x10203040U}; - HandshakeMutation handshake_mutation_{HandshakeMutation::None}; - bool handshake_identity_mutated_{false}; - bool preserve_stale_rejected_owner_{false}; - std::uint32_t handshake_acceptance_delay_ms_{20}; - bool cyclic_stream_open_{false}; - std::uint32_t last_cyclic_sample_sequence_{0}; - std::uint16_t port_{0}; -}; - -config::MotorGroupConfig makeConfig(const std::uint16_t port) -{ - config::MotorGroupConfig group; - group.set_id("test_plc"); - group.set_bus_type(config::MOTOR_BUS_MODBUS_TCP); - group.set_vendor(config::MOTOR_VENDOR_PLC_GENERIC); - group.set_protocol(config::MOTOR_PROTOCOL_CMVR_PLC_V1); - auto* modbus = group.mutable_modbus_tcp(); - modbus->set_host("127.0.0.1"); - modbus->set_port(port); - modbus->set_unit_id(1); - modbus->set_connect_timeout_ms(200); - modbus->set_io_timeout_ms(100); - modbus->set_heartbeat_period_ms(20); - modbus->set_communication_watchdog_ms(500); - modbus->set_status_poll_period_ms(2); - modbus->set_reconnect_min_ms(10); - modbus->set_reconnect_max_ms(50); - modbus->set_command_ack_timeout_ms(500); - modbus->set_cyclic_watchdog_ms(500); - auto* axis = modbus->add_axes(); - axis->set_motor_id(1); - axis->set_axis_index(0); - return group; -} - -template -bool waitUntil(Predicate&& predicate, const std::chrono::milliseconds timeout) -{ - const auto deadline = std::chrono::steady_clock::now() + timeout; - do { - if (predicate()) { - return true; - } - std::this_thread::sleep_for(5ms); - } while (std::chrono::steady_clock::now() < deadline); - return predicate(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ExecutesSupportedCmvrPlcV1Commands) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()) << runtime->lastError(); - ASSERT_NE(runtime->sessionId(), 0U); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - protocol.setLimitQ(1, 2.0, -2.0); - protocol.setLimitQd(1, 2.0); - protocol.setLimitQdd(1, 5.0, -5.0); - - EXPECT_TRUE(protocol.torqueOn(1)); - EXPECT_TRUE(protocol.commandProfilePosition(1, 1.25, 0.5, 1.0)); - EXPECT_EQ(plc.lastProfileCommandTimeoutMs(), 600000U); - EXPECT_TRUE(protocol.reachedTargetQ(1)); - EXPECT_NEAR(protocol.getQ(1), 1.25, 1e-6); - EXPECT_TRUE(protocol.commandProfileVelocity(1, -0.4, 1.0)); - EXPECT_EQ(plc.lastProfileCommandTimeoutMs(), 600000U); - EXPECT_NEAR(protocol.getQd(1), -0.4, 1e-6); - EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.75, 0.1)); - EXPECT_EQ(plc.cyclicOpenCount(), 1U); - EXPECT_EQ(plc.cyclicApplyCount(), 1U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - EXPECT_NEAR(protocol.getQ(1), 0.75, 1e-6); - EXPECT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.5, 0.1)); - EXPECT_EQ(plc.cyclicOpenCount(), 2U); - EXPECT_EQ(plc.cyclicApplyCount(), 2U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - EXPECT_NEAR(protocol.getQ(1), 0.5, 1e-6); - EXPECT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(protocol.commandCyclicVelocity(1, -0.2)); - EXPECT_EQ(plc.cyclicOpenCount(), 3U); - EXPECT_EQ(plc.cyclicApplyCount(), 3U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - CmvrPlcAxisStatus cyclic_velocity_status; - ASSERT_TRUE(runtime->readAxisStatus(1, cyclic_velocity_status)); - EXPECT_EQ(cyclic_velocity_status.current_mode, - msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); - EXPECT_NE(cyclic_velocity_status.status_flags & - cmvr_plc::StatusFlag::StreamActive, - 0U); - EXPECT_EQ(cyclic_velocity_status.last_applied_cyclic_sequence, 1U); - EXPECT_EQ(cyclic_velocity_status.actual_velocity, -200000); - EXPECT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(protocol.calibrateZeroQ(1)); - EXPECT_NEAR(protocol.getQ(1), 0.0, 1e-6); - EXPECT_TRUE(protocol.calibrateZeroQ(1)); - EXPECT_EQ(plc.lastExpectedZeroEpoch(), 1U); - EXPECT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(protocol.torqueOff(1)); - EXPECT_FALSE(protocol.commandCyclicTorque(1, 0.1)); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, AvoidsRedundantBaselineAndHeartbeatWrites) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto group = makeConfig(plc.port()); - group.mutable_modbus_tcp()->set_heartbeat_period_ms(1000); - group.mutable_modbus_tcp()->set_communication_watchdog_ms(3000); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(group)); - ASSERT_TRUE(runtime->start()); - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - protocol.setLimitQ(1, 2.0, -2.0); - protocol.setLimitQd(1, 2.0); - protocol.setLimitQdd(1, 5.0, -5.0); - - const auto heartbeat_writes = plc.clientHeartbeatWriteCount(); - const auto reads_before_profile = plc.fullStatusReadCount(); - ASSERT_TRUE(protocol.commandProfilePosition(1, 0.5, 0.5, 1.0)); - EXPECT_EQ(plc.fullStatusReadCount() - reads_before_profile, 1U); - EXPECT_EQ(plc.clientHeartbeatWriteCount(), heartbeat_writes); - - const auto reads_before_zero = plc.fullStatusReadCount(); - ASSERT_TRUE(protocol.calibrateZeroQ(1)); - EXPECT_EQ(plc.fullStatusReadCount() - reads_before_zero, 2U); - EXPECT_EQ(plc.clientHeartbeatWriteCount(), heartbeat_writes); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ReconnectCreatesNewSessionWithoutReplayingCommands) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.2, 0.1)); - const auto old_session = runtime->sessionId(); - const auto old_epoch = runtime->connectionEpoch(); - const auto command_count = plc.commandCount(); - plc.disconnectClient(); - - ASSERT_TRUE(waitUntil( - [&] { return runtime->connected() && runtime->connectionEpoch() > old_epoch; }, - 1500ms)) << runtime->lastError(); - EXPECT_NE(runtime->sessionId(), old_session); - std::this_thread::sleep_for(100ms); - EXPECT_EQ(plc.commandCount(), command_count); - ASSERT_TRUE(protocol.commandProfileVelocity(1, -0.1, 0.1)); - EXPECT_EQ(plc.commandCount(), command_count + 1U); - const auto session_before_stop = runtime->sessionId(); - runtime->stop(); - EXPECT_EQ(runtime->sessionId(), 0U); - ASSERT_TRUE(runtime->start()) << runtime->lastError(); - EXPECT_NE(runtime->sessionId(), session_before_stop); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, - ActiveCyclicGenerationRequiresExplicitRestartAfterReconnect) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - ASSERT_TRUE(protocol.commandCyclicPosition(1, 0.1, 0.0)); - ASSERT_EQ(plc.cyclicOpenCount(), 1U); - ASSERT_EQ(plc.cyclicApplyCount(), 1U); - - const auto old_epoch = runtime->connectionEpoch(); - plc.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms)) << runtime->lastError(); - - const auto commands_after_reconnect = plc.commandCount(); - const auto opens_after_reconnect = plc.cyclicOpenCount(); - const auto samples_after_reconnect = plc.cyclicApplyCount(); - // The first post-reconnect sample latches this upper-layer generation. - // Further calls from the same generation remain fail-closed and neither - // Open nor sample is committed into the new PLC session. - EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.2, 0.0)); - EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.3, 0.0)); - EXPECT_EQ(plc.commandCount(), commands_after_reconnect); - EXPECT_EQ(plc.cyclicOpenCount(), opens_after_reconnect); - EXPECT_EQ(plc.cyclicApplyCount(), samples_after_reconnect); - - // A new gRPC stream calls setMode even when its mode is unchanged. That - // explicit boundary establishes a new generation and clears the latch. - protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.4, 0.0)); - EXPECT_EQ(plc.cyclicOpenCount(), opens_after_reconnect + 1U); - EXPECT_EQ(plc.cyclicApplyCount(), samples_after_reconnect + 1U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, - CyclicOpenQueuedAcrossReconnectCannotEnterNewSession) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - - auto axis_mutex = - ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); - ASSERT_NE(axis_mutex, nullptr); - std::unique_lock queue_hold(*axis_mutex); - const auto old_epoch = runtime->connectionEpoch(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - const auto opens_before = plc.cyclicOpenCount(); - const auto samples_before = plc.cyclicApplyCount(); - const auto admissions_before = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - - std::atomic result{true}; - std::thread queued([&] { - result.store(protocol.commandCyclicPosition(1, 0.1, 0.0)); - }); - const bool admitted = waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before; - }, - 500ms); - - // The protocol already bound OpenCyclicPosition to old_epoch, but the - // runtime invocation is still queued. Reconnect before releasing the - // queue and prove that expected_connection_epoch prevents both Open and - // its sample from entering the new PLC ownership session. - plc.disconnectClient(); - const bool reconnected = waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms); - queue_hold.unlock(); - queued.join(); - - EXPECT_TRUE(admitted); - EXPECT_TRUE(reconnected) << runtime->lastError(); - EXPECT_FALSE(result.load()); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - EXPECT_EQ(plc.cyclicOpenCount(), opens_before); - EXPECT_EQ(plc.cyclicApplyCount(), samples_before); - - // Failure while crossing epochs permanently latches this generation. - EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.2, 0.0)); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - EXPECT_EQ(plc.cyclicOpenCount(), opens_before); - EXPECT_EQ(plc.cyclicApplyCount(), samples_before); - - // Only the explicit boundary of a new upper-layer stream may recover. - protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.3, 0.0)); - EXPECT_EQ(plc.cyclicOpenCount(), opens_before + 1U); - EXPECT_EQ(plc.cyclicApplyCount(), samples_before + 1U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ProfileFeedbackIsBoundToConnectionEpoch) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - // Zero is deliberately chosen: the new PLC session's safe stopped - // velocity is also zero, so value equality cannot prove command identity. - ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.0, 0.1)); - ASSERT_TRUE(std::isfinite(protocol.getQd(1))); - ASSERT_TRUE(protocol.reachedTargetQ(1)); - - auto old_epoch = runtime->connectionEpoch(); - plc.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms)) << runtime->lastError(); - - EXPECT_TRUE(std::isnan(protocol.getQ(1))); - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - EXPECT_FALSE(protocol.reachedTargetQ(1)); - - // A successfully ACKed profile in the current session rebinds feedback. - ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.0, 0.1)); - EXPECT_TRUE(std::isfinite(protocol.getQ(1))); - EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); - - old_epoch = runtime->connectionEpoch(); - plc.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms)) << runtime->lastError(); - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - - // Successful explicit safety recovery also clears the stale binding. - ASSERT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(std::isfinite(protocol.getQ(1))); - EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, SupervisorConnectsWhenPlcComesOnlineAfterStart) -{ - const auto port = reserveLoopbackPort(); - ASSERT_NE(port, 0U); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(port))); - EXPECT_TRUE(runtime->start()); - EXPECT_FALSE(runtime->connected()); - CmvrPlcAxisStatus status; - EXPECT_FALSE(runtime->readAxisStatus(1, status)); - - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start(port)); - ASSERT_TRUE(waitUntil([&] { return runtime->connected(); }, 1500ms)) - << runtime->lastError(); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, StopDuringHandshakeLeavesRuntimeDisconnected) -{ - FakeCmvrPlc plc; - plc.setHandshakeAcceptanceDelay(500); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - std::atomic start_result{false}; - std::thread starter([&] { start_result.store(runtime.start()); }); - std::this_thread::sleep_for(50ms); - runtime.stop(); - starter.join(); - EXPECT_TRUE(start_result.load()); - EXPECT_FALSE(runtime.connected()); - EXPECT_EQ(runtime.sessionId(), 0U); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ConcurrentStartStopCannotResurrectWorker) -{ - const auto port = reserveLoopbackPort(); - ASSERT_NE(port, 0U); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(port))); - for (int iteration = 0; iteration < 20; ++iteration) { - std::atomic ready{0}; - auto synchronize = [&] { - ready.fetch_add(1); - while (ready.load() < 2) { - std::this_thread::yield(); - } - }; - std::thread starter([&] { - synchronize(); - runtime.start(); - }); - std::thread stopper([&] { - synchronize(); - runtime.stop(); - }); - starter.join(); - stopper.join(); - // If start linearized after stop, this final stop owns that later - // lifecycle. It must always terminate and leave no resurrected worker. - runtime.stop(); - EXPECT_FALSE(runtime.connected()); - EXPECT_EQ(runtime.sessionId(), 0U); - } -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsZeroPlcBootId) -{ - FakeCmvrPlc plc; - plc.setInitialBootId(0); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - EXPECT_TRUE(runtime.start()); - EXPECT_FALSE(runtime.connected()); - EXPECT_NE(runtime.lastError().find("boot_id must be non-zero"), - std::string::npos); - runtime.stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsBootIdChangeDuringHandshake) -{ - FakeCmvrPlc plc; - plc.setHandshakeMutation(FakeCmvrPlc::HandshakeMutation::BootId); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - EXPECT_TRUE(runtime.start()); - EXPECT_FALSE(runtime.connected()); - EXPECT_NE(runtime.lastError().find("boot_id changed"), - std::string::npos); - runtime.stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsProtocolVersionChangeDuringHandshake) -{ - FakeCmvrPlc plc; - plc.setHandshakeMutation( - FakeCmvrPlc::HandshakeMutation::ProtocolMajor); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - EXPECT_TRUE(runtime.start()); - EXPECT_FALSE(runtime.connected()); - EXPECT_NE(runtime.lastError().find("identity changed"), - std::string::npos); - runtime.stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, IgnoresRejectedDecisionForStaleSession) -{ - FakeCmvrPlc plc; - plc.preserveStaleRejectedOwnerDuringHandshake(true); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - EXPECT_TRUE(runtime.start()) << runtime.lastError(); - EXPECT_TRUE(runtime.connected()); - runtime.stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsInProgressAndOddStatusSnapshots) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcAxisStatus status; - plc.holdStatusSnapshotInProgress(true); - EXPECT_FALSE(runtime->readAxisStatus(1, status)); - EXPECT_NE(runtime->lastError().find("bounded seqlock"), - std::string::npos); - - plc.holdStatusSnapshotInProgress(false); - plc.publishOddEqualStatusSnapshot(true); - EXPECT_FALSE(runtime->readAxisStatus(1, status)); - EXPECT_NE(runtime->lastError().find("bounded seqlock"), - std::string::npos); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RetriesTornReadAndAcceptsAdvancingStableVersions) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcAxisStatus status; - for (std::int32_t position = 100; position <= 300; position += 100) { - plc.publishPosition(position); - ASSERT_TRUE(runtime->readAxisStatus(1, status)); - EXPECT_EQ(status.actual_position, position); - } - - plc.injectMixedStablePositionOnce(999999, 424242); - ASSERT_TRUE(runtime->readAxisStatus(1, status)) << runtime->lastError(); - EXPECT_EQ(status.actual_position, 424242); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, QuickStopPreemptsPendingNormalCommand) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - plc.delayProfileAcks(true); - - CmvrPlcAxisCommand normal; - normal.code = cmvr_plc::CommandCode::ProfileVelocity; - normal.target_velocity = 100000; - normal.command_timeout_ms = 5000; - std::atomic normal_result{true}; - std::thread pending([&] { - normal_result.store(runtime->submitAxisCommand(1, normal, false)); - }); - std::this_thread::sleep_for(50ms); - - CmvrPlcAxisCommand stop; - stop.code = cmvr_plc::CommandCode::QuickStop; - stop.command_timeout_ms = 1000; - const auto started = std::chrono::steady_clock::now(); - EXPECT_TRUE(runtime->submitAxisSafetyCommand(1, stop)); - const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - started); - pending.join(); - EXPECT_FALSE(normal_result.load()); - EXPECT_LT(elapsed.count(), 500); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, QueuedNormalCommandKeepsPreSafetyGeneration) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - plc.delayProfileAcks(true); - - CmvrPlcAxisCommand normal_a; - normal_a.code = cmvr_plc::CommandCode::ProfileVelocity; - normal_a.target_velocity = 100000; - normal_a.command_timeout_ms = 5000; - std::atomic result_a{true}; - std::thread first([&] { - result_a.store(runtime->submitAxisCommand(1, normal_a, false)); - }); - EXPECT_TRUE(waitUntil( - [&] { return plc.observedProfileMailboxCount() == 1U; }, 500ms)); - - CmvrPlcAxisCommand normal_b = normal_a; - normal_b.target_velocity = 200000; - std::atomic result_b{true}; - const auto admissions_before_b = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - std::thread second([&] { - result_b.store(runtime->submitAxisCommand(1, normal_b, false)); - }); - // A is still holding axis_mutex. This counter advances in the exact - // critical section where B snapshots the pre-safety generation, before B - // starts waiting for the per-axis queue. - EXPECT_TRUE(waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before_b; - }, - 500ms)); - - CmvrPlcAxisCommand stop; - stop.code = cmvr_plc::CommandCode::QuickStop; - stop.command_timeout_ms = 1000; - EXPECT_TRUE(runtime->submitAxisSafetyCommand(1, stop)); - first.join(); - second.join(); - - EXPECT_FALSE(result_a.load()); - EXPECT_FALSE(result_b.load()); - EXPECT_EQ(plc.observedProfileMailboxCount(), 1U); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, QueuedNormalCommandCannotCrossReconnectEpoch) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - auto axis_mutex = - ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); - ASSERT_NE(axis_mutex, nullptr); - std::unique_lock queue_hold(*axis_mutex); - const auto old_epoch = runtime->connectionEpoch(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - const auto admissions_before = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::ProfileVelocity; - command.target_velocity = 200000; - command.command_timeout_ms = 1000; - std::atomic result{true}; - std::thread queued([&] { - result.store(runtime->submitAxisCommand(1, command, false)); - }); - const bool admitted = waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before; - }, - 500ms); - - // Keep the old invocation behind axis_mutex while the worker detects the - // broken socket and completes a new ownership handshake. - plc.disconnectClient(); - const bool reconnected = waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms); - queue_hold.unlock(); - queued.join(); - - EXPECT_TRUE(admitted); - EXPECT_TRUE(reconnected) << runtime->lastError(); - EXPECT_FALSE(result.load()); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, - ProfileSubmitQueuedAcrossReconnectInvalidatesFeedback) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - - auto axis_mutex = - ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); - ASSERT_NE(axis_mutex, nullptr); - std::unique_lock queue_hold(*axis_mutex); - const auto old_epoch = runtime->connectionEpoch(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - const auto admissions_before = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - - std::atomic result{true}; - std::thread queued([&] { - result.store(protocol.commandProfileVelocity(1, 0.0, 0.1)); - }); - const bool admitted = waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before; - }, - 500ms); - - plc.disconnectClient(); - const bool reconnected = waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms); - queue_hold.unlock(); - queued.join(); - - EXPECT_TRUE(admitted); - EXPECT_TRUE(reconnected) << runtime->lastError(); - EXPECT_FALSE(result.load()); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - // Even though the safe S2 process image reports qdot=0, the ambiguous S1 - // invocation cannot claim that value as successful Profile feedback. - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - ASSERT_TRUE(protocol.quickStop(1)); - EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, - RejectsCallerExpectedOldEpochWithoutMailboxCommit) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - const auto old_epoch = runtime->connectionEpoch(); - plc.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms)) << runtime->lastError(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::ProfileVelocity; - command.target_velocity = 100000; - command.command_timeout_ms = 1000; - EXPECT_FALSE(runtime->submitAxisCommand( - 1, command, false, old_epoch)); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, QueuedSafetyCommandCannotCrossReconnectEpoch) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - auto safety_mutex = - ModbusTcpMotorBusRuntimeTestAccess::safetyMutex(*runtime, 1); - ASSERT_NE(safety_mutex, nullptr); - std::unique_lock queue_hold(*safety_mutex); - const auto old_epoch = runtime->connectionEpoch(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - const auto admissions_before = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - - CmvrPlcAxisCommand stop; - stop.code = cmvr_plc::CommandCode::QuickStop; - stop.command_timeout_ms = 1000; - std::atomic result{true}; - std::thread queued([&] { - result.store(runtime->submitAxisSafetyCommand(1, stop)); - }); - const bool admitted = waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before; - }, - 500ms); - - plc.disconnectClient(); - const bool reconnected = waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms); - queue_hold.unlock(); - queued.join(); - - EXPECT_TRUE(admitted); - EXPECT_TRUE(reconnected) << runtime->lastError(); - EXPECT_FALSE(result.load()); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ConcurrentSafetyCommandsAreSerialized) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - plc.setQuickStopProcessingDelay(50); - - CmvrPlcAxisCommand stop; - stop.code = cmvr_plc::CommandCode::QuickStop; - stop.command_timeout_ms = 1000; - std::atomic first{false}; - std::atomic second{false}; - std::atomic ready{0}; - auto invoke = [&](std::atomic& result) { - ready.fetch_add(1); - while (ready.load() < 2) { - std::this_thread::yield(); - } - result.store(runtime->submitAxisSafetyCommand(1, stop)); - }; - std::thread first_thread(invoke, std::ref(first)); - std::thread second_thread(invoke, std::ref(second)); - first_thread.join(); - second_thread.join(); - EXPECT_TRUE(first.load()); - EXPECT_TRUE(second.load()); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, FatalStatusFlagsInvalidateMotionFeedback) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - - plc.publishPosition(123456); - ASSERT_TRUE(std::isfinite(protocol.getQ(1))); - plc.publishFatalFlags(); - EXPECT_TRUE(std::isnan(protocol.getQ(1))); - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - EXPECT_FALSE(protocol.reachedTargetQ(1)); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsOutOfRangeAxisAndReportsInvalidSamples) -{ - auto hostname_group = makeConfig(15020); - hostname_group.mutable_modbus_tcp()->set_host("localhost"); - ModbusTcpMotorBusRuntime hostname_runtime; - EXPECT_FALSE(hostname_runtime.init(hostname_group)); - - auto group = makeConfig(15020); - group.mutable_modbus_tcp()->mutable_axes(0)->set_axis_index(1000); - ModbusTcpMotorBusRuntime invalid_runtime; - EXPECT_FALSE(invalid_runtime.init(group)); - - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(15020))); - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - EXPECT_TRUE(std::isnan(protocol.getQ(1))); - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - EXPECT_FALSE(protocol.commandProfilePosition( - 1, std::numeric_limits::quiet_NaN(), 0.1, 0.1)); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RepositorySampleConfigParsesAndInitializesRuntime) -{ - std::ifstream input(CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH); - ASSERT_TRUE(input.is_open()) << CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH; - const std::string text((std::istreambuf_iterator(input)), - std::istreambuf_iterator()); - config::MotorRootConfig root; - ASSERT_TRUE(google::protobuf::TextFormat::ParseFromString(text, &root)); - ASSERT_TRUE(root.has_motor()); - ASSERT_EQ(root.motor().motor_groups_size(), 1); - - ModbusTcpMotorBusRuntime runtime; - EXPECT_TRUE(runtime.init(root.motor().motor_groups(0))) << runtime.lastError(); -} - -} // namespace -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt b/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt deleted file mode 100644 index 7dbd3cdc..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -add_library(modbus_plc_motor_driver SHARED - src/cmvr_plc_motor_protocol.cpp - src/modbus_plc_motor.cpp -) - -target_include_directories(modbus_plc_motor_driver - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/include -) - -target_link_libraries(modbus_plc_motor_driver - PUBLIC - cmvr_es::device::motor_core - cmvr_es::device::motor_bus_runtime - PRIVATE - cmvr_es::proto - glog -) - -add_library(cmvr_es::device::modbus_plc_motor_driver ALIAS modbus_plc_motor_driver) -install(TARGETS modbus_plc_motor_driver LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h b/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h deleted file mode 100644 index 0bf534ba..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H -#define CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H - -#include -#include -#include -#include -#include - -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" -#include "devices/motor/motor_protocol_interface.h" - -namespace cmvr::device { - -class CmvrPlcMotorProtocol final : public MotorProtocolInterface { -public: - explicit CmvrPlcMotorProtocol(std::shared_ptr bus_runtime); - ~CmvrPlcMotorProtocol() override = default; - - bool initNode(std::uint8_t node_id) override; - void setMode(std::uint8_t node_id, msgs::RunMode mode) override; - msgs::RunMode getMode(std::uint8_t node_id) override; - void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override; - void setLimitQd(std::uint8_t node_id, double qd) override; - void setLimitQ(std::uint8_t node_id, double ub, double lb) override; - bool calibrateZeroQ(std::uint8_t node_id) override; - bool reachedTargetQ(std::uint8_t node_id) override; - bool commandProfilePosition(std::uint8_t node_id, - double target_q, - double max_qd, - double max_qdd) override; - bool commandProfileVelocity(std::uint8_t node_id, - double target_qd, - double max_qdd) override; - bool commandCyclicPosition(std::uint8_t node_id, - double target_q, - double target_qd) override; - bool commandCyclicVelocity(std::uint8_t node_id, double target_qd) override; - bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override; - void setMotorConversion(std::uint8_t node_id, - double encoder_counts_per_rev, - double gear_ratio) override; - bool torqueOn(std::uint8_t node_id) override; - bool torqueOff(std::uint8_t node_id) override; - bool brakeRelease(std::uint8_t node_id) override; - bool quickStop(std::uint8_t node_id) override; - double getQ(std::uint8_t node_id) override; - double getQd(std::uint8_t node_id) override; - -private: - struct NodeState { - msgs::RunMode requested_mode{msgs::RUN_MODE_UNSPECIFIED}; - double limit_q_lb{0.0}; - double limit_q_ub{0.0}; - double limit_qd{0.0}; - double limit_qdd{0.0}; - bool cyclic_stream_open{false}; - bool cyclic_reconnect_latched{false}; - std::uint32_t cyclic_sequence{0}; - std::uint64_t cyclic_generation{0}; - std::uint64_t cyclic_connection_epoch{0}; - bool profile_feedback_bound{false}; - std::uint64_t profile_connection_epoch{0}; - }; - - bool submitSimple_(std::uint8_t node_id, - cmvr_plc::CommandCode code, - bool wait_for_terminal, - std::uint32_t timeout_ms); - bool ensureCyclicOpen_(std::uint8_t node_id, - NodeState& state, - msgs::RunMode mode); - static std::optional toMicroUnits_(double value); - static double fromMicroUnits_(std::int32_t value); - static bool isSupportedMode_(msgs::RunMode mode); - bool validatePosition_(const NodeState& state, double position) const; - bool validateVelocity_(const NodeState& state, double velocity) const; - bool validateAcceleration_(const NodeState& state, double acceleration) const; - bool readMotionStatus_(std::uint8_t node_id, CmvrPlcAxisStatus& status); - NodeState& nodeStateLocked_(std::uint8_t node_id); - - std::shared_ptr bus_runtime_; - std::mutex nodes_mutex_; - std::unordered_map nodes_; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h b/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h deleted file mode 100644 index f0a96831..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef CMVR_ES_MODBUS_PLC_MOTOR_H -#define CMVR_ES_MODBUS_PLC_MOTOR_H - -#include "cmvr/config/motor_config/motor_config.pb.h" -#include "devices/motor/abstract_motor.h" - -namespace cmvr::device { - -class ModbusPlcMotor final : public AbstractMotor { -public: - explicit ModbusPlcMotor(const config::MotorConfigItem& config); - - std::string typeName() const override { return "ModbusPlcMotor"; } - bool init() override; - bool torqueOff() override; - bool quickStop() override; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_MODBUS_PLC_MOTOR_H diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp b/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp deleted file mode 100644 index 3867303c..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp +++ /dev/null @@ -1,582 +0,0 @@ -#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" - -#include -#include -#include - -#include "common/base/logging/logger.h" - -namespace cmvr::device { -namespace { - -// MotorService accepts waits up to 10 minutes. The PLC timeout is a ceiling; -// shorter RPC timeouts still actively QuickStop from the service layer. -constexpr std::uint32_t kProfileCommandTimeoutCeilingMs = 600000; -constexpr std::uint32_t kSafetyCommandTimeoutMs = 5000; - -bool statusAllowsMotionFeedback(const CmvrPlcAxisStatus& status) -{ - constexpr std::uint16_t kFatalFlags = - cmvr_plc::StatusFlag::Fault | - cmvr_plc::StatusFlag::CommunicationWatchdogExpired | - cmvr_plc::StatusFlag::CyclicWatchdogExpired; - return (status.status_flags & kFatalFlags) == 0U && - status.result_code == cmvr_plc::ResultCode::Ok && - static_cast(status.command_state) <= - static_cast( - cmvr_plc::CommandState::CommunicationLost) && - !cmvr_plc::isFailure(status.command_state); -} - -} // namespace - -CmvrPlcMotorProtocol::CmvrPlcMotorProtocol( - std::shared_ptr bus_runtime) - : bus_runtime_(std::move(bus_runtime)) -{ - comm_proto = CommProto::CUSTOM; -} - -bool CmvrPlcMotorProtocol::initNode(const std::uint8_t node_id) -{ - if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) { - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] missing PLC axis mapping for motor " - << static_cast(node_id); - return false; - } - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); - state.cyclic_stream_open = false; - state.cyclic_reconnect_latched = false; - state.cyclic_sequence = 0; - state.cyclic_generation = 0; - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - return true; -} - -void CmvrPlcMotorProtocol::setMode( - const std::uint8_t node_id, - const msgs::RunMode mode) -{ - if (!isSupportedMode_(mode)) { - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] unsupported mode " - << msgs::RunMode_Name(mode); - return; - } - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - const bool cyclic_mode = - mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION || - mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; - if (cyclic_mode) { - // Every explicit cyclic setMode call is a new upper-layer stream - // generation, even when the mode value itself is unchanged. This is - // the only transition allowed to clear a reconnect latch. - state.requested_mode = mode; - state.cyclic_connection_epoch = - bus_runtime_ ? bus_runtime_->connectionEpoch() : 0U; - state.cyclic_stream_open = false; - state.cyclic_reconnect_latched = false; - state.cyclic_sequence = 0; - if (++state.cyclic_generation == 0U) { - ++state.cyclic_generation; - } - return; - } - if (state.requested_mode != mode) { - state.cyclic_stream_open = false; - } - state.requested_mode = mode; -} - -msgs::RunMode CmvrPlcMotorProtocol::getMode(const std::uint8_t node_id) -{ - CmvrPlcAxisStatus status; - if (bus_runtime_ && bus_runtime_->readAxisStatus(node_id, status)) { - const auto mode = static_cast(status.current_mode); - if (isSupportedMode_(mode)) { - return mode; - } - } - std::lock_guard lock(nodes_mutex_); - return nodeStateLocked_(node_id).requested_mode; -} - -void CmvrPlcMotorProtocol::setLimitQdd( - const std::uint8_t node_id, - const double u_qdd, - const double l_qdd) -{ - std::lock_guard lock(nodes_mutex_); - nodeStateLocked_(node_id).limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd)); -} - -void CmvrPlcMotorProtocol::setLimitQd( - const std::uint8_t node_id, - const double qd) -{ - std::lock_guard lock(nodes_mutex_); - nodeStateLocked_(node_id).limit_qd = std::abs(qd); -} - -void CmvrPlcMotorProtocol::setLimitQ( - const std::uint8_t node_id, - const double ub, - const double lb) -{ - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - state.limit_q_ub = ub; - state.limit_q_lb = lb; -} - -bool CmvrPlcMotorProtocol::calibrateZeroQ(const std::uint8_t node_id) -{ - return submitSimple_(node_id, cmvr_plc::CommandCode::SetZero, true, - kSafetyCommandTimeoutMs); -} - -bool CmvrPlcMotorProtocol::reachedTargetQ(const std::uint8_t node_id) -{ - CmvrPlcAxisStatus status; - return readMotionStatus_(node_id, status) && - statusAllowsMotionFeedback(status) && - (status.status_flags & cmvr_plc::StatusFlag::TargetReached) != 0U; -} - -bool CmvrPlcMotorProtocol::commandProfilePosition( - const std::uint8_t node_id, - const double target_q, - const double max_qd, - const double max_qdd) -{ - NodeState state; - { - std::lock_guard lock(nodes_mutex_); - auto& stored = nodeStateLocked_(node_id); - stored.requested_mode = msgs::RUN_MODE_PROFILE_POSITION; - stored.cyclic_stream_open = false; - state = stored; - } - if (!validatePosition_(state, target_q) || - !validateVelocity_(state, max_qd) || - !validateAcceleration_(state, max_qdd)) { - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-position command"; - return false; - } - const auto position = toMicroUnits_(target_q); - const auto velocity = toMicroUnits_(std::abs(max_qd)); - const auto acceleration = toMicroUnits_(std::abs(max_qdd)); - if (!position || !velocity || !acceleration) { - return false; - } - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::ProfilePosition; - command.target_position = *position; - command.target_velocity = *velocity; - command.acceleration = *acceleration; - command.command_timeout_ms = kProfileCommandTimeoutCeilingMs; - if (!bus_runtime_) { - return false; - } - const auto submission_epoch = bus_runtime_->connectionEpoch(); - const auto submitted = - bus_runtime_->submitAxisCommand( - node_id, command, false, submission_epoch); - const auto completed_epoch = bus_runtime_->connectionEpoch(); - if (!submitted && completed_epoch == submission_epoch) { - return false; - } - { - std::lock_guard lock(nodes_mutex_); - auto& stored = nodeStateLocked_(node_id); - // Bind on every cross-epoch outcome, including an ambiguous failed - // submit, so feedback remains invalid until safety cleanup. - stored.profile_feedback_bound = true; - stored.profile_connection_epoch = submission_epoch; - } - return submitted && completed_epoch == submission_epoch; -} - -bool CmvrPlcMotorProtocol::commandProfileVelocity( - const std::uint8_t node_id, - const double target_qd, - const double max_qdd) -{ - NodeState state; - { - std::lock_guard lock(nodes_mutex_); - auto& stored = nodeStateLocked_(node_id); - stored.requested_mode = msgs::RUN_MODE_PROFILE_VELOCITY; - stored.cyclic_stream_open = false; - state = stored; - } - if (!validateVelocity_(state, target_qd) || - !validateAcceleration_(state, max_qdd)) { - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-velocity command"; - return false; - } - const auto velocity = toMicroUnits_(target_qd); - const auto acceleration = toMicroUnits_(std::abs(max_qdd)); - if (!velocity || !acceleration) { - return false; - } - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::ProfileVelocity; - command.target_velocity = *velocity; - command.acceleration = *acceleration; - command.command_timeout_ms = kProfileCommandTimeoutCeilingMs; - if (!bus_runtime_) { - return false; - } - const auto submission_epoch = bus_runtime_->connectionEpoch(); - const auto submitted = - bus_runtime_->submitAxisCommand( - node_id, command, false, submission_epoch); - const auto completed_epoch = bus_runtime_->connectionEpoch(); - if (!submitted && completed_epoch == submission_epoch) { - return false; - } - { - std::lock_guard lock(nodes_mutex_); - auto& stored = nodeStateLocked_(node_id); - stored.profile_feedback_bound = true; - stored.profile_connection_epoch = submission_epoch; - } - return submitted && completed_epoch == submission_epoch; -} - -bool CmvrPlcMotorProtocol::commandCyclicPosition( - const std::uint8_t node_id, - const double target_q, - const double target_qd) -{ - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - if (!validatePosition_(state, target_q) || - !validateVelocity_(state, target_qd) || - !ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { - return false; - } - const auto position = toMicroUnits_(target_q); - const auto velocity = toMicroUnits_(target_qd); - if (!position || !velocity) { - return false; - } - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::CyclicPositionSample; - command.target_position = *position; - command.target_velocity = *velocity; - command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); - if (++state.cyclic_sequence == 0) { - ++state.cyclic_sequence; - } - command.cyclic_sample_sequence = state.cyclic_sequence; - const auto submitted = - bus_runtime_->submitAxisCommand( - node_id, command, false, state.cyclic_connection_epoch); - if (submitted) { - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - } else if ( - bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) { - state.cyclic_reconnect_latched = true; - state.cyclic_stream_open = false; - state.cyclic_sequence = 0; - state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); - } - return submitted; -} - -bool CmvrPlcMotorProtocol::commandCyclicVelocity( - const std::uint8_t node_id, - const double target_qd) -{ - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - if (!validateVelocity_(state, target_qd) || - !ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) { - return false; - } - const auto velocity = toMicroUnits_(target_qd); - if (!velocity) { - return false; - } - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::CyclicVelocitySample; - command.target_velocity = *velocity; - command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); - if (++state.cyclic_sequence == 0) { - ++state.cyclic_sequence; - } - command.cyclic_sample_sequence = state.cyclic_sequence; - const auto submitted = - bus_runtime_->submitAxisCommand( - node_id, command, false, state.cyclic_connection_epoch); - if (submitted) { - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - } else if ( - bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) { - state.cyclic_reconnect_latched = true; - state.cyclic_stream_open = false; - state.cyclic_sequence = 0; - state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); - } - return submitted; -} - -bool CmvrPlcMotorProtocol::commandCyclicTorque( - const std::uint8_t node_id, - const double target_tau) -{ - (void)target_tau; - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] cyclic torque is unsupported, motor=" - << static_cast(node_id); - return false; -} - -void CmvrPlcMotorProtocol::setMotorConversion( - const std::uint8_t node_id, - const double encoder_counts_per_rev, - const double gear_ratio) -{ - (void)node_id; - (void)encoder_counts_per_rev; - (void)gear_ratio; - // CMVR PLC v1 exchanges SI quantities in fixed-point micro-units. -} - -bool CmvrPlcMotorProtocol::torqueOn(const std::uint8_t node_id) -{ - return submitSimple_(node_id, cmvr_plc::CommandCode::Enable, true, - kSafetyCommandTimeoutMs); -} - -bool CmvrPlcMotorProtocol::torqueOff(const std::uint8_t node_id) -{ - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::Disable; - command.command_timeout_ms = kSafetyCommandTimeoutMs; - const auto result = - bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command); - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - state.cyclic_stream_open = false; - if (result) { - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - } - return result; -} - -bool CmvrPlcMotorProtocol::brakeRelease(const std::uint8_t node_id) -{ - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] brake release is unsupported, motor=" - << static_cast(node_id); - return false; -} - -bool CmvrPlcMotorProtocol::quickStop(const std::uint8_t node_id) -{ - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::QuickStop; - command.command_timeout_ms = kSafetyCommandTimeoutMs; - const auto result = - bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command); - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - state.cyclic_stream_open = false; - if (result) { - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - } - return result; -} - -double CmvrPlcMotorProtocol::getQ(const std::uint8_t node_id) -{ - CmvrPlcAxisStatus status; - if (!readMotionStatus_(node_id, status) || - !statusAllowsMotionFeedback(status)) { - return std::numeric_limits::quiet_NaN(); - } - return fromMicroUnits_(status.actual_position); -} - -double CmvrPlcMotorProtocol::getQd(const std::uint8_t node_id) -{ - CmvrPlcAxisStatus status; - if (!readMotionStatus_(node_id, status) || - !statusAllowsMotionFeedback(status)) { - return std::numeric_limits::quiet_NaN(); - } - return fromMicroUnits_(status.actual_velocity); -} - -bool CmvrPlcMotorProtocol::readMotionStatus_( - const std::uint8_t node_id, - CmvrPlcAxisStatus& status) -{ - if (!bus_runtime_) { - return false; - } - std::lock_guard lock(nodes_mutex_); - const auto& state = nodeStateLocked_(node_id); - const auto read_epoch = bus_runtime_->connectionEpoch(); - if (state.profile_feedback_bound && - state.profile_connection_epoch != read_epoch) { - return false; - } - if (!bus_runtime_->readAxisStatus(node_id, status)) { - return false; - } - // Reject a status transaction that straddled a disconnect/reconnect even - // when no profile binding was active at the first check. - if (bus_runtime_->connectionEpoch() != read_epoch) { - return false; - } - return !state.profile_feedback_bound || - state.profile_connection_epoch == read_epoch; -} - -bool CmvrPlcMotorProtocol::submitSimple_( - const std::uint8_t node_id, - const cmvr_plc::CommandCode code, - const bool wait_for_terminal, - const std::uint32_t timeout_ms) -{ - CmvrPlcAxisCommand command; - command.code = code; - command.command_timeout_ms = timeout_ms; - return bus_runtime_ && - bus_runtime_->submitAxisCommand(node_id, command, wait_for_terminal); -} - -bool CmvrPlcMotorProtocol::ensureCyclicOpen_( - const std::uint8_t node_id, - NodeState& state, - const msgs::RunMode mode) -{ - if (!bus_runtime_) { - return false; - } - const auto connection_epoch = bus_runtime_->connectionEpoch(); - if (state.cyclic_connection_epoch != connection_epoch) { - // A cyclic generation is bound to the PLC ownership epoch in which it - // was created. Never reinterpret a setpoint from that generation as - // the first setpoint of a freshly reconnected PLC stream. - if (state.cyclic_generation != 0U) { - state.cyclic_reconnect_latched = true; - } - state.cyclic_connection_epoch = connection_epoch; - state.cyclic_stream_open = false; - state.cyclic_sequence = 0; - } - if (state.cyclic_reconnect_latched) { - CMVR_LOG(WARNING) - << "[CmvrPlcMotorProtocol] cyclic stream crossed a PLC session; " - "explicit setMode from a new upper-layer stream is required, motor=" - << static_cast(node_id); - return false; - } - if (state.cyclic_stream_open && state.requested_mode == mode) { - return true; - } - - // Preserve direct protocol use that does not call setMode explicitly: - // its first successful Open still establishes a generation. Once that - // generation has observed a reconnect, only explicit setMode can recover. - if (state.cyclic_generation == 0U) { - state.cyclic_generation = 1U; - state.cyclic_connection_epoch = connection_epoch; - } - - CmvrPlcAxisCommand command; - command.code = mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION - ? cmvr_plc::CommandCode::OpenCyclicPosition - : cmvr_plc::CommandCode::OpenCyclicVelocity; - command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); - command.command_timeout_ms = kSafetyCommandTimeoutMs; - if (!bus_runtime_->submitAxisCommand( - node_id, command, false, state.cyclic_connection_epoch)) { - if (bus_runtime_->connectionEpoch() != - state.cyclic_connection_epoch) { - state.cyclic_reconnect_latched = true; - state.cyclic_stream_open = false; - state.cyclic_sequence = 0; - state.cyclic_connection_epoch = - bus_runtime_->connectionEpoch(); - } - return false; - } - state.requested_mode = mode; - state.cyclic_stream_open = true; - state.cyclic_sequence = 0; - return true; -} - -std::optional CmvrPlcMotorProtocol::toMicroUnits_(const double value) -{ - if (!std::isfinite(value)) { - return std::nullopt; - } - const auto scaled = std::round(value * cmvr_plc::kPositionScale); - if (scaled < static_cast(std::numeric_limits::min()) || - scaled > static_cast(std::numeric_limits::max())) { - return std::nullopt; - } - return static_cast(scaled); -} - -double CmvrPlcMotorProtocol::fromMicroUnits_(const std::int32_t value) -{ - return static_cast(value) / cmvr_plc::kPositionScale; -} - -bool CmvrPlcMotorProtocol::isSupportedMode_(const msgs::RunMode mode) -{ - return mode == msgs::RUN_MODE_PROFILE_POSITION || - mode == msgs::RUN_MODE_PROFILE_VELOCITY || - mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION || - mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; -} - -bool CmvrPlcMotorProtocol::validatePosition_( - const NodeState& state, - const double position) const -{ - return std::isfinite(position) && - (state.limit_q_ub <= state.limit_q_lb || - (position >= state.limit_q_lb && position <= state.limit_q_ub)); -} - -bool CmvrPlcMotorProtocol::validateVelocity_( - const NodeState& state, - const double velocity) const -{ - return std::isfinite(velocity) && - (state.limit_qd <= 0.0 || std::abs(velocity) <= state.limit_qd); -} - -bool CmvrPlcMotorProtocol::validateAcceleration_( - const NodeState& state, - const double acceleration) const -{ - return std::isfinite(acceleration) && acceleration >= 0.0 && - (state.limit_qdd <= 0.0 || acceleration <= state.limit_qdd); -} - -CmvrPlcMotorProtocol::NodeState& CmvrPlcMotorProtocol::nodeStateLocked_( - const std::uint8_t node_id) -{ - return nodes_[node_id]; -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp b/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp deleted file mode 100644 index faa9952a..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h" - -#include "common/base/logging/logger.h" -#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" -#include - -namespace cmvr::device { - -ModbusPlcMotor::ModbusPlcMotor(const config::MotorConfigItem& config) -{ - info_.id = config.id(); - info_.joint_name = config.joint_name(); - info_.limit_q_lb = config.limit_q_lb(); - info_.limit_q_ub = config.limit_q_ub(); - info_.limit_qd = config.limit_qd(); - info_.limit_qdd = config.limit_qdd(); - node_id_ = static_cast(config.id()); -} - -bool ModbusPlcMotor::init() -{ - if (!protocol_ || - !std::dynamic_pointer_cast(protocol_)) { - CMVR_LOG(ERROR) << "[ModbusPlcMotor] invalid CMVR PLC protocol for " - << info_.joint_name; - return false; - } - if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) || - !std::isfinite(info_.limit_qd) || !std::isfinite(info_.limit_qdd) || - info_.limit_q_ub <= info_.limit_q_lb || - info_.limit_qd <= 0.0 || info_.limit_qdd <= 0.0) { - CMVR_LOG(ERROR) << "[ModbusPlcMotor] finite position/velocity/acceleration " - "limits are required for " - << info_.joint_name; - return false; - } - setLimitQ(info_.limit_q_ub, info_.limit_q_lb); - setLimitQd(info_.limit_qd); - setLimitQdd(info_.limit_qdd, -info_.limit_qdd); - return true; -} - -bool ModbusPlcMotor::torqueOff() -{ - auto protocol = std::dynamic_pointer_cast(protocol_); - return protocol && protocol->torqueOff(node_id_); -} - -bool ModbusPlcMotor::quickStop() -{ - auto protocol = std::dynamic_pointer_cast(protocol_); - return protocol && protocol->quickStop(node_id_); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/manager/CMakeLists.txt b/cmvr-es/devices/motor/manager/CMakeLists.txt index 46f66040..8ea5f495 100644 --- a/cmvr-es/devices/motor/manager/CMakeLists.txt +++ b/cmvr-es/devices/motor/manager/CMakeLists.txt @@ -13,7 +13,6 @@ target_link_libraries(motor_manager cmvr_es::device::ti5_canopen_motor_driver cmvr_es::device::mujoco_motor_driver cmvr_es::device::ethercat_motor_driver - cmvr_es::device::modbus_plc_motor_driver cmvr_es::ik_solver glog ) diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index e67a263e..3bbd2cab 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -78,10 +78,6 @@ private: const config::MotorGroupConfig& group_cfg, const std::vector& motor_cfgs, const std::shared_ptr& bus_runtime) const; - std::vector> createModbusTcpMotors_( - const config::MotorGroupConfig& group_cfg, - const std::vector& motor_cfgs, - const std::shared_ptr& bus_runtime) const; private: config::MotorConfig cfg_; diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index ff2f37ff..5e83b043 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -15,14 +15,11 @@ #include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h" #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" #include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" #include "devices/motor/drivers/mujoco/include/mujoco_motor.h" -#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" -#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h" #include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h" #include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" @@ -418,16 +415,6 @@ std::shared_ptr MotorManager::createBusRuntime_( return std::make_shared(); case config::MOTOR_BUS_MUJOCO: return std::make_shared(); - case config::MOTOR_BUS_MODBUS_TCP: - if (group_cfg.vendor() == config::MOTOR_VENDOR_PLC_GENERIC && - group_cfg.protocol() == config::MOTOR_PROTOCOL_CMVR_PLC_V1) { - return std::make_shared(); - } - CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP motor: vendor=" - << config::MotorVendor_Name(group_cfg.vendor()) - << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) - << ", group=" << group_cfg.id(); - return nullptr; case config::MOTOR_BUS_ETHERCAT: { auto runtime = std::make_shared(); if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU && @@ -467,8 +454,6 @@ std::vector> MotorManager::createMotors_( return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime); case config::MOTOR_BUS_ETHERCAT: return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime); - case config::MOTOR_BUS_MODBUS_TCP: - return createModbusTcpMotors_(group_cfg, motor_cfgs, bus_runtime); default: CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: " << config::MotorBusType_Name(group_cfg.bus_type()) @@ -611,50 +596,4 @@ std::vector> MotorManager::createEthercatMotors_( return motors; } -std::vector> MotorManager::createModbusTcpMotors_( - const config::MotorGroupConfig& group_cfg, - const std::vector& motor_cfgs, - const std::shared_ptr& bus_runtime) const -{ - auto modbus_runtime = - std::dynamic_pointer_cast(bus_runtime); - if (!modbus_runtime || !group_cfg.has_modbus_tcp()) { - CMVR_LOG(ERROR) << "[MotorManager] missing Modbus TCP runtime/config: " - << group_cfg.id(); - return {}; - } - if (group_cfg.vendor() != config::MOTOR_VENDOR_PLC_GENERIC || - group_cfg.protocol() != config::MOTOR_PROTOCOL_CMVR_PLC_V1) { - CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP PLC motor: vendor=" - << config::MotorVendor_Name(group_cfg.vendor()) - << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()); - return {}; - } - for (const auto& motor_cfg : motor_cfgs) { - if (motor_cfg.id() < 0 || motor_cfg.id() > 255 || - !modbus_runtime->hasMotor(static_cast(motor_cfg.id()))) { - CMVR_LOG(ERROR) << "[MotorManager] missing PLC axis mapping for motor id " - << motor_cfg.id() << " in group: " << group_cfg.id(); - return {}; - } - } - - auto protocol = std::make_shared(modbus_runtime); - std::vector> motors; - motors.reserve(motor_cfgs.size()); - for (const auto& cfg : motor_cfgs) { - auto motor = std::make_shared(cfg); - motor->setProtocol(protocol); - // initNode() and ModbusPlcMotor::init() only establish local mappings and - // limits. The shared TCP connection starts after all motors are created. - if (!motor->init()) { - CMVR_LOG(ERROR) << "[MotorManager] failed to init Modbus PLC motor: " - << cfg.joint_name(); - return {}; - } - motors.push_back(std::move(motor)); - } - return motors; -} - } // namespace cmvr::device diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 9f54e4df..f4a39cf3 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -221,44 +221,6 @@ if(BUILD_TESTING) ENVIRONMENT "${_grpc_agv_test_environment}" ) - set(_grpc_motor_modbus_e2e_libmodbus_root - "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11") - add_executable(grpc_motor_service_modbus_e2e_test - grpc/tests/grpc_motor_service_modbus_e2e_test.cpp - ) - target_include_directories(grpc_motor_service_modbus_e2e_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ${_grpc_motor_modbus_e2e_libmodbus_root}/include - ) - target_link_directories(grpc_motor_service_modbus_e2e_test - PRIVATE - ${_grpc_motor_modbus_e2e_libmodbus_root}/lib - ) - target_link_libraries(grpc_motor_service_modbus_e2e_test - PRIVATE - service - modbus - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_motor_service_modbus_e2e_test - COMMAND grpc_motor_service_modbus_e2e_test - ) - set(_grpc_motor_modbus_e2e_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_motor_modbus_e2e_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_motor_service_modbus_e2e_test PROPERTIES - TIMEOUT 20 - ENVIRONMENT "${_grpc_motor_modbus_e2e_environment}" - ) endif() # -------------------------------------------------------- diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md index 49e053cc..2369d49e 100644 --- a/cmvr-es/service/README.md +++ b/cmvr-es/service/README.md @@ -42,7 +42,7 @@ grpcurl -plaintext \ ## MotorService `MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的 -`MotorManager` 和 `AbstractMotor`，不直接持有 PLC、现场总线或厂商驱动。 +`MotorManager` 和 `AbstractMotor`，不直接持有现场总线或厂商驱动。 关键文件： @@ -52,8 +52,6 @@ grpcurl -plaintext \ 和 [`grpc/src/grpc_motor_service.cpp`](grpc/src/grpc_motor_service.cpp) - 注册：[`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp) - 单元测试：[`grpc/tests/grpc_motor_service_test.cpp`](grpc/tests/grpc_motor_service_test.cpp) -- gRPC–Modbus 端到端测试： - [`grpc/tests/grpc_motor_service_modbus_e2e_test.cpp`](grpc/tests/grpc_motor_service_modbus_e2e_test.cpp) 服务按单电机仲裁。同步 Profile 命令、Cyclic Position/Velocity 双向流、 `setEnabled`、状态读取和软件 `emergencyStop` 共用同一控制权状态： @@ -66,11 +64,6 @@ grpcurl -plaintext \ - 只有成功执行 `setEnabled(true)` 才解除服务内软件急停锁存； - 服务层 Quick Stop 和 `emergencyStop` 都不具备功能安全等级。 -PLC 后端的连接 epoch、stream epoch、寄存器、ACK 和 TIA Portal 要求见： - -- [Modbus TCP PLC runtime](../devices/motor/bus_runtime/modbus_tcp/README.md) -- [MotorService 与 CMVR PLC v1 完整协议](../../docs/motor_service_modbus_tcp.md) - AUBO 控制柜 IO 不经过 `MotorService`，由 `ArmService/ExecuteJsonCommand` 转发到目标 `RobotArm`。厂商命令和安全约束见 [AUBO 控制柜 IO](../devices/arm/aubo_arm/README.md)。 diff --git a/cmvr-es/service/grpc/src/grpc_motor_service.cpp b/cmvr-es/service/grpc/src/grpc_motor_service.cpp index 1e2f01f4..a08e67de 100644 --- a/cmvr-es/service/grpc/src/grpc_motor_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_motor_service.cpp @@ -966,7 +966,7 @@ grpc::Status gRPCMotorServiceImpl::setZeroImpl( } if (!calibrated) { const std::string error = - "zero calibration outcome unknown; inspect PLC zero_epoch/session before retry"; + "zero calibration outcome unknown; inspect device state before retry"; setLastError(resolved.control, error); lease.reset(); fillFeedback(response->mutable_header(), false, error); @@ -1984,7 +1984,7 @@ grpc::Status gRPCMotorServiceImpl::setEnabledImpl( } } - // A false acknowledgement may still mean the PLC committed the + // A false acknowledgement may still mean the backend committed the // request. A successful enable also needs rollback if cancellation or // E-stop won while torqueOn was in flight. const bool cleanup_required = diff --git a/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp b/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp deleted file mode 100644 index 142608d6..00000000 --- a/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp +++ /dev/null @@ -1,907 +0,0 @@ -#include "service/grpc/include/grpc_motor_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "cmvr/config/motor_config/motor_config.pb.h" -#include "common/config/config_files.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" -#include "devices/motor/manager/include/motor_manager.h" -#include "manager/device_manager/include/device_manager.h" - -namespace cmvr::service { -namespace { - -using namespace std::chrono_literals; -namespace plc = device::cmvr_plc; - -constexpr char kManagerId[] = "e2e_plc_motors"; -constexpr char kMotorGroupId[] = "e2e_plc_axis_group"; -constexpr char kJointName[] = "E2E_PLC_AXIS"; - -std::uint16_t reserveLoopbackPort() -{ - const int socket_fd = ::socket(AF_INET, SOCK_STREAM, 0); - if (socket_fd < 0) { - return 0; - } - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); - address.sin_port = 0; - if (::bind(socket_fd, reinterpret_cast(&address), - sizeof(address)) != 0) { - ::close(socket_fd); - return 0; - } - - socklen_t length = sizeof(address); - if (::getsockname(socket_fd, reinterpret_cast(&address), - &length) != 0) { - ::close(socket_fd); - return 0; - } - const auto port = ntohs(address.sin_port); - ::close(socket_fd); - return port; -} - -template -bool waitUntil(Predicate&& predicate, const std::chrono::milliseconds timeout) -{ - const auto deadline = std::chrono::steady_clock::now() + timeout; - do { - if (predicate()) { - return true; - } - std::this_thread::sleep_for(5ms); - } while (std::chrono::steady_clock::now() < deadline); - return predicate(); -} - -class FakeCmvrPlc final { -public: - ~FakeCmvrPlc() { stop(); } - - bool start() - { - port_ = reserveLoopbackPort(); - if (port_ == 0) { - std::fprintf(stderr, "reserveLoopbackPort failed: %s\n", - std::strerror(errno)); - return false; - } - - context_ = modbus_new_tcp("127.0.0.1", port_); - mapping_ = modbus_mapping_new( - 1, 1, plc::kAxisFirstOffset + plc::kAxisRegisterStride, 1); - if (!context_ || !mapping_) { - std::fprintf(stderr, "fake PLC allocation failed: %s\n", - modbus_strerror(errno)); - stop(); - return false; - } - - auto* registers = mapping_->tab_registers; - registers[plc::kMagicCmOffset] = plc::kMagicCm; - registers[plc::kMagicVrOffset] = plc::kMagicVr; - registers[plc::kProtocolMajorOffset] = plc::kProtocolMajor; - registers[plc::kProtocolMinorOffset] = plc::kProtocolMinor; - registers[plc::kAxisCountOffset] = 1; - registers[plc::kOwnerStateOffset] = - static_cast(plc::OwnerState::None); - plc::encodeUint32(registers, plc::kPlcBootIdOffset, 0x45453245U); - plc::encodeUint32(registers, plc::kPlcHeartbeatOffset, - plc_heartbeat_); - - const auto status_base = - plc::axisBase(0) + plc::kAxisStatusRelativeOffset; - registers[status_base + plc::kCommandState] = - static_cast(plc::CommandState::Idle); - registers[status_base + plc::kResultCode] = - static_cast(plc::ResultCode::Ok); - publishSnapshot_(registers + status_base); - - listen_socket_.store(modbus_tcp_listen(context_, 2)); - if (listen_socket_.load() < 0) { - std::fprintf(stderr, "modbus_tcp_listen failed on %u: %s\n", - port_, modbus_strerror(errno)); - stop(); - return false; - } - - running_.store(true); - worker_ = std::thread(&FakeCmvrPlc::loop_, this); - return true; - } - - void stop() - { - running_.store(false); - const auto client = client_socket_.exchange(-1); - if (client >= 0) { - ::shutdown(client, SHUT_RDWR); - ::close(client); - } - const auto listener = listen_socket_.exchange(-1); - if (listener >= 0) { - ::shutdown(listener, SHUT_RDWR); - ::close(listener); - } - if (worker_.joinable()) { - worker_.join(); - } - if (mapping_) { - modbus_mapping_free(mapping_); - mapping_ = nullptr; - } - if (context_) { - modbus_free(context_); - context_ = nullptr; - } - } - - void disconnectClient() - { - const auto client = client_socket_.load(); - if (client >= 0) { - ::shutdown(client, SHUT_RDWR); - } - } - - std::uint16_t port() const { return port_; } - std::uint32_t commandCount() const { return command_count_.load(); } - std::uint32_t acceptedSessionCount() const - { - return accepted_session_count_.load(); - } - std::uint32_t enableCount() const { return enable_count_.load(); } - std::uint32_t profilePositionCount() const - { - return profile_position_count_.load(); - } - std::uint32_t profileVelocityCount() const - { - return profile_velocity_count_.load(); - } - std::uint32_t cyclicOpenCount() const - { - return cyclic_open_count_.load(); - } - std::uint32_t cyclicSampleCount() const - { - return cyclic_sample_count_.load(); - } - std::uint32_t quickStopCount() const - { - return quick_stop_count_.load(); - } - -private: - void loop_() - { - std::array request{}; - while (running_.load()) { - int listener = listen_socket_.load(); - const int accepted = - listener < 0 ? -1 : modbus_tcp_accept(context_, &listener); - if (accepted < 0) { - if (running_.load()) { - std::this_thread::sleep_for(5ms); - } - continue; - } - - client_socket_.store(accepted); - while (running_.load()) { - const auto request_length = - modbus_receive(context_, request.data()); - if (request_length <= 0) { - break; - } - if (modbus_reply(context_, request.data(), request_length, - mapping_) < 0) { - break; - } - // Treat the mapping as the PLC's published process image. - // Read-only FC3 requests must not themselves advance the - // seqlock; otherwise the runtime's guard/full/guard snapshot - // validation could never observe one stable scan. - if (request_length > 7 && request[7] == 0x10U) { - processMailbox_(); - } - } - - const auto client = client_socket_.exchange(-1); - if (client >= 0) { - ::close(client); - } - } - } - - void processMailbox_() - { - auto* registers = mapping_->tab_registers; - const auto base = plc::axisBase(0); - const auto status_base = base + plc::kAxisStatusRelativeOffset; - beginSnapshot_(registers + status_base); - - plc::encodeUint32(registers, plc::kPlcHeartbeatOffset, - ++plc_heartbeat_); - const auto session = - plc::decodeUint32(registers, plc::kCmvrSessionIdOffset); - if (session != observed_session_) { - // A new session is not observable as Accepted until the old - // stream/mailbox state has been made safe. - registers[plc::kOwnerStateOffset] = - static_cast(plc::OwnerState::Accepting); - observed_session_ = session; - active_session_ = session; - last_sequence_ = 0; - registers[status_base + plc::kStatusFlags] &= - static_cast(~plc::StatusFlag::StreamActive); - plc::encodeUint32(registers + base, plc::kPayloadSequence, 0); - plc::encodeUint32(registers + base, - plc::kPayloadSequenceMirror, 0); - plc::encodeUint32(registers + base, - plc::kCommitSequenceRelativeOffset, 0); - plc::encodeUint32(registers, plc::kOwnerSessionIdOffset, - active_session_); - registers[plc::kOwnerStateOffset] = - static_cast( - active_session_ == 0 ? plc::OwnerState::None - : plc::OwnerState::Accepted); - if (active_session_ != 0) { - accepted_session_count_.fetch_add(1); - } - } - - if (active_session_ == 0 || active_session_ != session) { - publishSnapshot_(registers + status_base); - return; - } - - const auto sequence = - plc::decodeUint32(registers + base, plc::kPayloadSequence); - const auto sequence_mirror = - plc::decodeUint32(registers + base, - plc::kPayloadSequenceMirror); - const auto commit = - plc::decodeUint32(registers + base, - plc::kCommitSequenceRelativeOffset); - const auto command_session = - plc::decodeUint32(registers + base, plc::kCommandSessionId); - if (sequence == 0 || sequence == last_sequence_ || - sequence != sequence_mirror || sequence != commit || - command_session != active_session_) { - publishSnapshot_(registers + status_base); - return; - } - - last_sequence_ = sequence; - command_count_.fetch_add(1); - plc::encodeUint32(registers + status_base, plc::kAckSequence, - sequence); - plc::encodeUint32(registers + status_base, plc::kActiveSequence, - sequence); - plc::encodeUint32(registers + status_base, plc::kAckSessionId, - active_session_); - registers[status_base + plc::kResultCode] = - static_cast(plc::ResultCode::Ok); - - const auto code = static_cast( - registers[base + plc::kCommandCode]); - const auto target_position = - plc::decodeInt32(registers + base, plc::kTargetPosition); - const auto target_velocity = - plc::decodeInt32(registers + base, plc::kTargetVelocity); - auto& flags = registers[status_base + plc::kStatusFlags]; - auto state = plc::CommandState::Completed; - - switch (code) { - case plc::CommandCode::ProfilePosition: - profile_position_count_.fetch_add(1); - registers[status_base + plc::kCurrentMode] = - msgs::RUN_MODE_PROFILE_POSITION; - plc::encodeInt32(registers + status_base, - plc::kActualPosition, target_position); - plc::encodeInt32(registers + status_base, - plc::kActualVelocity, 0); - plc::encodeInt32(registers + status_base, - plc::kTargetPositionStatus, target_position); - flags |= plc::StatusFlag::TargetReached; - flags &= static_cast( - ~plc::StatusFlag::QuickStopActive); - state = plc::CommandState::TargetReached; - break; - case plc::CommandCode::ProfileVelocity: - profile_velocity_count_.fetch_add(1); - registers[status_base + plc::kCurrentMode] = - msgs::RUN_MODE_PROFILE_VELOCITY; - plc::encodeInt32(registers + status_base, - plc::kActualVelocity, target_velocity); - plc::encodeInt32(registers + status_base, - plc::kTargetVelocityStatus, target_velocity); - flags &= static_cast( - ~plc::StatusFlag::QuickStopActive); - state = plc::CommandState::Accepted; - break; - case plc::CommandCode::OpenCyclicPosition: - cyclic_open_count_.fetch_add(1); - registers[status_base + plc::kCurrentMode] = - msgs::RUN_MODE_CYCLIC_SYNC_POSITION; - plc::encodeUint32( - registers + status_base, - plc::kLastAppliedCyclicSequence, 0); - flags |= plc::StatusFlag::StreamActive; - flags &= static_cast( - ~plc::StatusFlag::QuickStopActive); - state = plc::CommandState::Accepted; - break; - case plc::CommandCode::CyclicPositionSample: - cyclic_sample_count_.fetch_add(1); - plc::encodeInt32(registers + status_base, - plc::kActualPosition, target_position); - plc::encodeInt32(registers + status_base, - plc::kActualVelocity, target_velocity); - plc::encodeUint32( - registers + status_base, - plc::kLastAppliedCyclicSequence, - plc::decodeUint32(registers + base, - plc::kCyclicSampleSequence)); - state = plc::CommandState::Accepted; - break; - case plc::CommandCode::QuickStop: - quick_stop_count_.fetch_add(1); - plc::encodeInt32(registers + status_base, - plc::kActualVelocity, 0); - flags |= plc::StatusFlag::QuickStopActive; - flags &= static_cast( - ~plc::StatusFlag::StreamActive); - state = plc::CommandState::QuickStopped; - break; - case plc::CommandCode::Enable: - enable_count_.fetch_add(1); - flags |= plc::StatusFlag::Enabled; - flags &= static_cast( - ~plc::StatusFlag::QuickStopActive); - break; - case plc::CommandCode::Disable: - flags &= static_cast( - ~plc::StatusFlag::Enabled); - flags &= static_cast( - ~plc::StatusFlag::StreamActive); - break; - default: - registers[status_base + plc::kResultCode] = - static_cast( - plc::ResultCode::Unsupported); - state = plc::CommandState::Rejected; - break; - } - - registers[status_base + plc::kCommandState] = - static_cast(state); - publishSnapshot_(registers + status_base); - } - - void publishSnapshot_(std::uint16_t* status) - { - if (pending_state_sequence_ == 0U) { - beginSnapshot_(status); - } - plc::encodeUint32(status, plc::kHeartbeatAge, 0); - plc::encodeUint32(status, plc::kStateSequenceMirror, - pending_state_sequence_); - plc::encodeUint32(status, plc::kStateSequence, - pending_state_sequence_); - state_sequence_ = pending_state_sequence_; - pending_state_sequence_ = 0U; - } - - void beginSnapshot_(std::uint16_t* status) - { - auto next = state_sequence_ + 2U; - if (next == 0U) { - next = 2U; - } - pending_state_sequence_ = next; - plc::encodeUint32(status, plc::kStateSequence, next - 1U); - } - - modbus_t* context_{nullptr}; - modbus_mapping_t* mapping_{nullptr}; - std::thread worker_; - std::atomic running_{false}; - std::atomic listen_socket_{-1}; - std::atomic client_socket_{-1}; - std::atomic command_count_{0}; - std::atomic accepted_session_count_{0}; - std::atomic enable_count_{0}; - std::atomic profile_position_count_{0}; - std::atomic profile_velocity_count_{0}; - std::atomic cyclic_open_count_{0}; - std::atomic cyclic_sample_count_{0}; - std::atomic quick_stop_count_{0}; - std::uint32_t observed_session_{0}; - std::uint32_t active_session_{0}; - std::uint32_t plc_heartbeat_{1}; - std::uint32_t state_sequence_{0}; - std::uint32_t pending_state_sequence_{0}; - std::uint32_t last_sequence_{0}; - std::uint16_t port_{0}; -}; - -std::string writeTemporaryMotorConfig(const std::uint16_t port) -{ - char path[] = "/tmp/cmvr_motor_service_modbus_e2e_XXXXXX.pb.txt"; - const int descriptor = ::mkstemps(path, 7); - if (descriptor < 0) { - return {}; - } - ::close(descriptor); - - std::ofstream output(path, std::ios::out | std::ios::trunc); - if (!output.is_open()) { - std::remove(path); - return {}; - } - - output << "motor {\n" - << " id: \"" << kManagerId << "\"\n" - << " motor_groups {\n" - << " id: \"" << kMotorGroupId << "\"\n" - << " bus_type: MOTOR_BUS_MODBUS_TCP\n" - << " vendor: MOTOR_VENDOR_PLC_GENERIC\n" - << " protocol: MOTOR_PROTOCOL_CMVR_PLC_V1\n" - << " modbus_tcp {\n" - << " host: \"127.0.0.1\"\n" - << " port: " << port << "\n" - << " unit_id: 1\n" - << " connect_timeout_ms: 200\n" - << " io_timeout_ms: 50\n" - << " heartbeat_period_ms: 20\n" - << " communication_watchdog_ms: 300\n" - << " status_poll_period_ms: 2\n" - << " reconnect_min_ms: 10\n" - << " reconnect_max_ms: 50\n" - << " command_ack_timeout_ms: 300\n" - << " cyclic_watchdog_ms: 300\n" - << " protocol_major: 1\n" - << " protocol_minor: 0\n" - << " axes { motor_id: 1 axis_index: 0 }\n" - << " }\n" - << " joint_limits {\n" - << " enable: true\n" - << " source: JOINT_LIMIT_SOURCE_CUSTOM\n" - << " joints {\n" - << " joint_name: \"" << kJointName << "\"\n" - << " q_lb: -2.0\n" - << " q_ub: 2.0\n" - << " qd: 2.0\n" - << " qdd: 4.0\n" - << " }\n" - << " }\n" - << " motors {\n" - << " motors { id: 1 joint_name: \"" << kJointName << "\" }\n" - << " }\n" - << " }\n" - << "}\n"; - output.close(); - if (!output) { - std::remove(path); - return {}; - } - return path; -} - -class MotorServiceModbusE2eTest : public ::testing::Test { -protected: - void SetUp() override - { - device::DeviceManager::destroyInstance(); - ASSERT_TRUE(plc_.start()); - - config_path_ = writeTemporaryMotorConfig(plc_.port()); - ASSERT_FALSE(config_path_.empty()); - - config::MotorRootConfig parsed; - ASSERT_TRUE(ConfigHelper::loadConfigFileSilent(config_path_, parsed)); - ASSERT_EQ(parsed.motor().id(), kManagerId); - ASSERT_EQ(parsed.motor().motor_groups_size(), 1); - - config::DeviceManagerConfig device_config; - device_config.set_init_all_motors_when_no_active_joints(true); - auto* entry = device_config.add_devices(); - entry->set_id(kManagerId); - entry->set_type( - config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); - entry->set_config_file(config_path_); - entry->set_enable(true); - - auto& device_manager = - device::DeviceManager::getInstance(device_config); - manager_ = - device_manager.getDevice(kManagerId); - ASSERT_NE(manager_, nullptr); - ASSERT_NE(manager_->getMotor(1), nullptr); - ASSERT_EQ(manager_->getMotor(1)->jointName(), kJointName); - - service_ = std::make_unique(); - ASSERT_TRUE(startGrpcServer_()); - } - - void TearDown() override - { - if (grpc_server_) { - grpc_server_->Shutdown(); - grpc_server_->Wait(); - grpc_server_.reset(); - } - stub_.reset(); - service_.reset(); - - if (manager_) { - manager_->stop(); - manager_.reset(); - } - device::DeviceManager::destroyInstance(); - plc_.stop(); - - if (!grpc_socket_path_.empty()) { - std::remove(grpc_socket_path_.c_str()); - grpc_socket_path_.clear(); - } - if (!config_path_.empty()) { - std::remove(config_path_.c_str()); - config_path_.clear(); - } - } - - static api::MotorTarget makeTarget_() - { - api::MotorTarget target; - target.mutable_header()->set_device_id(kManagerId); - target.set_motor_id(1); - return target; - } - - bool startGrpcServer_() - { - grpc_socket_path_ = - "/tmp/cmvr_motor_service_modbus_e2e_" + - std::to_string(static_cast(::getpid())) + ".sock"; - std::remove(grpc_socket_path_.c_str()); - const std::string server_address = "unix:" + grpc_socket_path_; - - grpc::ServerBuilder builder; - builder.AddListeningPort( - server_address, grpc::InsecureServerCredentials()); - builder.RegisterService(service_.get()); - grpc_server_ = builder.BuildAndStart(); - if (!grpc_server_) { - return false; - } - stub_ = api::MotorService::NewStub( - grpc::CreateChannel( - server_address, - grpc::InsecureChannelCredentials())); - return stub_ != nullptr; - } - - FakeCmvrPlc plc_; - std::shared_ptr manager_; - std::unique_ptr service_; - std::unique_ptr grpc_server_; - std::unique_ptr stub_; - std::string config_path_; - std::string grpc_socket_path_; -}; - -TEST_F(MotorServiceModbusE2eTest, - StubTraversesManagerAndModbusRuntimeWithSafetyAndReconnect) -{ - api::SetMotorEnabledRequest enable_request; - *enable_request.mutable_target() = makeTarget_(); - enable_request.set_enabled(true); - grpc::ClientContext enable_context; - enable_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse enable_response; - const auto enable_status = - stub_->setEnabled(&enable_context, enable_request, &enable_response); - ASSERT_TRUE(enable_status.ok()) << enable_status.error_message(); - ASSERT_TRUE(enable_response.header().success()) - << enable_response.header().error_message(); - EXPECT_EQ(plc_.enableCount(), 1U); - - api::ProfilePositionRequest position_request; - *position_request.mutable_target() = makeTarget_(); - position_request.set_target_position_rad(0.75); - position_request.set_max_velocity_rad_s(0.5); - position_request.set_acceleration_rad_s2(1.0); - position_request.mutable_wait()->set_timeout_ms(1000); - position_request.mutable_wait()->set_poll_period_ms(2); - position_request.mutable_wait()->set_settle_sample_count(1); - grpc::ClientContext position_context; - position_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse position_response; - const auto position_status = stub_->profilePosition( - &position_context, position_request, &position_response); - ASSERT_TRUE(position_status.ok()) << position_status.error_message(); - ASSERT_TRUE(position_response.header().success()) - << position_response.header().error_message(); - EXPECT_NEAR(position_response.status().position_rad(), 0.75, 1e-6); - EXPECT_TRUE(position_response.status().target_reached()); - EXPECT_EQ(plc_.profilePositionCount(), 1U); - - api::GetMotorStatusRequest get_status_request; - *get_status_request.mutable_target() = makeTarget_(); - grpc::ClientContext get_status_context; - get_status_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::GetMotorStatusResponse get_status_response; - const auto get_status = stub_->getStatus( - &get_status_context, get_status_request, &get_status_response); - ASSERT_TRUE(get_status.ok()) << get_status.error_message(); - ASSERT_TRUE(get_status_response.header().success()) - << get_status_response.header().error_message(); - EXPECT_EQ(get_status_response.status().motor_id(), 1U); - EXPECT_EQ(get_status_response.status().joint_name(), kJointName); - EXPECT_EQ(get_status_response.status().run_mode(), - msgs::RUN_MODE_PROFILE_POSITION); - EXPECT_NEAR(get_status_response.status().position_rad(), 0.75, 1e-6); - - api::ProfileVelocityRequest velocity_request; - *velocity_request.mutable_target() = makeTarget_(); - velocity_request.set_target_velocity_rad_s(-0.2); - velocity_request.set_acceleration_rad_s2(0.5); - velocity_request.mutable_wait()->set_timeout_ms(1000); - velocity_request.mutable_wait()->set_poll_period_ms(2); - velocity_request.mutable_wait()->set_settle_sample_count(1); - grpc::ClientContext velocity_context; - velocity_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse velocity_response; - const auto velocity_status = stub_->profileVelocity( - &velocity_context, velocity_request, &velocity_response); - ASSERT_TRUE(velocity_status.ok()) << velocity_status.error_message(); - ASSERT_TRUE(velocity_response.header().success()) - << velocity_response.header().error_message(); - EXPECT_NEAR(velocity_response.status().velocity_rad_s(), -0.2, 1e-6); - EXPECT_EQ(plc_.profileVelocityCount(), 1U); - - const auto quick_stops_before_stream = plc_.quickStopCount(); - grpc::ClientContext stream_context; - stream_context.set_deadline( - std::chrono::system_clock::now() + 3s); - auto stream = stub_->streamCyclicPosition(&stream_context); - ASSERT_NE(stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget_(); - open_request.mutable_open()->set_watchdog_timeout_ms(500); - ASSERT_TRUE(stream->Write(open_request)); - - api::CyclicControlResponse stream_response; - ASSERT_TRUE(stream->Read(&stream_response)); - ASSERT_TRUE(stream_response.header().success()) - << stream_response.header().error_message(); - EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest setpoint_request; - auto* setpoint = setpoint_request.mutable_setpoint(); - setpoint->set_sequence(1); - setpoint->set_target_position_rad(0.25); - setpoint->set_target_velocity_rad_s(0.1); - ASSERT_TRUE(stream->Write(setpoint_request)); - - stream_response.Clear(); - ASSERT_TRUE(stream->Read(&stream_response)); - ASSERT_TRUE(stream_response.header().success()) - << stream_response.header().error_message(); - EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_APPLIED); - EXPECT_EQ(stream_response.sequence(), 1U); - EXPECT_FALSE(stream_response.has_status()); - EXPECT_EQ(plc_.cyclicOpenCount(), 1U); - EXPECT_EQ(plc_.cyclicSampleCount(), 1U); - - ASSERT_TRUE(stream->WritesDone()); - stream_response.Clear(); - ASSERT_TRUE(stream->Read(&stream_response)); - EXPECT_TRUE(stream_response.header().success()) - << stream_response.header().error_message(); - EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_STOPPED); - EXPECT_FALSE(stream->Read(&stream_response)); - const auto stream_finish = stream->Finish(); - ASSERT_TRUE(stream_finish.ok()) << stream_finish.error_message(); - EXPECT_GT(plc_.quickStopCount(), quick_stops_before_stream); - - const auto quick_stops_before_emergency = plc_.quickStopCount(); - api::EmergencyStopRequest emergency_request; - *emergency_request.mutable_target() = makeTarget_(); - grpc::ClientContext emergency_context; - emergency_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse emergency_response; - const auto emergency_status = stub_->emergencyStop( - &emergency_context, emergency_request, &emergency_response); - ASSERT_TRUE(emergency_status.ok()) << emergency_status.error_message(); - ASSERT_TRUE(emergency_response.header().success()) - << emergency_response.header().error_message(); - EXPECT_TRUE(emergency_response.status().emergency_stopped()); - EXPECT_GT(plc_.quickStopCount(), quick_stops_before_emergency); - - const auto commands_before_rejected_motion = plc_.commandCount(); - api::ProfilePositionRequest rejected_request; - *rejected_request.mutable_target() = makeTarget_(); - rejected_request.set_target_position_rad(0.5); - rejected_request.set_max_velocity_rad_s(0.5); - rejected_request.set_acceleration_rad_s2(1.0); - grpc::ClientContext rejected_context; - rejected_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse rejected_response; - const auto rejected_status = stub_->profilePosition( - &rejected_context, rejected_request, &rejected_response); - EXPECT_EQ(rejected_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(rejected_response.header().success()); - EXPECT_EQ(plc_.commandCount(), commands_before_rejected_motion); - - const auto sessions_before_disconnect = - plc_.acceptedSessionCount(); - const auto commands_before_disconnect = plc_.commandCount(); - plc_.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return plc_.acceptedSessionCount() > - sessions_before_disconnect; - }, - 3s)); - std::this_thread::sleep_for(100ms); - EXPECT_EQ(plc_.commandCount(), commands_before_disconnect); - - api::SetMotorEnabledRequest reenable_request; - *reenable_request.mutable_target() = makeTarget_(); - reenable_request.set_enabled(true); - grpc::ClientContext reenable_context; - reenable_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse reenable_response; - const auto reenable_status = stub_->setEnabled( - &reenable_context, reenable_request, &reenable_response); - ASSERT_TRUE(reenable_status.ok()) << reenable_status.error_message(); - EXPECT_TRUE(reenable_response.header().success()) - << reenable_response.header().error_message(); - EXPECT_EQ(plc_.commandCount(), commands_before_disconnect + 1U); - EXPECT_FALSE(reenable_response.status().emergency_stopped()); -} - -TEST_F(MotorServiceModbusE2eTest, - ActiveCyclicStreamFailsClosedAcrossReconnectAndNewStreamRecovers) -{ - grpc::ClientContext old_context; - old_context.set_deadline(std::chrono::system_clock::now() + 8s); - auto old_stream = stub_->streamCyclicPosition(&old_context); - ASSERT_NE(old_stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget_(); - open_request.mutable_open()->set_watchdog_timeout_ms(1000); - ASSERT_TRUE(old_stream->Write(open_request)); - - api::CyclicControlResponse response; - ASSERT_TRUE(old_stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest first_sample; - first_sample.mutable_setpoint()->set_sequence(1); - first_sample.mutable_setpoint()->set_target_position_rad(0.1); - first_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); - ASSERT_TRUE(old_stream->Write(first_sample)); - response.Clear(); - ASSERT_TRUE(old_stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); - ASSERT_EQ(plc_.cyclicOpenCount(), 1U); - ASSERT_EQ(plc_.cyclicSampleCount(), 1U); - - const auto accepted_sessions_before = plc_.acceptedSessionCount(); - plc_.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return plc_.acceptedSessionCount() > - accepted_sessions_before; - }, - 3s)); - - const auto opens_after_reconnect = plc_.cyclicOpenCount(); - const auto samples_after_reconnect = plc_.cyclicSampleCount(); - const auto stops_after_reconnect = plc_.quickStopCount(); - api::CyclicPositionRequest stale_sample; - stale_sample.mutable_setpoint()->set_sequence(2); - stale_sample.mutable_setpoint()->set_target_position_rad(0.2); - stale_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); - ASSERT_TRUE(old_stream->Write(stale_sample)); - - response.Clear(); - ASSERT_TRUE(old_stream->Read(&response)); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); - EXPECT_NE(response.header().error_message().find( - "rejected cyclic position setpoint"), - std::string::npos); - EXPECT_FALSE(old_stream->Read(&response)); - const auto old_finish = old_stream->Finish(); - EXPECT_TRUE( - old_finish.error_code() == grpc::StatusCode::FAILED_PRECONDITION || - old_finish.error_code() == grpc::StatusCode::CANCELLED) - << old_finish.error_message(); - // Stream cleanup may commit a safety QuickStop, but the stale generation - // must never commit an Open or sample into the new PLC session. - EXPECT_EQ(plc_.cyclicOpenCount(), opens_after_reconnect); - EXPECT_EQ(plc_.cyclicSampleCount(), samples_after_reconnect); - EXPECT_GT(plc_.quickStopCount(), stops_after_reconnect); - - grpc::ClientContext new_context; - new_context.set_deadline(std::chrono::system_clock::now() + 5s); - auto new_stream = stub_->streamCyclicPosition(&new_context); - ASSERT_NE(new_stream, nullptr); - ASSERT_TRUE(new_stream->Write(open_request)); - - response.Clear(); - ASSERT_TRUE(new_stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest fresh_sample; - fresh_sample.mutable_setpoint()->set_sequence(1); - fresh_sample.mutable_setpoint()->set_target_position_rad(0.3); - fresh_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); - ASSERT_TRUE(new_stream->Write(fresh_sample)); - response.Clear(); - ASSERT_TRUE(new_stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); - EXPECT_EQ(plc_.cyclicOpenCount(), opens_after_reconnect + 1U); - EXPECT_EQ(plc_.cyclicSampleCount(), samples_after_reconnect + 1U); - - ASSERT_TRUE(new_stream->WritesDone()); - response.Clear(); - ASSERT_TRUE(new_stream->Read(&response)); - EXPECT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_STOPPED); - EXPECT_FALSE(new_stream->Read(&response)); - const auto new_finish = new_stream->Finish(); - EXPECT_TRUE(new_finish.ok()) << new_finish.error_message(); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp index 79ecf8f5..df4a56fe 100644 --- a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp @@ -309,7 +309,7 @@ protected: std::string grpc_socket_path_; }; -TEST_F(MotorServiceTest, SetZeroAckLossReportsUnknownOutcome) +TEST_F(MotorServiceTest, SetZeroBackendFailureReportsUnknownOutcome) { protocol_->calibrate_success_ = false; @@ -323,7 +323,7 @@ TEST_F(MotorServiceTest, SetZeroAckLossReportsUnknownOutcome) EXPECT_FALSE(response.header().success()); EXPECT_NE(response.header().error_message().find("outcome unknown"), std::string::npos); - EXPECT_NE(response.header().error_message().find("zero_epoch"), + EXPECT_NE(response.header().error_message().find("device state"), std::string::npos); EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); } diff --git a/docs/motor_service_modbus_tcp.md b/docs/motor_service_modbus_tcp.md deleted file mode 100644 index 623aa0a3..00000000 --- a/docs/motor_service_modbus_tcp.md +++ /dev/null @@ -1,1022 +0,0 @@ -# MotorService 与 Modbus TCP PLC 电机控制 - -本文说明当前 `MotorService`、CMVR PLC v1 寄存器协议和 Siemens -S7-1215C DC/DC/DC 侧的对接方法。本文只描述当前源码已经存在的接口和 -协议；寄存器表中标为“预留”或“当前未发送”的内容，不代表上层已经开放。 - -## 1. 当前范围 - -当前实现是一条刻意保持简单的单轴控制链： - -```text -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` 必须且只能设置一个： - -```text -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 - -当前有两个双向流： - -```proto -rpc streamCyclicPosition(stream CyclicPositionRequest) - returns (stream CyclicControlResponse); - -rpc streamCyclicVelocity(stream CyclicVelocityRequest) - returns (stream CyclicControlResponse); -``` - -首帧必须是 `open`： - -```text -open.target 选择一个 MotorManager 中的单个电机 -open.watchdog_timeout_ms gRPC 层输入 watchdog -``` - -watchdog 为 `0` 时默认 500 ms，非零值被限制到 20 ms 至 60000 ms。打开 -成功后，服务端先返回： - -```text -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 后返回： - -```text -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`，内容如下： - -```textproto -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： - -```textproto -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 位有符号或无符号值均使用： - -```text -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` 的基地址： - -```text -B(i) = 100 + 128 * i -``` - -每轴占 128 个 Holding Registers： - -```text -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` 定义： - -```text -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` 定义： - -```text -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： - -```text -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 的协议数据块，例如： - -```scl -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` 实例。典型参数 -包括： - -```text -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 侧应显式实现以下等价逻辑： - -```text -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 状态机： - -```text -无 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： - -```text -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`。标准构建流程： - -```bash -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 电机链路时，可执行： - -```bash -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。 - -运行安装产物： - -```bash -./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 后，可先确认服务和状态： - -```bash -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/驱动器允许使能后，显式使能并执行一个同步位置命令： - -```bash -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： - -```bash -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 的普通软件停止不具备功能安全等级。 diff --git a/protos/cmvr/api/motor_command.proto b/protos/cmvr/api/motor_command.proto index a02657d5..a15c1811 100644 --- a/protos/cmvr/api/motor_command.proto +++ b/protos/cmvr/api/motor_command.proto @@ -101,7 +101,7 @@ message SetMotorEnabledRequest { message CyclicStreamOpen { MotorTarget target = 1; - // The PLC/driver watchdog is authoritative. This service watchdog prevents a + // The device/driver watchdog is authoritative. This service watchdog prevents a // stalled gRPC client from retaining control indefinitely. uint32 watchdog_timeout_ms = 2; } diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index 6ec4ec3b..322debf2 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -76,35 +76,13 @@ message MujocoMotorGroupConfig { string world_id = 1; } -message ModbusTcpAxisConfig { - int32 motor_id = 1; - uint32 axis_index = 2; -} - -message ModbusTcpConfig { - string host = 1; - uint32 port = 2; - uint32 unit_id = 3; - uint32 connect_timeout_ms = 4; - uint32 io_timeout_ms = 5; - uint32 heartbeat_period_ms = 6; - uint32 communication_watchdog_ms = 7; - uint32 status_poll_period_ms = 8; - uint32 reconnect_min_ms = 9; - uint32 reconnect_max_ms = 10; - uint32 command_ack_timeout_ms = 11; - uint32 protocol_major = 12; - uint32 protocol_minor = 13; - uint32 cyclic_watchdog_ms = 14; - repeated ModbusTcpAxisConfig axes = 20; -} - enum MotorBusType { MOTOR_BUS_UNKNOWN = 0; MOTOR_BUS_CAN = 1; MOTOR_BUS_ETHERCAT = 2; MOTOR_BUS_MUJOCO = 3; - MOTOR_BUS_MODBUS_TCP = 4; + reserved 4; + reserved "MOTOR_BUS_MODBUS_TCP"; } enum MotorVendor { @@ -112,7 +90,8 @@ enum MotorVendor { MOTOR_VENDOR_TI5 = 1; MOTOR_VENDOR_MUJOCO = 2; MOTOR_VENDOR_EYOU = 3; - MOTOR_VENDOR_PLC_GENERIC = 4; + reserved 4; + reserved "MOTOR_VENDOR_PLC_GENERIC"; } enum MotorProtocol { @@ -120,10 +99,14 @@ enum MotorProtocol { MOTOR_PROTOCOL_CANOPEN = 1; MOTOR_PROTOCOL_ETHERCAT_CIA402 = 2; MOTOR_PROTOCOL_MUJOCO = 3; - MOTOR_PROTOCOL_CMVR_PLC_V1 = 4; + reserved 4; + reserved "MOTOR_PROTOCOL_CMVR_PLC_V1"; } message MotorGroupConfig { + reserved 13; + reserved "modbus_tcp"; + string id = 1; MotorBusType bus_type = 2; MotorVendor vendor = 3; @@ -133,7 +116,6 @@ message MotorGroupConfig { SocketCanConfig can = 10; EtherCATConfig ethercat = 11; MujocoMotorGroupConfig mujoco = 12; - ModbusTcpConfig modbus_tcp = 13; } JointLimitsConfig joint_limits = 30;