cpp sdk更新

add cpp benchmark
更新说明
2026-07-30 19:23:47 +08:00 · 2026-07-30 17:25:39 +08:00 · 2026-07-30 15:29:30 +08:00
12 changed files with 666 additions and 106 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -4,7 +4,7 @@ __pycache__/
 *.so
 .Python
 build/
-build-cpp*/
+build-*/
 CMakeFiles/
 dist/
 *.egg-info/
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(Boost REQUIRED)

 option(GO1_PRO_BUILD_EXAMPLES "Build portable official-API C++ examples" ON)
 option(GO1_PRO_BUILD_TESTS "Build C++ compatibility tests" ON)
+option(GO1_PRO_BUILD_BENCHMARKS "Build C++ codec benchmarks" OFF)

 add_library(unitree_legged_sdk
  src/loop.cpp
@@ -89,6 +90,19 @@ if(GO1_PRO_BUILD_TESTS)
  set_tests_properties(cpp_package_run PROPERTIES DEPENDS cpp_package_build)
 endif()

+if(GO1_PRO_BUILD_BENCHMARKS)
+  add_executable(benchmark_cpp_codec tests/cpp/benchmark_codec.cpp)
+  target_include_directories(benchmark_cpp_codec PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src)
+  set(GO1_PRO_BENCHMARK_CAPTURE
+      "${CMAKE_CURRENT_SOURCE_DIR}/data/captures/mcu_response_new.bin" CACHE FILEPATH
+      "Optional real-robot LowState packet used by the C++ codec benchmark")
+  if(EXISTS "${GO1_PRO_BENCHMARK_CAPTURE}")
+    target_compile_definitions(benchmark_cpp_codec PRIVATE
+      GO1_PRO_BENCHMARK_CAPTURE="${GO1_PRO_BENCHMARK_CAPTURE}")
+  endif()
+  target_link_libraries(benchmark_cpp_codec PRIVATE unitree_legged_sdk)
+endif()
+
 install(TARGETS unitree_legged_sdk EXPORT unitree_legged_sdkTargets
  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
--- a/README.md
+++ b/README.md
@@ -16,6 +16,26 @@ Unitree Go1 **PRO** 机型的低层电机控制 Python/C++ SDK。完整逆向 PR

 ## 快速上手

+### 语言与入口
+
+| 使用方式 | 源码位置 | 构建入口 | 用户入口 |
+|---|---|---|---|
+| Python 原生 PRO API | `go1_pro_sdk/` | `pyproject.toml` | `import go1_pro_sdk` |
+| Python 官方兼容 API | `robot_interface.py`、`go1_pro_sdk/compat/` | `pyproject.toml` | `import robot_interface as sdk` |
+| Python 可选原生加速 | `fast_lowcmd_cpp/` | `fast_lowcmd_cpp/setup.py` | `FastLowCmdBuilder` |
+| C++ 官方兼容 SDK | `include/`、`src/` | `CMakeLists.txt` | `#include <unitree_legged_sdk/unitree_legged_sdk.h>` |
+
+`fast_lowcmd_cpp` 是供 Python 调用的 CPython 扩展，不是 C++ 用户的公共 SDK。C++ 用户
+只依赖根目录 CMake 生成的 `unitree_legged_sdk` 库以及 `include/unitree_legged_sdk/`
+中的公共头文件。两套实现共享同一份 PRO 协议和 Blowfish state，但运行时不互相依赖。
+
+Python 开发环境安装：
+
+```bash
+conda activate free_dog_sdk
+python -m pip install -e .
+```
+
 ### 官方 Python SDK 兼容接口

 需要复用官方 `unitree_legged_sdk` Python 示例时，可以继续使用原来的模块名和
@@ -105,6 +125,96 @@ with MCUClient() as client:
    client.safe_stop()                       # 退出前发 damping
 ```

+## 调试与验证
+
+### 离线完整回归
+
+以下测试使用 fake socket 和 `data/captures/` 中的实机历史抓包，不连接机器狗，也不会向
+MCU 发送 UDP 数据。先在仓库根目录准备 Python 包和可选原生扩展：
+
+```bash
+conda run -n free_dog_sdk python -m pip install -e .
+cd fast_lowcmd_cpp
+PYTHONPATH=.. conda run -n free_dog_sdk python setup.py build_ext --inplace
+cd ..
+conda run -n free_dog_sdk python -m pytest -q tests fast_lowcmd_cpp/test_fast_lowcmd.py
+```
+
+C++ Release 回归会检查官方结构布局、PRO 明密文字节一致性、LowState 实机抓包、安全接口、
+Loop，以及安装后被独立 CMake 项目消费：
+
+```bash
+cmake -S . -B build-cpp -DCMAKE_BUILD_TYPE=Release \
+  -DGO1_PRO_CAPTURE_DIR="$PWD/data/captures"
+cmake --build build-cpp --parallel
+ctest --test-dir build-cpp --output-on-failure
+```
+
+配置阶段若显示 `Private data/captures fixtures not found`，说明只运行公共契约测试，未运行
+实机抓包字节回归。`ctest` 不会运行 `examples/` 中会发送命令的示例程序。
+
+### 定点调试
+
+Python 可用 pytest 节点路径只运行单项并显示输出；C++ 可直接运行兼容测试，或交给
+LLDB（macOS）/GDB（Linux）：
+
+```bash
+conda run -n free_dog_sdk python -m pytest -vv -s \
+  tests/test_capture_fixtures.py
+ctest --test-dir build-cpp -R cpp_official_compat -V
+lldb -- build-cpp/test_cpp_compat
+# Linux: gdb --args build-cpp/test_cpp_compat
+```
+
+需要验证另一份 Blowfish state 或外部抓包目录时：
+
+```bash
+GO1_PRO_BLOWFISH_STATE=/path/to/blowfish_state.bin ./build-cpp/test_cpp_compat
+cmake -S . -B build-cpp -DGO1_PRO_CAPTURE_DIR=/path/to/captures
+cmake --build build-cpp --target test_cpp_compat --parallel
+```
+
+### C++ 编译与运行时检查
+
+先用严格警告检查可移植性；ASan/UBSan 用于检查越界、生命周期和未定义行为，TSan 单独
+构建，不能与 ASan 混用：
+
+```bash
+cmake -S . -B build-cpp-warn -DCMAKE_BUILD_TYPE=Release \
+  -DCMAKE_CXX_FLAGS="-Wall -Wextra -Wpedantic"
+cmake --build build-cpp-warn --parallel
+ctest --test-dir build-cpp-warn --output-on-failure
+
+cmake -S . -B build-cpp-sanitize -DCMAKE_BUILD_TYPE=Debug \
+  -DCMAKE_CXX_FLAGS="-fsanitize=address,undefined -fno-omit-frame-pointer"
+cmake --build build-cpp-sanitize --parallel
+ctest --test-dir build-cpp-sanitize --output-on-failure
+
+cmake -S . -B build-cpp-tsan -DCMAKE_BUILD_TYPE=Debug \
+  -DCMAKE_CXX_FLAGS="-fsanitize=thread -fno-omit-frame-pointer"
+cmake --build build-cpp-tsan --parallel
+ctest --test-dir build-cpp-tsan --output-on-failure
+```
+
+### C++ 编解码基准
+
+基准默认不参与构建。存在 `data/captures/mcu_response_new.bin` 时会同时测量
+`EncodeLowCmd` 和 `DecodeLowState`；缺少抓包时只测编码：
+
+```bash
+cmake -S . -B build-cpp-benchmark -DCMAKE_BUILD_TYPE=Release \
+  -DGO1_PRO_BUILD_BENCHMARKS=ON
+cmake --build build-cpp-benchmark --target benchmark_cpp_codec --parallel
+./build-cpp-benchmark/benchmark_cpp_codec
+```
+
+### 实机烟雾验证
+
+自动测试通过后再进入下一节的实机流程。`monitor_state.py` 和 `monitor_remote.py` 虽不发送
+运动目标，但会执行 `wake_mcu()` 并持续发送 damping LowCmd 以维持回包，因此不是纯被动
+抓包；运行前仍须悬空机器狗、停掉抢占源，结束后恢复 sportMode。C++ position 示例会发送
+实际关节位置命令，只应按 [`docs/SAFETY.md`](docs/SAFETY.md) 的检查清单手动运行。
+
 ## 准备工作

 ### 1. 提取 Blowfish 密钥（仅需一次）
@@ -141,10 +251,10 @@ bash tools/stop_sportmode.sh stop
 ### 3. 跑示例

 ```bash
-# 只读监听 LowState
+# 状态监听（会发送 damping LowCmd，不是纯被动监听）
 python examples/monitor_state.py --duration 30 --verbose

-# 监听遥控器
+# 遥控器监听（会发送 damping LowCmd）
 python examples/monitor_remote.py

 # 单腿 sin 摆动 (狗悬空, 振幅 0.3 rad)
@@ -172,19 +282,36 @@ bash tools/stop_sportmode.sh start
 ## 包结构

 ```
-go1_pro_sdk/
-├── connection/   # MCUClient: 高层 UDP 客户端
-├── codec/        # Blowfish + LowCmd 序列化 + LowState 解析
-├── types/        # MotorCmd, LowState, IMU, BMS, RemoteState 等
-├── safety/       # PositionLimit / PowerProtect / PositionProtect
-├── utils/        # CRC, 浮点编解码, 关节常量
-└── _data/        # blowfish_state.bin
+go1_pro_sdk/                  # Python 原生 PRO SDK
+├── connection/              # MCUClient: UDP 客户端
+├── codec/                   # Python Blowfish、LowCmd、LowState
+├── compat/                  # 官方 robot_interface Python facade
+├── highlevel/               # sportMode MQTT API
+├── safety/                  # Python Safety 实现
+├── types/                   # Python 数据结构
+├── utils/                   # CRC、常量和字段编解码
+└── _data/                   # Python wheel 内的 Blowfish state

-include/unitree_legged_sdk/  # 官方 C++ 头文件兼容层
-src/                         # PRO UDP/Blowfish/Safety 实现
-tests/cpp/                   # C++ 契约、抓包和安装消费测试
+robot_interface.py           # 官方 Python SDK 同名顶层入口
+pyproject.toml               # Python 构建和安装入口
+fast_lowcmd_cpp/             # Python 可选 CPython 加速扩展
+
+include/unitree_legged_sdk/  # C++ 公共头文件，路径与官方一致
+src/                         # C++ 私有实现，不作为公共头文件安装
+cmake/                       # C++ find_package 配置模板
+CMakeLists.txt               # C++ 构建、测试和安装入口
+
+examples/*.py                # Python 示例
+examples/cpp/                # C++ 同源码兼容示例
+tests/*.py                   # Python、实机抓包和 facade 回归
+tests/cpp/                   # C++ 契约、安装消费测试与可选性能基准
+data/captures/               # 本机实机抓包，存在时自动参与回归
 ```

+Python 和 C++ 的公共 API 不交叉包含：Python 安装由 `pyproject.toml` 管理，不会安装
+C++ 头文件；C++ 安装由 CMake 管理，不会安装 Python 包。两边唯一共享的运行数据是
+`blowfish_state.bin`，安装时分别进入 Python package data 和 C++ data directory。
+
 ## 文档

 - `docs/PROTOCOL.md` — PRO LowCmd/LowState 字节级格式规格
--- a/docs/ARCHITECTURE.md
+++ b/docs/ARCHITECTURE.md
@@ -1,6 +1,23 @@
 # 包结构与数据流

-## 模块依赖图
+## 仓库分层
+
+本仓库同时提供 Python SDK 和独立 C++ SDK，两者按构建系统和公共入口区分：
+
+| 层 | Python | C++ |
+|---|---|---|
+| 公共 API | `go1_pro_sdk/`、`robot_interface.py` | `include/unitree_legged_sdk/` |
+| 私有实现 | Python package 子模块 | `src/` |
+| 构建入口 | `pyproject.toml` | `CMakeLists.txt` |
+| 示例 | `examples/*.py` | `examples/cpp/*.cpp` |
+| 测试 | `tests/*.py` | `tests/cpp/` |
+
+`fast_lowcmd_cpp/` 属于 Python 侧：它生成 CPython extension，用来加速 Python 的命令
+编解码和状态解析。它不提供 C++ 公共头文件，也不应被 C++ 应用直接链接。
+
+## Python 架构
+
+### 模块依赖图

 ```
                ┌──────────────────────┐
@@ -26,7 +43,7 @@
                └─────────────┘
 ```

-## 数据流: 收一帧 → 用户处理 → 发命令
+### 数据流: 收一帧 → 用户处理 → 发命令

 ```
   UDP 858B 密文                 UDP 616B 密文
@@ -64,16 +81,16 @@
 └─────────────────────────────────────────────────┘
 ```

-## 各包职责
+### 各包职责

-### utils/
+#### utils/

 最底层。纯函数, 无副作用, 无依赖其他模块。

 - `common.py`: CRC, float↔hex, tau↔2B, Kp↔2B, Kd↔2B, decode_sn/version
 - `constants.py`: MCU 地址, 关节命名, 限位, TAU_MAX, DAMPING_POSE

-### types/
+#### types/

 数据结构 (dataclass)。依赖 utils, 不依赖 codec/safety/connection。

@@ -84,7 +101,7 @@
 - `low_cmd.py`: LowCmd (12 motorCmd + 元数据)
 - `low_state.py`: LowState (12 motorState + IMU + BMS + remote 等)

-### codec/
+#### codec/

 加解密和序列化。依赖 types + utils。

@@ -92,21 +109,21 @@
 - `lowcmd_builder.py`: LowCmd → 616B 明文 → 加密后 616B
 - `lowstate_parser.py`: 解密后 807B → LowState 结构

-### safety/
+#### safety/

 控制保护层。依赖 types + utils。

 - `safety.py`: PositionLimit/PowerProtect/PositionProtect 三个保护 + 统一入口 apply_safety

-### connection/
+#### connection/

 UDP 高层抽象。依赖前面所有。

 - `mcu_client.py`: MCUClient (socket + Blowfish + 序列化 + safe_stop)

-## 用户三种用法
+### 用户三种用法

-### Level 1: 高层 (推荐)
+#### Level 1: 高层 (推荐)

 ```python
 from go1_pro_sdk import MCUClient, LowCmd, MotorCmd, MotorMode
@@ -120,7 +137,7 @@ with MCUClient() as client:
    client.safe_stop()
 ```

-### Level 2: 中层 (自己管 socket, 用编解码)
+#### Level 2: 中层 (自己管 socket, 用编解码)

 ```python
 from go1_pro_sdk import Blowfish, build_low_cmd_encrypted, parse_low_state, LowCmd
@@ -136,7 +153,49 @@ data, _ = sock.recvfrom(2048)
 state = parse_low_state(bf.decrypt_ecb(data[:856]))
 ```

-### Level 3: 底层 (诊断/逆向)
+#### Level 3: 底层 (诊断/逆向)

 直接用 `Blowfish.encrypt_block(b8)` / `Blowfish.decrypt_block(b8)`, 自己处理 8B 块。
 适合协议研究或字节级 diff。
+
+## C++ 架构
+
+```text
+应用源码
+  #include <unitree_legged_sdk/unitree_legged_sdk.h>
+                         │
+                         ▼
+include/unitree_legged_sdk/       公共、官方兼容声明
+  comm.h / udp.h / safety.h / loop.h / quadruped.h
+                         │
+                         ▼
+src/
+  udp.cpp       POSIX UDP、线程安全收发、官方方法适配
+  safety.cpp    PositionLimit/PowerProtect/PositionProtect
+  loop.cpp      Loop/LoopFunc 调度和 Linux CPU affinity
+  pro_codec.cpp 616B LowCmd、858B LowState、CRC、Blowfish
+  quadruped.cpp 版本、长度常量和 InitEnvironment
+```
+
+`src/pro_codec.h` 是库内部头文件，不安装给用户。C++ 应用只能包含
+`include/unitree_legged_sdk/` 下的头文件，这保证业务源码切换到官方 SDK 时不依赖 PRO
+专有声明。PRO 的加密和私有线协议全部封装在 `UDP` 实现内部。
+
+CMake 对外提供：
+
+- 构建树目标：`unitree_legged_sdk`；
+- namespaced alias：`unitree_legged_sdk::unitree_legged_sdk`；
+- 安装后的 `find_package(unitree_legged_sdk CONFIG REQUIRED)`；
+- 安装目录中的公共头文件、静态库、CMake config 和 Blowfish state。
+
+## 共享边界
+
+Python 与 C++ 没有运行时语言绑定关系，也不会互相调用。它们共享的是协议规格、实机
+抓包回归基准和 Blowfish state。修改协议实现时必须同时运行：
+
+```bash
+conda run -n free_dog_sdk python -m pytest -q tests fast_lowcmd_cpp/test_fast_lowcmd.py
+cmake -S . -B build-cpp -DCMAKE_BUILD_TYPE=Release
+cmake --build build-cpp
+ctest --test-dir build-cpp --output-on-failure
+```
--- a/examples/README.md
+++ b/examples/README.md
@@ -1,5 +1,12 @@
 # Examples

+## 目录划分
+
+- `examples/*.py`：Python 原生 PRO API 和 Python 官方兼容 facade 示例；
+- `examples/cpp/*.cpp`：独立 C++ SDK 示例，只使用官方 C++ 公共接口。
+
+`fast_lowcmd_cpp` 是 Python 可选加速扩展，不是另一套 C++ 示例入口。
+
 ## 高层 (HighLevel) — 通过 sportMode + MQTT, 不需停 Pi 进程

 适合: 走/跳/姿态/表演动作. 跟 lowlevel 控制方式互不兼容, 同时只用一种.
@@ -89,3 +96,16 @@ python examples/example_remote_control.py
 ## 安全

 所有控制类示例都内置 `apply_safety(cmd, state, power_factor=1)` (10% 力矩限制), 出问题会立即停机. 见 `docs/SAFETY.md`.
+
+## C++ 官方兼容示例
+
+[`cpp/example_official_compatible_position.cpp`](cpp/example_official_compatible_position.cpp)
+使用与官方 `unitree_legged_sdk` 相同的头文件、命名空间、`UDP` 和 `Safety` API。构建：
+
+```bash
+cmake -S . -B build-cpp -DCMAKE_BUILD_TYPE=Release
+cmake --build build-cpp --target example_official_compatible_position
+```
+
+该示例会先发送 stop sentinel，收到第一帧有效 `LowState` 后才启用位置目标。PRO 环境
+链接本项目库；官方 Go1 环境使用相同源码重新链接官方库。
--- a/fast_lowcmd_cpp/fast_mcu.py
+++ b/fast_lowcmd_cpp/fast_mcu.py
@@ -1,12 +1,12 @@
-"""Fast MCU client using native LowState decrypt/parse.
+"""Fast MCU client using native LowState decrypt/parse and UDP drain.

 The public shape mirrors go1_pro_sdk.connection.MCUClient for deployment code,
 while keeping the receive hot path out of pure Python.
 """
-import socket
 import time

 from fast_lowcmd import FastLowCmdBuilder, default_state_path
+from go1_fast_lowcmd import FastNativeMCUClient
 from go1_pro_sdk import MCU_IP, MCU_PORT
 from go1_pro_sdk.utils.constants import RCVBUF_SIZE

@@ -127,16 +127,19 @@ class FastMCUClient:
            raise ValueError("FastMCUClient currently supports only little-endian Blowfish state")
        self.mcu_ip = mcu_ip
        self.mcu_port = mcu_port
-        self.builder = FastLowCmdBuilder(state_path or default_state_path())
-
-        sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-        sock.setblocking(False)
-        sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, RCVBUF_SIZE)
-        sock.bind(("", local_port))
-        self.sock = sock
-        self.local_port = sock.getsockname()[1]
+        resolved_state_path = state_path or default_state_path()
+        self.builder = FastLowCmdBuilder(resolved_state_path)
+        self._native = FastNativeMCUClient(
+            resolved_state_path,
+            mcu_ip,
+            int(mcu_port),
+            int(local_port),
+            int(RCVBUF_SIZE),
+            endian,
+        )
+        self.local_port = self._native.local_port()
        self._last_state = None
-        self.backend = "cpp_lowcmd_cpp_lowstate"
+        self.backend = "cpp_lowcmd_cpp_lowstate_cpp_udp"

    def __enter__(self):
        return self
@@ -145,27 +148,16 @@ class FastMCUClient:
        self.close()

    def close(self):
-        if self.sock:
-            self.sock.close()
-            self.sock = None
+        native = getattr(self, "_native", None)
+        if native is not None:
+            native.close()
+            self._native = None

    def send_raw(self, raw_cipher):
-        self.sock.sendto(raw_cipher, (self.mcu_ip, self.mcu_port))
-        return len(raw_cipher)
+        return self._native.send_raw(raw_cipher)

    def recv_latest(self):
-        last_data = None
-        while True:
-            try:
-                data, _ = self.sock.recvfrom(2048)
-                last_data = data
-            except (BlockingIOError, socket.timeout):
-                break
-
-        if last_data is None:
-            return None
-
-        fields = self.builder.decrypt_lowstate(last_data)
+        fields = self._native.recv_latest_fields()
        if fields is None:
            return None
        self._last_state = FastLowState(fields)
--- a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp
+++ b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp
@@ -1,6 +1,7 @@
 #define PY_SSIZE_T_CLEAN
 #include <Python.h>
 #include <array>
+#include <cerrno>
 #include <cmath>
 #include <cstdio>
 #include <cstdint>
@@ -9,6 +10,12 @@
 #include <string>
 #include <vector>

+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
 namespace {

 constexpr Py_ssize_t LOWCMD_SIZE = 616;
@@ -460,6 +467,31 @@ struct FastLowCmdBuilder {
    uint32_t S[4][256];
 };

+bool load_blowfish_state(const char* state_path, uint32_t P[18], uint32_t S[4][256]) {
+    std::ifstream f(state_path, std::ios::binary);
+    if (!f) {
+        PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path);
+        return false;
+    }
+    std::array<uint8_t, 4168> buf{};
+    f.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(buf.size()));
+    if (f.gcount() < static_cast<std::streamsize>(buf.size())) {
+        PyErr_Format(PyExc_ValueError, "Blowfish state file must contain at least 4168 bytes: %s", state_path);
+        return false;
+    }
+    const uint8_t* p = buf.data();
+    for (int i = 0; i < 18; ++i) {
+        P[i] = get_u32_le(p + i * 4);
+    }
+    p += 18 * 4;
+    for (int s = 0; s < 4; ++s) {
+        for (int i = 0; i < 256; ++i) {
+            S[s][i] = get_u32_le(p + (s * 256 + i) * 4);
+        }
+    }
+    return true;
+}
+
 uint32_t bf_f(FastLowCmdBuilder* self, uint32_t x) {
    return (((self->S[0][(x >> 24) & 0xffu] + self->S[1][(x >> 16) & 0xffu]) & 0xffffffffu)
            ^ self->S[2][(x >> 8) & 0xffu]) + self->S[3][x & 0xffu];
@@ -623,28 +655,7 @@ int FastLowCmdBuilder_init(FastLowCmdBuilder* self, PyObject* args, PyObject* kw
        return -1;
    }

-    std::ifstream f(state_path, std::ios::binary);
-    if (!f) {
-        PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path);
-        return -1;
-    }
-    std::array<uint8_t, 4168> buf{};
-    f.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(buf.size()));
-    if (f.gcount() < static_cast<std::streamsize>(buf.size())) {
-        PyErr_Format(PyExc_ValueError, "Blowfish state file must contain at least 4168 bytes: %s", state_path);
-        return -1;
-    }
-    const uint8_t* p = buf.data();
-    for (int i = 0; i < 18; ++i) {
-        self->P[i] = get_u32_le(p + i * 4);
-    }
-    p += 18 * 4;
-    for (int s = 0; s < 4; ++s) {
-        for (int i = 0; i < 256; ++i) {
-            self->S[s][i] = get_u32_le(p + (s * 256 + i) * 4);
-        }
-    }
-    return 0;
+    return load_blowfish_state(state_path, self->P, self->S) ? 0 : -1;
 }

 PyObject* FastLowCmdBuilder_build_plain_damping(FastLowCmdBuilder*, PyObject*) {
@@ -865,27 +876,19 @@ PyObject* FastLowCmdBuilder_encrypt(FastLowCmdBuilder* self, PyObject* args) {
    return result;
 }

-PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* args) {
-    Py_buffer view;
-    if (!PyArg_ParseTuple(args, "y*", &view)) {
-        return nullptr;
-    }
-    if (view.len < 807) {
-        PyBuffer_Release(&view);
+PyObject* parse_lowstate_packet(FastLowCmdBuilder* self, const uint8_t* cipher, Py_ssize_t len) {
+    if (len < 807) {
        Py_RETURN_NONE;
    }
-    Py_ssize_t aligned = (view.len / 8) * 8;
+    Py_ssize_t aligned = (len / 8) * 8;
    if (aligned < 807) {
-        PyBuffer_Release(&view);
        Py_RETURN_NONE;
    }

-    const uint8_t* cipher = reinterpret_cast<const uint8_t*>(view.buf);
    std::vector<uint8_t> data(static_cast<size_t>(aligned));
    for (Py_ssize_t i = 0; i < aligned; i += 8) {
        decrypt_block_le(self, cipher + i, data.data() + i);
    }
-    PyBuffer_Release(&view);

    if (data[0] != 0xfe || data[1] != 0xef || data[2] != 0xff || data[3] != 0x00) {
        Py_RETURN_NONE;
@@ -983,6 +986,207 @@ PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject*
    return out;
 }

+PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* args) {
+    Py_buffer view;
+    if (!PyArg_ParseTuple(args, "y*", &view)) {
+        return nullptr;
+    }
+    PyObject* result = parse_lowstate_packet(
+        self, reinterpret_cast<const uint8_t*>(view.buf), view.len);
+    PyBuffer_Release(&view);
+    return result;
+}
+
+struct FastNativeMCUClient {
+    PyObject_HEAD
+    int sock_fd;
+    int local_port;
+    FastLowCmdBuilder codec;
+};
+
+void close_native_socket(FastNativeMCUClient* self) {
+    if (self->sock_fd >= 0) {
+        close(self->sock_fd);
+        self->sock_fd = -1;
+    }
+}
+
+PyObject* FastNativeMCUClient_new(PyTypeObject* type, PyObject*, PyObject*) {
+    auto* self = reinterpret_cast<FastNativeMCUClient*>(type->tp_alloc(type, 0));
+    if (self) {
+        self->sock_fd = -1;
+        self->local_port = 0;
+        std::memset(&self->codec, 0, sizeof(self->codec));
+    }
+    return reinterpret_cast<PyObject*>(self);
+}
+
+void FastNativeMCUClient_dealloc(FastNativeMCUClient* self) {
+    close_native_socket(self);
+    Py_TYPE(self)->tp_free(reinterpret_cast<PyObject*>(self));
+}
+
+int set_nonblocking(int fd) {
+    int flags = fcntl(fd, F_GETFL, 0);
+    if (flags < 0) {
+        return -1;
+    }
+    return fcntl(fd, F_SETFL, flags | O_NONBLOCK);
+}
+
+int FastNativeMCUClient_init(FastNativeMCUClient* self, PyObject* args, PyObject* kwargs) {
+    const char* state_path = nullptr;
+    const char* mcu_ip = "192.168.123.10";
+    const char* endian = "little";
+    int mcu_port = 8007;
+    int local_port = 0;
+    int rcvbuf_size = 4096;
+    static const char* kwlist[] = {
+        "state_path", "mcu_ip", "mcu_port", "local_port", "rcvbuf_size", "endian", nullptr};
+    if (!PyArg_ParseTupleAndKeywords(
+            args, kwargs, "s|siiis", const_cast<char**>(kwlist),
+            &state_path, &mcu_ip, &mcu_port, &local_port, &rcvbuf_size, &endian)) {
+        return -1;
+    }
+    if (std::strcmp(endian, "little") != 0) {
+        PyErr_SetString(PyExc_ValueError, "FastNativeMCUClient currently supports only little-endian Blowfish state");
+        return -1;
+    }
+    close_native_socket(self);
+    if (!load_blowfish_state(state_path, self->codec.P, self->codec.S)) {
+        return -1;
+    }
+
+    self->sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
+    if (self->sock_fd < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        return -1;
+    }
+    int reuse = 1;
+    setsockopt(self->sock_fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
+    if (rcvbuf_size > 0) {
+        setsockopt(self->sock_fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf_size, sizeof(rcvbuf_size));
+    }
+    if (set_nonblocking(self->sock_fd) < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        close_native_socket(self);
+        return -1;
+    }
+
+    sockaddr_in local{};
+    local.sin_family = AF_INET;
+    local.sin_addr.s_addr = htonl(INADDR_ANY);
+    local.sin_port = htons(static_cast<uint16_t>(local_port));
+    if (bind(self->sock_fd, reinterpret_cast<sockaddr*>(&local), sizeof(local)) < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        close_native_socket(self);
+        return -1;
+    }
+
+    sockaddr_in target{};
+    target.sin_family = AF_INET;
+    target.sin_port = htons(static_cast<uint16_t>(mcu_port));
+    if (inet_pton(AF_INET, mcu_ip, &target.sin_addr) != 1) {
+        PyErr_Format(PyExc_ValueError, "invalid IPv4 target: %s", mcu_ip);
+        close_native_socket(self);
+        return -1;
+    }
+    if (connect(self->sock_fd, reinterpret_cast<sockaddr*>(&target), sizeof(target)) < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        close_native_socket(self);
+        return -1;
+    }
+
+    sockaddr_in actual{};
+    socklen_t actual_len = sizeof(actual);
+    if (getsockname(self->sock_fd, reinterpret_cast<sockaddr*>(&actual), &actual_len) == 0) {
+        self->local_port = ntohs(actual.sin_port);
+    } else {
+        self->local_port = local_port;
+    }
+    return 0;
+}
+
+PyObject* FastNativeMCUClient_close(FastNativeMCUClient* self, PyObject*) {
+    close_native_socket(self);
+    Py_RETURN_NONE;
+}
+
+PyObject* FastNativeMCUClient_local_port(FastNativeMCUClient* self, PyObject*) {
+    return PyLong_FromLong(self->local_port);
+}
+
+PyObject* FastNativeMCUClient_send_raw(FastNativeMCUClient* self, PyObject* args) {
+    if (self->sock_fd < 0) {
+        PyErr_SetString(PyExc_RuntimeError, "FastNativeMCUClient is closed");
+        return nullptr;
+    }
+    Py_buffer view;
+    if (!PyArg_ParseTuple(args, "y*", &view)) {
+        return nullptr;
+    }
+    ssize_t sent = 0;
+    Py_BEGIN_ALLOW_THREADS
+    sent = send(self->sock_fd, view.buf, static_cast<size_t>(view.len), 0);
+    Py_END_ALLOW_THREADS
+    PyBuffer_Release(&view);
+    if (sent < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        return nullptr;
+    }
+    return PyLong_FromSsize_t(sent);
+}
+
+PyObject* FastNativeMCUClient_recv_latest_fields(FastNativeMCUClient* self, PyObject*) {
+    if (self->sock_fd < 0) {
+        PyErr_SetString(PyExc_RuntimeError, "FastNativeMCUClient is closed");
+        return nullptr;
+    }
+
+    std::array<uint8_t, 2048> buffer{};
+    std::array<uint8_t, 2048> latest{};
+    ssize_t latest_len = -1;
+    while (true) {
+        ssize_t received = 0;
+        Py_BEGIN_ALLOW_THREADS
+        received = recv(self->sock_fd, buffer.data(), buffer.size(), 0);
+        Py_END_ALLOW_THREADS
+        if (received > 0) {
+            latest_len = received;
+            std::memcpy(latest.data(), buffer.data(), static_cast<size_t>(received));
+            continue;
+        }
+        if (received == 0) {
+            break;
+        }
+        if (errno == EINTR) {
+            continue;
+        }
+        if (errno == EAGAIN || errno == EWOULDBLOCK) {
+            break;
+        }
+        PyErr_SetFromErrno(PyExc_OSError);
+        return nullptr;
+    }
+
+    if (latest_len < 0) {
+        Py_RETURN_NONE;
+    }
+    return parse_lowstate_packet(&self->codec, latest.data(), latest_len);
+}
+
+PyMethodDef FastNativeMCUClient_methods[] = {
+    {"close", reinterpret_cast<PyCFunction>(FastNativeMCUClient_close), METH_NOARGS,
+     "Close the native UDP socket."},
+    {"local_port", reinterpret_cast<PyCFunction>(FastNativeMCUClient_local_port), METH_NOARGS,
+     "Return the bound local UDP port."},
+    {"send_raw", reinterpret_cast<PyCFunction>(FastNativeMCUClient_send_raw), METH_VARARGS,
+     "Send an already encrypted LowCmd packet through the native UDP socket."},
+    {"recv_latest_fields", reinterpret_cast<PyCFunction>(FastNativeMCUClient_recv_latest_fields), METH_NOARGS,
+     "Drain pending UDP packets and return parsed fields for the newest LowState packet."},
+    {nullptr, nullptr, 0, nullptr},
+};
+
 PyMethodDef FastLowCmdBuilder_methods[] = {
    {"build_plain_damping", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_damping), METH_NOARGS,
     "Build a plain all-damping LowCmd packet."},
@@ -1015,6 +1219,10 @@ PyTypeObject FastLowCmdBuilderType = {
    PyVarObject_HEAD_INIT(nullptr, 0)
 };

+PyTypeObject FastNativeMCUClientType = {
+    PyVarObject_HEAD_INIT(nullptr, 0)
+};
+
 PyModuleDef module = {
    PyModuleDef_HEAD_INIT,
    "go1_fast_lowcmd",
@@ -1038,6 +1246,19 @@ PyMODINIT_FUNC PyInit_go1_fast_lowcmd(void) {
        return nullptr;
    }

+    FastNativeMCUClientType.tp_name = "go1_fast_lowcmd.FastNativeMCUClient";
+    FastNativeMCUClientType.tp_basicsize = sizeof(FastNativeMCUClient);
+    FastNativeMCUClientType.tp_flags = Py_TPFLAGS_DEFAULT;
+    FastNativeMCUClientType.tp_doc = "Native UDP MCU client with C++ LowState drain/decrypt/parse.";
+    FastNativeMCUClientType.tp_new = FastNativeMCUClient_new;
+    FastNativeMCUClientType.tp_init = reinterpret_cast<initproc>(FastNativeMCUClient_init);
+    FastNativeMCUClientType.tp_dealloc = reinterpret_cast<destructor>(FastNativeMCUClient_dealloc);
+    FastNativeMCUClientType.tp_methods = FastNativeMCUClient_methods;
+
+    if (PyType_Ready(&FastNativeMCUClientType) < 0) {
+        return nullptr;
+    }
+
    PyObject* m = PyModule_Create(&module);
    if (!m) {
        return nullptr;
@@ -1048,6 +1269,12 @@ PyMODINIT_FUNC PyInit_go1_fast_lowcmd(void) {
        Py_DECREF(m);
        return nullptr;
    }
+    Py_INCREF(&FastNativeMCUClientType);
+    if (PyModule_AddObject(m, "FastNativeMCUClient", reinterpret_cast<PyObject*>(&FastNativeMCUClientType)) < 0) {
+        Py_DECREF(&FastNativeMCUClientType);
+        Py_DECREF(m);
+        return nullptr;
+    }
    PyModule_AddIntConstant(m, "LOWCMD_SIZE", LOWCMD_SIZE);
    return m;
 }
--- a/fast_lowcmd_cpp/test_fast_lowcmd.py
+++ b/fast_lowcmd_cpp/test_fast_lowcmd.py
@@ -3,6 +3,8 @@ import os
 import pytest

 from fast_lowcmd import FastLowCmdBuilder, default_state_path
+from fast_mcu import FastMCUClient
+from go1_fast_lowcmd import FastNativeMCUClient
 from go1_pro_sdk import Blowfish, LowCmd, MotorCmd, MotorMode, PowerProtectViolation
 from go1_pro_sdk import apply_safety
 from go1_pro_sdk.codec.lowcmd_builder import build_low_cmd_encrypted, build_low_cmd_plain
@@ -140,9 +142,30 @@ def test_state_file_is_required():
        FastLowCmdBuilder(os.path.join(os.path.dirname(__file__), "missing_state.bin"))


+def test_native_mcu_client_empty_recv():
+    client = FastNativeMCUClient(default_state_path(), "127.0.0.1", 8007, 0, 4096, "little")
+    try:
+        assert client.local_port() > 0
+        assert client.recv_latest_fields() is None
+    finally:
+        client.close()
+
+
+def test_fast_mcu_uses_native_udp_client():
+    client = FastMCUClient(mcu_ip="127.0.0.1", mcu_port=8007, local_port=0)
+    try:
+        assert client.backend == "cpp_lowcmd_cpp_lowstate_cpp_udp"
+        assert client.local_port > 0
+        assert client.recv_latest() is None
+    finally:
+        client.close()
+
+
 def test_lowstate_decrypt_matches_python(builder, bf):
    root = os.path.dirname(os.path.dirname(__file__))
    packet_path = os.path.join(root, "data", "captures", "mcu_response.bin")
+    if not os.path.exists(packet_path):
+        pytest.skip("private LowState capture is not available")
    with open(packet_path, "rb") as f:
        packet = f.read()

--- a/src/pro_codec.cpp
+++ b/src/pro_codec.cpp
@@ -31,6 +31,22 @@ constexpr std::size_t kMotorOffset = 22;
 constexpr std::size_t kMotorWireSize = 27;
 constexpr std::size_t kCrcOffset = 612;
 constexpr uint32_t kCrcPoly = 0x04c11db7u;
+constexpr std::size_t kLowStateDecodeSize =
+    ((kLowStateParsedSize + 7) / 8) * 8;
+
+std::array<uint32_t, 256> MakeCrcTable() {
+  std::array<uint32_t, 256> table{};
+  for (uint32_t byte = 0; byte < 256; ++byte) {
+    uint32_t crc = byte << 24;
+    for (int bit = 0; bit < 8; ++bit) {
+      crc = (crc & 0x80000000u) ? (crc << 1) ^ kCrcPoly : crc << 1;
+    }
+    table[byte] = crc;
+  }
+  return table;
+}
+
+const std::array<uint32_t, 256> kCrcTable = MakeCrcTable();

 uint16_t GetU16Le(const uint8_t* p) {
  return static_cast<uint16_t>(p[0]) |
@@ -221,23 +237,13 @@ std::string ProCodec::FindStateFile() {
 }

 uint32_t ProCodec::Crc32(const uint8_t* data, std::size_t size) {
-  static const std::array<uint32_t, 256> table = [] {
-    std::array<uint32_t, 256> result{};
-    for (uint32_t byte = 0; byte < 256; ++byte) {
-      uint32_t crc = byte << 24;
-      for (int bit = 0; bit < 8; ++bit) {
-        crc = (crc & 0x80000000u) ? (crc << 1) ^ kCrcPoly : crc << 1;
-      }
-      result[byte] = crc;
-    }
-    return result;
-  }();
  uint32_t crc = 0xffffffffu;
  for (std::size_t offset = 0; offset + 4 <= size; offset += 4) {
    const uint32_t word = GetU32Le(data + offset);
-    for (int shift = 24; shift >= 0; shift -= 8) {
-      crc = (crc << 8) ^ table[((crc >> 24) ^ (word >> shift)) & 0xffu];
-    }
+    crc = (crc << 8) ^ kCrcTable[((crc >> 24) ^ (word >> 24)) & 0xffu];
+    crc = (crc << 8) ^ kCrcTable[((crc >> 24) ^ (word >> 16)) & 0xffu];
+    crc = (crc << 8) ^ kCrcTable[((crc >> 24) ^ (word >> 8)) & 0xffu];
+    crc = (crc << 8) ^ kCrcTable[((crc >> 24) ^ word) & 0xffu];
  }
  return crc;
 }
@@ -320,11 +326,9 @@ std::array<uint8_t, kLowCmdWireSize> ProCodec::EncodeLowCmd(

 bool ProCodec::DecodeLowState(const uint8_t* encrypted, std::size_t size,
                              UNITREE_LEGGED_SDK::LowState& state) const {
-  if (size < kLowStateParsedSize) return false;
-  const std::size_t aligned = (size / 8) * 8;
-  if (aligned < kLowStateParsedSize) return false;
-  std::vector<uint8_t> data(aligned);
-  Decrypt(encrypted, data.data(), aligned);
+  if (size < kLowStateDecodeSize) return false;
+  std::array<uint8_t, kLowStateDecodeSize> data;
+  Decrypt(encrypted, data.data(), data.size());
  if (data[0] != 0xfe || data[1] != 0xef ||
      data[2] != UNITREE_LEGGED_SDK::LOWLEVEL || data[3] != 0) {
    return false;
--- a/src/pro_codec.h
+++ b/src/pro_codec.h
@@ -7,7 +7,6 @@
 #include <cstddef>
 #include <cstdint>
 #include <string>
-#include <vector>

 namespace go1_pro_internal {

--- a/tests/cpp/benchmark_codec.cpp
+++ b/tests/cpp/benchmark_codec.cpp
@@ -0,0 +1,84 @@
+#include "pro_codec.h"
+#include "unitree_legged_sdk/unitree_legged_sdk.h"
+
+#include <chrono>
+#include <cstdint>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <iterator>
+#include <string>
+#include <vector>
+
+namespace {
+
+using Clock = std::chrono::steady_clock;
+
+#ifdef GO1_PRO_BENCHMARK_CAPTURE
+std::vector<uint8_t> ReadFile(const std::string& path) {
+  std::ifstream stream(path, std::ios::binary);
+  return std::vector<uint8_t>(
+      (std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>());
+}
+#endif
+
+double NanosecondsPerIteration(Clock::time_point start, Clock::time_point end,
+                               std::size_t iterations) {
+  return std::chrono::duration<double, std::nano>(end - start).count() /
+         static_cast<double>(iterations);
+}
+
+}  // namespace
+
+int main() {
+  constexpr std::size_t kIterations = 200000;
+  go1_pro_internal::ProCodec codec(go1_pro_internal::ProCodec::FindStateFile());
+
+  UNITREE_LEGGED_SDK::LowCmd cmd{};
+  go1_pro_internal::InitLowCmd(cmd);
+  volatile uint64_t checksum = 0;
+
+  for (std::size_t i = 0; i < 100; ++i) {
+    const auto packet = codec.EncodeLowCmd(cmd);
+    checksum += packet[i % packet.size()];
+  }
+  const auto encode_start = Clock::now();
+  for (std::size_t i = 0; i < kIterations; ++i) {
+    const auto packet = codec.EncodeLowCmd(cmd);
+    checksum += packet[i % packet.size()];
+  }
+  const auto encode_end = Clock::now();
+
+  std::cout << std::fixed << std::setprecision(1)
+            << "EncodeLowCmd: "
+            << NanosecondsPerIteration(encode_start, encode_end, kIterations)
+            << " ns/frame\n";
+
+#ifdef GO1_PRO_BENCHMARK_CAPTURE
+  const auto capture = ReadFile(GO1_PRO_BENCHMARK_CAPTURE);
+  if (capture.size() != go1_pro_internal::kLowStateDatagramSize) {
+    std::cerr << "unexpected benchmark capture size: " << capture.size() << '\n';
+    return 1;
+  }
+
+  UNITREE_LEGGED_SDK::LowState state{};
+  for (std::size_t i = 0; i < 100; ++i) {
+    if (!codec.DecodeLowState(capture.data(), capture.size(), state)) return 1;
+    checksum += state.motorState[i % state.motorState.size()].temperature;
+  }
+  const auto decode_start = Clock::now();
+  for (std::size_t i = 0; i < kIterations; ++i) {
+    if (!codec.DecodeLowState(capture.data(), capture.size(), state)) return 1;
+    checksum += state.motorState[i % state.motorState.size()].temperature;
+  }
+  const auto decode_end = Clock::now();
+  std::cout << "DecodeLowState: "
+            << NanosecondsPerIteration(decode_start, decode_end, kIterations)
+            << " ns/frame\n";
+#else
+  std::cout << "DecodeLowState: skipped (capture fixture unavailable)\n";
+#endif
+
+  std::cout << "checksum: " << checksum << '\n';
+  return 0;
+}
--- a/tests/cpp/test_compat.cpp
+++ b/tests/cpp/test_compat.cpp
@@ -101,6 +101,17 @@ int main() {
  CHECK(std::isfinite(state.motorState[FR_1].q));
  CHECK(state.wirelessRemote[0] == 0x55 && state.wirelessRemote[1] == 0x51);

+  LowState truncated_state{};
+  CHECK(!codec.DecodeLowState(state_packet.data(),
+                              go1_pro_internal::kLowStateParsedSize,
+                              truncated_state));
+  CHECK(codec.DecodeLowState(state_packet.data(),
+                             go1_pro_internal::kLowStateParsedSize + 1,
+                             truncated_state));
+  CHECK(truncated_state.motorState[FR_1].temperature ==
+        state.motorState[FR_1].temperature);
+  CHECK(truncated_state.crc == state.crc);
+
  const auto old_state_packet = ReadFile(GO1_PRO_TEST_CAPTURE_OLD);
  CHECK(old_state_packet.size() == go1_pro_internal::kLowStateDatagramSize);
  LowState old_state{};
Author	SHA1	Message	Date
cyy_mac	7e4f632268	cpp sdk更新	2026-07-30 19:23:47 +08:00
cyy_mac	bd07331734	add cpp benchmark	2026-07-30 17:25:39 +08:00
cyy_mac	3008203f82	更新说明	2026-07-30 15:29:30 +08:00