XGO Duck 双核通信与控制循环分析
Arduino Uno Q · STM32 (Zephyr RTOS) + Linux (Python FastAPI) · 15 自由度四足机器人
一、双核通信原理
架构总览
Arduino Uno Q 是一块双核板：STM32 核心 运行 Zephyr RTOS + Arduino Sketch，负责实时硬件控制；Linux 核心 运行 Python FastAPI 应用，负责策略推理和 Web 界面。两者通过 Bridge RPC 机制 进行通信。
Linux → STM32 (命令/指令)
STM32 → Linux (状态/反馈)
SCS 舵机总线
I2C (IMU)
MsgPack RPC 封装层
物理链路
| 层面 | 说明 |
|---|
| 串口 | UART 串口，波特率 2 Mbps（BRIDGE_BAUD = 2000000） |
| 路由守护 | Linux 侧 arduino-router 守护进程，将 Unix socket 桥接到 /dev/ttyHS1 |
| 硬件 FIFO | STM32U5 UART FIFO 默认未启用，2 Mbps 下每字节仅 5µs，必须先开 FIFO（uart_fifo.h 中 enableUartFifo()） |
| 波特率配置 | router-2m.sh 通过 systemd override 将 arduino-router 配置为 2 Mbps |
RPC 协议栈与关键补丁
通信建立在 Arduino 官方的 RouterBridge + RPClite + MsgPack 库之上，但做了两处关键补丁（prepare-bridge.py）：
| 补丁 | 问题 | 修复 |
|---|
| RouterBridge 栈扩容 |
RPC 线程栈仅 500 字节，二进制 payload 比文本 RPC 更大，会栈溢出 |
将 UPDATE_THREAD_STACK_SIZE 从 500 → 4096 字节 |
| 有界 MsgPack 帧解析 |
原始 RPClite 的 MsgPack::Unpacker 对不完整二进制数据不安全且频繁分配内存 |
用 duck_msgpack_frame.h 替换 parse_packet()：先无分配扫描出完整 MessagePack 对象字节数，再交给 Unpacker 解析；超限或非法直接丢弃 |
有界帧解析原理：duckMsgpackFrame() 是一个状态机，逐字节解析 MessagePack 类型前缀，计算 fixstr/bin/array/map 等容器及其子元素的字节数，递归深度 ≤16，总大小 ≤ DECODER_BUFFER_SIZE/4。返回完整对象长度（正数）、不完整（0）或非法（-1）。只有完整帧才传入 Unpacker，杜绝了部分数据导致的内存安全问题。
二、数据通道与二进制 ABI
通道总览
通信通道通过 Bridge.provide() / Bridge.notify() 注册，每个通道承载一个固定大小的 packed 结构体（protocol.h / wire.py），两端用相同的 struct 布局 + magic 字节校验：
| 通道名 | 方向 | 结构体 | 大小 | magic | 用途 |
|---|
duck_command | L→S | CommandPacket | 80B | DQC1 | 关节目标角度 + 模式 (ARM/DISARM/POLICY/HOLD) |
duck_state | S→L | StatePacket | 240B | DQS1 | IMU数据 + 15关节角度/速度/目标 + 状态标志 |
duck_raw | S→L | RawPacket | 38B | DQR2 | 舵机原始编码器位置（标定时每tick发，平时每20帧） |
duck_cal | L→S | CalPacket | 10B | DQL2 | 标定指令（进入/退出/单关节居中） |
duck_cal_reply | S→L | CalReplyPacket | 10B | DQA1 | 标定结果回复 |
duck_servo | L→S | ServoPacket | 12B | DQV2 | 舵机调试指令（改ID/设增益/Ping/复位中位） |
duck_servo_reply | S→L | ServoReplyPacket | 14B | DQS3 | 舵机调试结果回复 |
duck_boot_pd | S→L | 32B (原始) | 32B | — | 启动时读取的舵机永久 P/D 增益，每100帧发一次 |
CommandPacket (80 字节)
// Little endian, packed
struct CommandPacket {
char magic[4]; // "DQC1"
uint32_t seq; // 命令序列号
uint32_t sourceUs; // 源时间戳(µs)
uint32_t ttlUs; // 生存时间
uint8_t mode; // DISARM/ARM/POLICY/HOLD
uint8_t reserved;
uint16_t reserved2;
float target[15]; // 关节目标角度(°)
};
StatePacket (240 字节)
struct StatePacket {
char magic[4]; // "DQS1"
uint8_t version, flags;// 状态位
uint16_t servoMask; // 在线舵机位图
uint32_t seq, sampleUs;
uint32_t imuUs, commandSeq;
uint32_t commandAgeUs;
uint32_t periodUs, workUs;
uint32_t overruns, reportDrops;
uint32_t invalidCommands;
int16_t acc[3], gyro[3];
float angle[15]; // 关节角度
float velocity[15];// 关节速度
float target[15]; // 当前目标
};
STM32 线程模型与同步原语
| 线程/对象 | 类型 | 说明 |
|---|
| controlThread | 优先级4 · 栈4096 | 100Hz 实时控制循环，舵机读写 + IMU 读取 |
| reportThread | 优先级6 · 栈4096 | 状态上报循环，阻塞在信号量上 |
| 主线程 (loop) | Arduino loop | 仅 delay(100)，Bridge RPC 回调在此执行 |
commandMu | k_mutex | 保护 latest/pending 命令缓冲 |
stateMu + reportReady | k_mutex + k_sem | controlTask 产出 state 后 give 信号量，reportTask take 后发出 |
motorsMu | k_mutex | 保护 motors[] 数组（controlTask 与 Bridge 回调共享） |
hostMu | k_mutex | 保护 cal/servo 调试指令的挂起缓冲 |
三、端到端控制流水线：从按钮点击到舵机运动
本节追踪从用户在浏览器中点击「Walk / get up」按钮、拖动摇杆，到最终舵机执行动作的完整调用链。整个流水线跨越三个执行域：浏览器 JavaScript → Linux Python 服务 → STM32 Zephyr 固件，涉及 HTTP API、线程间通信、ONNX 推理、UART RPC 和 SCS 总线协议。
阶段 ① — 浏览器交互层 (app.js)
Step 1: 用户点击模式按钮
用户在控制页面点击「Walk / get up」按钮（data-mode="policy"）。app.js 第 13 行通过 querySelectorAll('[data-mode]') 绑定了所有模式按钮的 onclick 事件：
// app.js L12-13
async function setMode(mode) {
await post('mode', {mode}); // → POST /api/mode
}
document.querySelectorAll('[data-mode]').forEach(
b => b.onclick = () => setMode(b.dataset.mode));
post() 是统一的 HTTP 封装函数（L10），发送 POST /api/mode，body 为 {"mode":"policy"}。
Step 2: 摇杆 / 键盘运动指令（并行持续发送）
用户拖动左侧摇杆控制前进/侧移，右侧摇杆控制转向。joystick() 函数（L60）绑定 Pointer 事件：
// app.js L60-81 — 摇杆事件
zone.onpointerdown = e => {
pointer = e.pointerId;
move(e); // 更新 sticks[side] = {x, y}
sendCommand(); // 立即发送
};
zone.onpointermove = e => { if(e.pointerId===pointer) move(e); };
// 释放 → 归零 → sendCommand()
zone.onpointerup = release;
updateMotion()（L40）将摇杆值映射为 targetMotion = [vx, vy, ωz]，考虑死区（15%）和模式限制（普通行走 vs 轮式行走）。sendCommand()（L51）对运动做指数平滑（0.3*old + 0.7*target），然后 POST /api/command：
// app.js L51-58 — 发送运动指令
async function sendCommand() {
motion = motion.map((old,i) => .3*old + .7*targetMotion[i]);
await post('command', {
twist: [...motion], // [vx, vy, ωz]
head: [$('head0').value, ...],// [4个头部角度]
mouth: +$('mouth').value // 嘴部 0-30°
});
}
此外，app.js 第 107 行设有 50ms 心跳定时器：即使用户不操作摇杆，也每 50ms 自动重发当前运动指令，确保 Controller 的 web_time 心跳不会超时。键盘 WASD/QE 同样触发 sendCommand()。
阶段 ② — FastAPI 路由层 (main.py)
Step 3: HTTP 路由分发
FastAPI Web 服务器（端口 9527）在 main.py 中注册了所有 API 端点。两个关键入口：
// main.py L22-32
def mode(body: dict):
return controller.set_mode(body['mode'])
def command(body: dict):
return controller.command(**body)
ui.expose_api('POST', '/api/mode', mode)
ui.expose_api('POST', '/api/command', command)
路由层只做参数提取和异常包装，核心逻辑全部在 Controller 中。
阶段 ③ — Controller 方法 (controller.py)
Step 4: set_mode('policy') — 模式切换
Controller.set_mode()（L173）执行模式切换的核心逻辑：
// controller.py L173-216 — 简化
def set_mode(self, mode):
with self.lock:
// 校验: 需要有有效的 feedback
if mode in ('hold', 'policy'):
fb = self._require_feedback(0) // 检查 IMU & 舵机在线
self.generation += 1 // 版本号，用于丢弃过期推理结果
self.mode = mode // 'policy'
self.recovery.reset()
self.policy_needs_reset = True
// 关键: 标记需要 ARM (首次使能)
self.arm_pending = (fb is not None and not fb.enabled)
// 如果之前在标定/舵机调试模式，发送退出指令
if previous == 'calibrate':
self.bridge.notify('duck_cal', encode_cal(CAL_EXIT))
arm_pending 是关键标志：它告诉 50Hz 策略线程在下一次 step() 时发送 ARM 命令（而非 POLICY），并且目标角度设为当前测量角度，避免使能瞬间关节跳变。
Step 5: command() — 存储运动指令
Controller.command()（L327）只做校验和存储，不直接发送命令给 STM32：
// controller.py L327-348 — 简化
def command(self, twist=None, head=None, mouth=None):
// 校验范围: twist=[0.4,0.3,1.0], head=[1.0]×4, mouth=[0,30]
t = values(twist, [.4,.3,1.0])
h = values(head, [1.0]*4)
m = float(mouth)
with self.lock:
if self.mode in EXCLUSIVE_MODES:
raise ValueError('motion disabled')
if not self.pick_active:
self.twist = t // [vx, vy, ωz] — 写入共享变量
self.head = h // [4个头部角度]
self.mouth = m // 嘴部角度
self.web_time = time.monotonic() // 心跳
运动指令被存储在 self.twist / self.head / self.mouth 中，供 50Hz 策略线程在下次 step() 时读取。self.web_time 更新心跳时间戳——如果超过 1 秒没有新的 HTTP 请求，策略线程会自动清零运动指令。
阶段 ④ — 50Hz 策略推理线程 (controller.step)
Step 6: controller.step() — 核心推理循环
controller.run()（L622）启动一个独立的 daemon 线程 policy50，每 20ms 调用 step()。这是整个系统的核心决策点，完整流程如下：
// controller.py L477-595 — step() 核心流程
def step(self):
with self.lock:
// 1. 读取最新反馈 (来自 100Hz receive 回调)
fb, filt = self.feedback, self.filtered
age = time.monotonic() - self.rx_time
// 2. 安全检查: 反馈过期 → 自动降级
if mode in ('hold','policy'):
if not fb or age > .15 or not fb.imu_ok:
self.mode = 'shadow'; self.generation += 1
self.arm_pending = False
// → DISARM
elif time.monotonic() - self.web_time > 1.0:
self.twist = [0.0]*3 // Web 心跳超时 → 停止行走
// 3. 恢复状态机: 根据重力倾角判断跌倒/起身
changed = self.recovery.update(filt['grav'], now, valid)
self.policy_needs_reset |= changed
phase = self.recovery.phase // walk / default_pose / getup
// 4. 策略选择
if rolling: selected = self.roulade_policy
elif picking: selected = self.pick_policy
elif phase == 'walk':
selected = self.policy // walk 或 roller
else:
selected = self.getup_policy
// 5. 读取用户运动指令
twist, head, mouth = self.twist, self.head, self.mouth
inferring = mode in ('shadow','policy') and valid
// === 释放锁后执行 ONNX 推理 ===
if inferring:
with self.policy_lock:
if reset_policy: selected.reset_action_filter()
// 6. ONNX 推理
action = selected.infer(filt['grav'], filt['gyro_rad'],
filt['angles_deg'], filt['vels_deg'],
twist, head)
// 7. 动作 → 关节角度
target = selected.action_to_deg(action)
// 8. 编码并发送命令
with self.lock:
if self.generation != generation: return // 期间被停止
command_mode = {'shadow':DISARM, 'policy':POLICY}[mode]
if self.arm_pending:
command_mode = ARM
target = list(fb.angles) // 首次使能: 用当前角度
self.arm_pending = False
target[MOUTH_INDEX] = mouth
self.seq += 1
payload = encode_command(self.seq, fb.sample_us,
command_mode, target)
self.bridge.notify('duck_command', payload)
Step 7: ONNX 推理详解 (policy.infer)
Policy.infer()（policy.py L99）执行完整的神经网络前向传播：
// policy.py L89-113 — 简化
def infer(self, grav, gyro_rad, angles_deg, vels_deg, cmd, head):
// 1. 构建观测向量 (61维)
self.build_obs(grav, gyro_rad, angles_deg, vels_deg, cmd, head)
// obs[0:3] = 陀螺角速度(rad/s)
// obs[3:6] = 重力向量(归一化)
// obs[6:20] = 14关节角度(相对home pose, rad)
// obs[20:34] = 14关节速度(rad/s)
// obs[34:48] = 上一步动作(14维)
// obs[48:51] = twist [vx, vy, ωz]
// obs[51:55] = head [4维]
// obs[55:61] = zeros (占位)
// 2. ONNX 前向传播
action = self.session.run(self._outputs, self._feed)[0]
// 输入: [1, 61] float32
// 输出: [1, 14] float32 (弧度, 相对home pose)
// 3. 动作低通滤波 (分三段不同 α)
self.prev_action[:] = action
for sl, alpha in ((slice(0,5), 0.45), // 左腿
(slice(5,10), 0.2), // 头颈
(slice(10,14), 0.45)):// 右腿
self._act_filt[sl] = self._act_filt[sl]*alpha + action[sl]*(1-alpha)
return self._act_filt.copy()
action_to_deg()（rl_core.py L162）将 14 维弧度动作转换为 15 个关节角度：
// rl_core.py L162-169
def action_to_deg(action, mouth_deg=0):
q_deg = HOME_DEG + np.rad2deg(ACTION_SCALE * action)
// 14维 → 按 POLICY_INDEX 映射到 15 维
targets[POLICY_INDEX] = q_deg
targets[MOUTH_INDEX] = clip(mouth_deg, 0, 30)
return targets.tolist() // 15个角度(°)
Step 8: 编码 CommandPacket 并发送
wire.py 的 encode_command() 将 15 个角度打包为 80 字节的二进制结构体：
// wire.py L86-94
def encode_command(seq, sample_us, mode, targets, ttl_us=250000):
return COMMAND.pack(
b'DQC1', // magic 校验
seq, // 序列号 (单调递增)
sample_us, // 对应 feedback 的 sample_us
ttl_us, // 生存时间 250ms
mode, // DISARM=0/ARM=1/POLICY=2/HOLD=3
0, 0, // reserved
*targets // 15个 float 角度
)
// → Bridge.notify('duck_command', 80字节 payload)
Bridge.notify() 将二进制 payload 包装在 MsgPack RPC 信封中，通过 Unix socket 发送给 arduino-router，再通过 UART 传到 STM32。
阶段 ⑤ — STM32 执行层 (sketch.ino)
Step 9: receiveCommand() — 接收与校验
STM32 主线程的 Bridge RPC 回调 receiveCommand()（sketch.ino L44）接收到 80 字节 payload：
// sketch.ino L44-59 — 简化
static void receiveCommand(bytes) {
CommandPacket cmd;
memcpy(&cmd, bytes, sizeof(cmd));
// 校验: magic=="DQC1" · mode≤HOLD · ttlUs有效
// 校验: 15个target均为有限值
bool valid = (memcmp(cmd.magic, "DQC1", 4) == 0)
&& cmd.mode <= HOLD && cmd.ttlUs > 0;
for (i=0; i<15; ++i)
valid = valid && isfinite(cmd.target[i]);
k_mutex_lock(&commandMu, K_FOREVER);
if (valid) { latest = cmd; pending = true; }
else ++invalidCommands;
k_mutex_unlock(&commandMu);
}
命令暂存在 latest 中，等待 100Hz 控制循环读取。注意此回调运行在主线程，与 controlTask 通过 commandMu 互斥锁同步。
Step 10: controlTask 100Hz — 执行命令
controlTask（sketch.ino L294）在每个 10ms tick 中读取并执行命令：
// sketch.ino L324-368 — 简化
// 读取最新命令
k_mutex_lock(&commandMu, K_FOREVER);
if (pending) { cmd = latest; pending = false; }
k_mutex_unlock(&commandMu);
// 校验命令新鲜度 & 序列号
if (got && !busTaken) {
fresh = (start - cmd.sourceUs) <= MAX_SOURCE_AGE_US; // 150ms
ordered = (cmd.seq - acceptedSeq) > 0;
if (fresh && ordered) {
acceptedSeq = cmd.seq;
ttl = cmd.ttlUs;
// ARM 模式: 首次使能
if (cmd.mode == ARM && !enabled && imu.ok)
enabled = true; // 力矩使能!
// 写入目标角度到 motors[]
k_mutex_lock(&motorsMu, K_FOREVER);
for (i=0; i<15; ++i)
motors[i].desAngleDeg = cmd.target[i];
k_mutex_unlock(&motorsMu);
}
}
// 超时检测
if (enabled && (start-lastCommandUs > ttl || ...))
enabled = false; // 命令超时 → 自动禁用
// 写舵机目标
if (enabled) {
k_mutex_lock(&motorsMu, K_FOREVER);
sendMotorDesPos(); // 角度→编码器→SCS同步写
k_mutex_unlock(&motorsMu);
}
setTorqueEnable(enabled); // 力矩开关
Step 11: sendMotorDesPos() — 角度到舵机编码器
sendMotorDesPos()（scs_bus.h L125）将角度（度）转换为舵机编码器位置（0-4095），然后通过 SCS 同步写指令一次性发送给 15 个舵机：
// scs_bus.h L125-132
static void sendMotorDesPos() {
for (i=0; i<15; i++) {
// 角度 → 编码器位置
pos = 2047 + (desAngleDeg / 360 * 4095 * sign);
motors[i].desPos = satPos(pos); // 饱和到 [0,4095]
}
syncWriteGoalsFromDesPos();
}
// scs_bus.h L109-123 — SCS 同步写
static void syncWriteGoalsFromDesPos() {
// 构造 sync write 帧: 0xFE + 0x83
// FF FF FE [len] 83 [addr=0x2A] [6] [ID1 pos1 0 0] [ID2 pos2 0 0] ...
scsWriteBuf(0xFE, 0x83, sync, ...);
// Serial1.write() @ 1Mbps → 15个舵机同时收到目标
}
SCS 总线协议帧格式：0xFF 0xFF [ID] [Length] [Function] [Data...] [~Checksum]。同步写（0x83）使用广播 ID 0xFE，一条指令更新所有 15 个舵机的目标位置，总线效率最高。
Step 12: 反馈回路 — STM32 → Linux → 浏览器
命令执行后，controlTask 继续读取舵机反馈和 IMU 数据，组装 StatePacket 并通过 reportTask 发回 Linux：
// sketch.ino L369-398 — 简化
drainServoRx(); // 清空残余应答
servoRxMask = 0;
readAllState(); // SCS同步读 → 15个舵机位置+速度
updateImu(); // I2C读QMI8658 → acc+gyro
parseServoRx(); // 解析舵机应答
// 组装 StatePacket (240字节)
s.flags = imu.ok | (enabled<<1) | (timedOut<<2) | ...;
s.angle[i] = motors[i].fbAngleDeg;
s.velocity[i] = motors[i].fbVelDeg;
s.target[i] = motors[i].desAngleDeg;
// 通知 reportTask 发送
k_sem_give(&reportReady);
// → reportTask: Bridge.notify('duck_state', 240B)
Linux 端 controller.receive()（100Hz 回调）解码 StatePacket，经 SignalFilter 滤波后存入 self.feedback / self.filtered。50Hz 策略线程在下次 step() 时读取这些数据，形成闭环。
同时，浏览器每 250ms 轮询 GET /api/status，获取机器人状态（关节角度、IMU、恢复阶段等）并更新 UI。
完整时序总结：用户点击按钮 → HTTP 请求（~1ms）→ Controller 存储模式/指令（~0.1ms）→ 50Hz 策略线程在下个 20ms 周期读取 → ONNX 推理（~10-20ms）→ 编码 80B 命令 → UART 传输（~0.3ms）→ STM32 receiveCommand（~0.01ms）→ 下个 10ms tick 的 controlTask 执行 → SCS 同步写（~1ms）→ 舵机执行。从用户操作到舵机动作的总延迟约 30-50ms（主要取决于策略线程的 20ms 周期 + ONNX 推理时间）。
四、STM32 实时控制循环 (100Hz)
核心在 controlTask()，周期 TICK_US = 10000µs（10ms）。每个 tick 的执行流程如下：
五、Linux 策略推理循环 (50Hz)
核心在 Controller.run() → step()，周期 CONTROL_PERIOD = 0.02s：
六、跌倒恢复状态机
七、安全机制总结
STM32 侧 (硬件保护)
| 机制 | 阈值 | 动作 |
|---|
| 命令 TTL | 250ms | 禁用力矩 + timedOut |
| 源时间戳 | 150ms | 拒绝执行该命令 |
| 序列号 | 必须递增 | 乱序 → invalidCommands++ |
| IMU 健康 | 100ms | 无 IMU 数据 → 禁用 |
| IMU 使能门 | 50ms | ARM 时必须有新鲜 IMU |
| 总线独占 | CAL/SERVO | 禁止正常电机控制 |
| deadline 超时 | — | overruns++ + 补偿跳周期 |
| 上报背压 | — | reportTask 未消费 → reportDrops++ |
Linux 侧 (软件保护)
| 机制 | 阈值 | 动作 |
|---|
| 反馈过期 | 150ms | 降级 shadow (DISARM) |
| IMU 丢失 | — | 降级 shadow |
| Web 心跳 | 1s | 清零运动指令, 保持力矩 |
| 调试心跳 | 2s | 自动退出标定/舵机调试 |
| generation 校验 | — | ONNX 期间收到停止 → 丢弃过期动作 |
| 序列回绕 | gap > 2³¹ | 全状态重置 |
| 滤波器复位 | gap > 0.1s | 重力/陀螺/速度滤波器重置 |
| 异常捕获 | — | step() 异常 → mode='off' |
关键设计思想
- 频率分层：STM32 以 100Hz 保证硬件实时性（舵机同步读写 + IMU），Linux 以 50Hz 做策略推理（ONNX 单线程约 10-20ms），两者解耦，互不阻塞
- 安全优先：多层超时保护——命令 TTL、源时间戳、IMU 健康检查、Web 心跳、MCU watchdog，任何一层异常都自动 DISARM
- 无锁尽量少锁：STM32 用 Zephyr mutex 保护共享缓冲，Linux 用 RLock；策略推理在
policy_lock 内执行避免阻塞 100Hz 接收
- 二进制 ABI 零拷贝：packed struct + magic 校验 +
static_assert 大小检查，Python 端用对应 struct.Struct 格式串，两端 ABI 严格对齐
- 有界帧解析：自定义
duckMsgpackFrame() 在 RPClite 层之前做无分配、有界的 MsgPack 完整帧检测，杜绝部分数据导致的内存安全问题
- 预热机制：空闲时
_keep_warm_idle() 轮流预热非活跃 ONNX session，避免策略切换时的冷启动延迟
- ARM 平滑过渡：首次 ARM 使能时，目标角度设为当前测量角度而非策略输出，避免使能瞬间关节跳变
XGO Duck Runtime · Arduino Uno Q · STM32 + Linux 双核架构
基于 Microduck by Pollen Robotics · 100Hz 硬件回路 + 50Hz 策略推理