init pro_sdk

2026-06-20 20:02:36 +08:00
commit 5ecd422a59
49 changed files with 4226 additions and 0 deletions
--- a/docs/PROTOCOL.md
+++ b/docs/PROTOCOL.md
@@ -0,0 +1,145 @@
+# Go1 PRO 协议规格
+
+完全实测/抓包验证 (2026-06-20)。
+
+## LowCmd (主控 → MCU)
+
+**616 字节 Blowfish ECB 密文** (明文 616B, 加密后还是 616B)，通过 UDP 发到 `192.168.123.10:8007`。
+
+### 明文 616B 布局
+
+| 偏移 | 字节 | 字段 | 说明 |
+|-----|------|------|------|
+| 0..2 | 2 | head | `fe ef` |
+| 2..3 | 1 | levelFlag | `0xff` |
+| 3..4 | 1 | frameReserve | 0 |
+| 4..12 | 8 | SN | 全 0 (PRO 不填) |
+| 12..20 | 8 | version | 全 0 (PRO 不填) |
+| 20..22 | 2 | bandWidth | **`3a c0` (BE 字节序, 不是 LE!)** |
+| 22..562 | 540 | motorCmd[20] | 每电机 27B (mode + q + dq + tau + Kp + Kd + reserve) |
+| 562..566 | 4 | bms | 0 |
+| 566..606 | 40 | wirelessRemote | 0 |
+| 606..610 | 4 | reserve | 0 |
+| **610..612** | 2 | **固定填充** | **`00 00`** |
+| **612..616** | 4 | **CRC** | **`gen_crc(cmd[:612])` 裸 CRC, 不 XOR** |
+
+### MotorCmd 单电机 27B
+
+```
+0..1:  mode (uint8)
+1..5:  q (float, Unitree 自定义 hex 编码, 不是标准 IEEE 754)
+5..9:  dq (同上)
+9..11: tau (2B 自定义)
+11..13: Kp (2B 自定义)
+13..15: Kd (2B 自定义)
+15..27: reserve (12B = 3 × 4B)
+```
+
+各字段编解码见 `go1_pro_sdk/utils/common.py`。
+
+### 跟 EDU 版差异 (free-dog-sdk lowCmd)
+
+| 项 | EDU | PRO |
+|----|-----|-----|
+| 总长 | 614B | **616B** |
+| CRC 偏移 | 610..614 | **612..616** |
+| CRC 算法 | `encryptCrc(genCrc(cmd[:-6]))` (XOR 0xedcab9de) | **裸 `gen_crc(cmd[:612])`** |
+| bandWidth 字节序 | LE | **BE** |
+| SN/version | 用户提供 | 全 0 |
+| 是否加密 | 否 | **Blowfish ECB** |
+
+
+## LowState (MCU → 主控)
+
+**858 字节 Blowfish ECB 密文** (Blowfish 8B 块, 858 取 8 对齐 = 856B 解密)。
+
+解密后 807B 是有效 LowState，后面 49B 填充/MCU 内部。
+
+### 明文 807B 布局 (跟 free-dog-sdk 的 ucl/lowState.py 一致)
+
+| 偏移 | 字节 | 字段 |
+|-----|------|------|
+| 0..2 | 2 | head |
+| 2..3 | 1 | levelFlag |
+| 3..4 | 1 | frameReserve |
+| 4..12 | 8 | SN |
+| 12..20 | 8 | version |
+| 20..22 | 2 | bandWidth |
+| 22..75 | 53 | IMU (quaternion 16B + gyro 12B + accel 12B + rpy 12B + temp 1B) |
+| 75..715 | 640 | motorState[20] × 32B |
+| 715..739 | 24 | BMS |
+| 739..759 | 20 | footForce / footForceEst (混合) |
+| 759..799 | 40 | **wirelessRemote** (遥控器原始数据) |
+| 799..803 | 4 | reserve |
+| 803..807 | 4 | CRC |
+
+
+## Blowfish
+
+**ECB 模式**，**LE word 字节序** (Unitree ARM64 LDP 读取顺序，不是标准 BE Blowfish)。
+
+state (4168B) = P[18] + S0[256] + S1[256] + S2[256] + S3[256]，每项 4B uint32。
+
+### F 函数
+
+```python
+def F(X):
+    a = (X >> 24) & 0xff
+    b = (X >> 16) & 0xff
+    c = (X >> 8) & 0xff
+    d = X & 0xff
+    return ((((S0[a] + S1[b]) & 0xFFFFFFFF) ^ S2[c]) + S3[d]) & 0xFFFFFFFF
+```
+
+**注意 Python 运算符优先级**: `+` 比 `^` 高，必须完整括号否则结果错。
+
+### 16 轮 Feistel
+
+标准 Blowfish 算法 (见 `go1_pro_sdk/codec/blowfish.py`)，无定制。
+
+### 已知明密文对 (用于验证 state)
+
+```
+encrypt(0000000000000000) = 12aa0354236b66e3
+encrypt(feefff0004020305) = 5d9874ad7eee5851
+encrypt(0802010000010900) = c5ce0887ceb86f91
+```
+
+
+## 遥控器 wirelessRemote (40B)
+
+布局 (Unitree 公开 `xRockerBtnDataStruct`):
+
+| 偏移 | 字节 | 字段 |
+|-----|------|------|
+| 0..2 | 2 | head 0x55 0xAA |
+| 2..4 | 2 | btn bitmap (uint16 LE) |
+| 4..8 | 4 | lx (float, -1..+1) |
+| 8..12 | 4 | rx |
+| 12..16 | 4 | ry |
+| 16..20 | 4 | L2 (0..1, 模拟扳机) |
+| 20..24 | 4 | ly |
+| 24..40 | 16 | reserved |
+
+### 按键 bitmap
+
+| 位 | 按键 | 位 | 按键 |
+|----|------|----|------|
+| 0x0001 | R1 | 0x0100 | A |
+| 0x0002 | L1 | 0x0200 | B |
+| 0x0004 | START | 0x0400 | X |
+| 0x0008 | SELECT | 0x0800 | Y |
+| 0x0010 | R2 | 0x1000 | UP |
+| 0x0020 | L2 | 0x2000 | RIGHT |
+| 0x0040 | F1 | 0x4000 | DOWN |
+| 0x0080 | F2 | 0x8000 | LEFT |
+
+
+## MCU 客户端切换机制
+
+MCU 按"最近发包者的 IP:port"切换客户端。也就是说:
+- 我们 Mac 不需要绑特定端口，任意 OS 分配的端口都行
+- 一旦我们发包，MCU 把后续 LowState 回复给我们 (Legged_sport 不再收到)
+- Legged_sport 没收到 state 就停止 sendto，等价于"挂起"
+
+**含义**: 必须先在 Pi 上杀 `keep_sport_alive.sh`、`Legged_sport`、`appTransit` 三个抢占者，否则会跟 MCU 反复切换导致控制失败。