四足开发实战（六）：固件篇——1 kHz 控制环与任务架构

上一篇（四足开发实战（五）：电子篇——DM-MC02 主控与 RS485 总线）结束时，12 个关节已全部在线，总线 CRC 错误小于万分之一。到这一步，真正的难点才刚开始——编写四足固件的难点不在于算法本身，而在于难以定位某一次任务抖动或某一次半双工切换失误，及其导致机器跌倒的具体时机。本篇说明这套固件的整体架构：任务划分方式、1 kHz 主循环的编写、17 字节协议的接口封装，以及将安全机制落实为具体结构的方法。

一张任务表装下整个固件

首先分层，这是后续所有决策的基础。固件分为五层：app 层负责状态机与上位机命令，control 层负责估计、运动学、步态与关节控制，driver 层负责协议打包与传感器，bsp 层负责 HAL 与寄存器，utils 层负责环形缓冲、CRC、日志。五层关系如图，所有箭头均只有一个方向：

flowchart TB
    subgraph APP["app 应用层"]
        SM["state_machine"]
        HCMD["host_cmd / rc"]
        GCMD["gait_cmd"]
    end
    subgraph CTRL["control 控制层"]
        EST["state_estimator"]
        KIN["kinematics"]
        JC["joint_controller"]
        MPC["mpc / wbc（预留）"]
    end
    subgraph DRV["driver 驱动层"]
        UM["rs485_motor"]
        IMU["icm42688"]
        BUS["rs485_if"]
        LINK["host_link"]
    end
    subgraph BSP["bsp 板级层"]
        UART["uart + dma"]
        DE["de / re 方向控制"]
        SPI["spi + dma"]
        TIME["dwt / tim"]
        FLASH["flash"]
    end
    subgraph UTIL["utils 工具层"]
        RB["ringbuf"]
        CRC["crc16"]
        LOG["log"]
        PARAM["param"]
    end
    APP --> CTRL
    CTRL --> DRV
    DRV --> BSP
    DRV --> UTIL
    CTRL --> UTIL

该图表达两层含义：调用只能向下，公共设施挂在 utils 且 utils 不依赖任何上层；横向之间不允许跳层，driver 不 include app，control 不接触 HAL。具体规则整理为四条：

1. 上层只能调下层的函数，不能反向；driver 不 include app 的头文件。
2. 只有 bsp 能碰 HAL 和寄存器，driver 只调 bsp_ 前缀的接口。
3. 跨层数据用结构体值传递，不用全局裸变量；共享数据加锁或走队列。
4. 配置全部集中在 cfg/ 里，代码里不出现魔法数。

依赖方向依靠 CMake 的 include 路径在编译期强制执行：每层单独设置 target_include_directories，driver 的 include 列表仅含 bsp 与 utils，任一模块 include 错层即直接编译报错，比口头约定更为可靠。错误处理也有约定：bsp 返回 HAL 状态，driver 统一转为整数错误码，control 只关心 online 与反馈值，app 做最终决策——将来更换总线、更换电机型号时，改动均被限制在 driver 内。举一个具体例子：RS485 发帧超时，bsp_uart 返回 HAL_TIMEOUT，rs485_if 转为负错误码，rs485_motor 将该电机的 online 清除，control 看到 online=0 即将该腿的力矩缓慢归零，app 最后决定报警或停机，错误码全程只朝一个方向传递，查日志时无需猜测。这套分层有一项实际好处：做仿真在环时只需将 driver 换为一组仿真桩，control 一行不改，同一套控制代码可先在 MuJoCo 中运行、再移植到真机，这是在真机上调参数而不损伤机器的保障。

仿真桩的实现也较简单：driver 中另设一套 sim 实现，motor_set_cmd 将 12 个关节指令经共享内存交给仿真进程，motor_read_fb 读回 MuJoCo 计算好的位置、速度与力矩，时间戳字段同样保留。真机与仿真的唯一差别即此层，其余四层为同一份源码，仅编译目标不同；反之，真机上出现异常现象时，也可将同一段控制输入回放到仿真中复现，先排除算法问题再检查硬件。

再往上即任务的划分。原则只有一条：频率越高、实时性越强，优先级越高，因此保护任务置于最高优先级，急停与摔倒检测不能被任何业务阻塞。时间基准统一使用 DWT 的微秒计数，不应混用 xTaskGetTickCount 与 HAL_GetTick，两者在中断中可能不同步，会导致日志无法对齐。下面是完整的任务表，共十个任务：

任务	优先级	周期	栈（word）	职责	通信方式
task_safety	8	1 ms	512	限幅、心跳、急停、摔倒检测	事件组
task_imu	7	1 ms	512	SPI 读 IMU，发快照	队列（深 4）
task_control	6	1 ms	2048	估计→状态机→步态→关节指令	队列 + 事件组
task_rs485_1	6	1 ms	512	电机 1~6 半双工收发（DMA）	信号量
task_rs485_2	6	1 ms	512	电机 7~12 半双工收发（DMA）	信号量
task_est	5	2 ms	1024	姿态/速度估计（EKF）	队列
task_comm	4	10 ms	1024	上位机协议收发	队列 + UART DMA
task_canfd	4	10 ms	512	可选：遥控/传感器/上位机备用	队列
task_monitor	3	50 ms	512	电压/温度/栈/CPU，喂狗	共享只读
task_log	1	20 ms	2048	组帧、环形缓冲、DMA 发送	队列

两路 RS485 各占一个任务的原因由上一篇的带宽核算决定：每电机每周期 17 字节控制加 16 字节反馈共 33 字节，8N1 每字节 10 bit，一路 6 个电机 198 B/ms、1.98 Mbps，占 4 Mbps 的 49.5%；12 个电机全挂一路为 3.96 Mbps、99%，没有重发余量。因此固定为两路 × 6 关节、4 Mbps、1 kHz，任务划分只是使该口径得以实现。

优先级的取值并非随意安排，遵循两条推理线。其一为安全优先：急停、摔倒、通信超时等事件必须立即响应，因此 task_safety 独占最高优先级 8，任何业务均无法抢占。其二为控制优先于观测与输出：task_control 是 1 kHz 的核心，优先级 6；两个 RS485 任务与其同级，因为二者仅负责 DMA 数据搬运，实际仲裁依靠信号量，均不应长期占用 CPU；task_est 以 500 Hz 提供状态，低一档让出时间片；task_comm、task_monitor 依次降低，task_log 降至最低，日志永远不能反向阻塞控制。据此安排，控制任务最坏情况下的抢占延迟来自 task_safety 与两个 RS485 任务，前者单次执行小于 10 µs，后者单次 DMA 交互小于 20 µs，合计远小于 1 ms 预算。

每个任务的边界应严格划定：task_safety 只读急停 GPIO、查询摔倒标志、遍历 12 个电机的超时与温度，不直接写电机，仅置事件位，由控制任务统一执行卸力，避免两个任务争用总线；task_imu 等待数据就绪中断，SPI DMA 读取 14 字节，换算为物理量并带时间戳推进队列，队列满即覆盖旧值，绝不阻塞；task_est 以 500 Hz 取最新快照运行 EKF，输出姿态、速度与摔倒标志，不直接读取任何硬件；task_comm 解析上位机帧，CRC 错误的只计数不回 NAK，避免错误帧引发应答风暴；task_canfd 为可选任务，板上三路 FDCAN 不接电机，留给遥控、扩展传感器与上位机备用，不使用则关闭以节省 512 word 栈；task_monitor 采集电压温度、检查各任务栈水位与心跳，所有心跳到齐才喂独立看门狗。任务之间的数据流仅四条：IMU 队列（深 4）、日志队列（深 16）、命令队列（深 8）与一个事件组（急停、摔倒、通信超时各占一位）。队列满的原则是控制链路丢旧保新，日志链路可丢弃但需计数，任何情况下均不应阻塞控制任务。浮点任务的栈从 2048 word 起步，启动时给每个栈填入 0xAA，运行三十分钟统计剩余量，低于三成即增加栈，不应等到 HardFault 才补充。

控制任务写给 RS485 任务的指令不走队列，而走双缓冲：g_cmd_buf[2][12] 加一个 volatile 索引，控制任务写完交换索引，RS485 任务读取当前缓冲。双缓冲本身不能防止半帧——真正起兜底作用的是 seqlock：写方先加序号、写数据、再加序号，读方前后序号一致才采用这一帧，不一致则丢弃并统计一次 old_frame。该细节虽小，但"关节保持上一拍指令"与"指令突然跳变"两类异常现象多半源于此；优先级反转是另一个易错点，共享资源必须使用带优先级继承的 mutex，以二值信号量充当锁的代码在日志繁忙时会使控制任务阻塞数十毫秒，进而导致机器跌倒。

FreeRTOS 的配置项中仅几处需要特别说明：tick 设为 1000 Hz，周期任务必须使用 vTaskDelayUntil 而非 vTaskDelay，后者会累积漂移；开启栈溢出检查（configCHECK_FOR_STACK_OVERFLOW=2），启用 mutex 优先级继承。H7 上还有三个特有易错点：一是中断中不应调用 HAL_Delay、printf 这类会阻塞的接口，ISR 只置标志发通知，繁重工作交由任务处理；二是 D-Cache 与 DMA 的一致性，开启 D-Cache 后 DMA 接收的数据可能为旧值、发送的帧会缺一段，DMA 缓冲区应放置于非缓存段，或在收发前后做 Cache 失效与回写；三是浮点任务的栈，FPU 上下文加局部矩阵很容易打穿按整数估算的栈，表现为随机 HardFault 而单步正常，因此浮点任务栈从 2048 word 起步，配合 0xAA 水位统计，不应等到故障才补充。

1 kHz 主循环与半双工时序

1 kHz 主循环是本固件的核心，代码不长，逻辑须保持清晰。每拍先以零超时读取 IMU 队列，取不到即用上一帧，绝不阻塞；随后执行一步估计、一步状态机、行走态跑步态，关节控制器计算 12 个指令，发布到双缓冲，最后记录一个时间戳，用 vTaskDelayUntil 精确对齐下一拍。实测约 305 µs——估计 90 µs、步态加运动学 150 µs、关节 PD 40 µs，余量 65%，留给后续 MPC。

for (;;) {
    uint32_t t0 = dwt_us();
    imu_sample_t imu;
    if (xQueuePeek(q_imu, &imu, 0) != pdTRUE) imu = g_imu_last; /* 绝不阻塞 */
    g_imu_last = imu;
    est_step(&g_est, &imu, g_joint_fb, 0.001f);
    sm_step(&g_sm, &g_est, &g_host_cmd);
    if (sm_state(&g_sm) == ST_WALK) gait_step(&g_gait, &g_host_cmd, 0.001f);
    joint_ctl_step(&g_jc, &g_gait, g_joint_fb, 0.001f);
    cmd_buf_publish(&g_jc);          /* 双缓冲交换，RS485 任务下一拍取 */
    log_mark_control(t0, dwt_us());
    vTaskDelayUntil(&last_wake, period);
}

这段代码只有两点需要解释。其一是 xQueuePeek 加零超时：peek 只读不取，IMU 任务照常向队列放入新值，控制任务始终取得最新一帧；取不到时用 g_imu_last 顶替一拍的旧数据，最坏情况是姿态估计滞后一个 IMU 周期，换来的是控制环绝不因等待而丢拍。其二是估计与控制的配合：task_est 那个 500 Hz 任务负责运行完整的 EKF 迭代，收敛零偏与协方差；此处的 est_step 是控制侧逐拍推进的轻量步骤，用最新 IMU 快照与上一拍关节反馈将状态外推至当前时刻。两者共享同一个估计器结构体，一个负责滤波收敛，一个负责逐拍递推，1 kHz 的循环无需等待 EKF 完整迭代。

305 µs 的分解见下列预算表（示例值，实测后替换）：

段	耗时	说明
取 IMU + 估计	90 µs	EKF 15 维
状态机	10 µs	纯分支
步态 + 运动学	150 µs	12 腿 FK/雅可比
关节 PD + 限幅	40 µs	12 关节
发布指令（双缓冲）	5 µs	指针交换
日志打点	10 µs	只写时间戳
合计	约 305 µs	余量 65%

RS485 收发完全不计入控制任务的开销：两个 RS485 任务各约 50 µs CPU、510 µs 墙钟，依靠 DMA 让出 CPU 与控制任务交错执行，这是跑满 1 kHz 的关键。控制任务自身不等待任何资源：队列零超时、日志只写指针，某拍超过 1.2 ms 即记 ERR_OVERRUN，连续十次自动降至 500 Hz 并报警——宁可降频，也不应丢拍仍强行运行。

抖动需量化验收，而非凭"感觉不卡"。控制任务每拍翻转一个 GPIO，用示波器观察周期直方图；或记录 tick 与 DWT 的差值，标准差小于 20 µs 即视为合格。抖动较大时应先检查临界区长度与 Flash 等待周期，再检查是否有任务在控制任务之前长时间关闭中断。

半双工时序是这套固件中最精细的部分，此处单独说明。一个电机的完整周期为：发 17 字节 → 等 TC → 切收 → 收 16 字节 → 校验，往返时序如下：

sequenceDiagram
    participant C as task_control
    participant R as task_rs485
    participant U as UART+DMA
    participant M as 关节电机
    C->>R: 双缓冲发布 12 关节指令
    loop 每路 6 个电机
        R->>U: 关接收，DE=1
        R->>U: DMA 发 17 B（42.5 µs）
        U->>M: 控制帧
        R->>U: 等 TC（最后一位发完）
        R->>U: 切收 DE=0，留 0.5 µs 保护
        M->>U: 16 B 反馈（40 µs）
        U-->>R: IDLE 中断判帧尾
        R->>R: 长度 + CRC 校验
    end
    R-->>C: 反馈表与统计

三个细节至关重要。第一，切接收必须等待 TC（发送完成标志）而非 TXE——TXE 仅表示数据寄存器已空、数据进入移位寄存器，最后几位仍在线上，此时将 DE 拉低切收，最后一个字节会被截掉，电机收到一帧缺尾的错误帧。第二，发送期间应关闭接收，否则 17 字节回显会被收进，前 16 字节恰好凑成"一帧"——长度吻合、CRC 全错，现象与真帧一致，最难排查；发前关闭接收、发完清 FIFO 再开启。第三，切换方向后留 1~2 个位时间的保护间隔，约 0.5 µs，待最后一个边沿稳定后再开始等待接收。

接收端使用 UART 的 IDLE 中断判定帧尾，而非"收满 16 字节"。反馈帧虽为定长，但总线上的干扰与回显会使数据错位，按字节数收满会把错位数据当作一帧，并连带下一帧出错；IDLE 在总线空闲满一个字符时间时才触发，天然即为帧边界，之后再校验长度与 CRC，错帧直接丢弃。解析放在任务中执行，中断内只做拷贝与计数，17 字节的 CRC 若在中断中计算会占用数十微秒。

实现上即打开 UART 的 IDLE 中断：DMA 缓冲按 16 字节配置，正常情况下收满中断先到，IDLE 紧随其后确认帧尾；若干扰或回显使数据错位，收满中断到来时帧尚未结束，此时不做处理，待总线静默满一个字符时间，IDLE 中断记录实际收到的字节数，任务再按该长度校验。该机制对回显的容错尤为关键：上一拍的回显未清干净时，按"收满即帧"的写法会把回显当作反馈，IDLE 加长度校验可将错帧挡在解析之前。清 FIFO 的时机也固定：每次切到接收方向之前清一次，DMA 计数器同时复位，保证下一帧从零字节开始接收。

每个电机的时间核算：发 17 字节约 42.5 µs、收 16 字节约 40 µs、切换加保护 1 µs，单个约 85 µs，六个约 510 µs 墙钟，但大部分时间在等待 DMA，CPU 每路仅 50 µs。两路各开一个任务才能并行跑满 1 kHz，放入同一个任务串行会拖至 1 ms 以上。超时处理也需明确，每个电机维护一个重发状态机：IDLE → SEND → WAIT_TC → RECV → CHECK 五步循环，任一阶段 200 µs 超时即回 IDLE 重发一次，同一拍两次失败置 online=0 由保护层卸力，下一拍继续尝试，恢复后自动回 online=1；重发次数与超时次数分开统计，不应只记录笼统的"通信故障"。最后补充广播的易错点：ID 15 是广播地址，所有电机执行但不回复，用于扫描只会等待超时，广播仅用于急停同步，发完延时 1 ms 再恢复轮询。

将两路的时间轴并列观察更为直观：1 ms 内 task_rs485_1 与 task_rs485_2 各自发起一轮 6 电机轮询，墙上各占约 510 µs，互不等待；DMA 工作的空隙恰好由 task_control 的 305 µs 计算与 task_est 的滤波占用。因此 CPU 占用应按任务分开统计——只看总占用 40% 会误以为余量充足，实际控制任务的窗口已经较紧。

电机抽象层：上层只调 motor_set_cmd

协议本身需封装为上层易用的接口。上层只使用物理量与归一化增益，不接触 17 字节帧：力矩为 q8（乘 256）、速度 q7（乘 128）、位置 q15（乘 32768），Kp 与 Kd 为 0 到 1 的归一化值乘 32767，CRC16-CCITT 只计算前 15 字节，低字节在前。反馈帧中温度为 int8，错误位 0 到 4 分别对应正常、过热、过流、过压、编码器故障，驱动层不做二次判断，原样填入结构体交给保护逻辑。三种定标各覆盖一段量程，精度均足够：q8 力矩一步 1/256 N·m，约 3.9 mN·m，远小于关节摩擦量级；q7 速度一步 1/128 rad/s；q15 位置一步约 1.9×10⁻⁴ rad，折算到腿末端为零点几毫米，控制上已足够。打包前统一做饱和：力矩按 cfg 上限截断，Kp/Kd 截断到 0~1，越界指令在驱动层被截断，而非溢出为反号数值——上位机发来异常指令时，第一道防线即在 pack 函数中。接口定义如下：

typedef struct { float pos_rad, vel_rad_s, tau_nm;
                 float k_pos, k_spd;            /* 0~1.0，归一化 */
} motor_cmd_t;
typedef struct { float pos_rad, vel_rad_s, tau_nm; int8_t temp_c;
                 uint8_t err; uint32_t stamp_us;
} motor_fb_t;
int  motor_init(motor_t *m, uint8_t id, uint8_t bus);
int  motor_enable(motor_t *m, bool en);
int  motor_set_cmd(motor_t *m, const motor_cmd_t *cmd);
int  motor_read_fb(motor_t *m, motor_fb_t *fb);

打包侧是对称的一段代码，q 定标的换算全部集中在此，上层传入的始终为 SI 单位。注意 lrintf 加 clampf 的组合：Kp 先截断到 0~1 再乘 32767，越界的浮点值不会被静默截断为负数：

static void put_i16(uint8_t *p, int16_t v) { p[0] = (uint8_t)v; p[1] = (uint8_t)(v >> 8); }
static void put_i32(uint8_t *p, int32_t v)
{
    p[0] = (uint8_t)v;         p[1] = (uint8_t)(v >> 8);
    p[2] = (uint8_t)(v >> 16); p[3] = (uint8_t)(v >> 24);
}

void go_pack_ctrl(uint8_t out[17], uint8_t id, uint8_t status, const motor_cmd_t *c)
{
    out[0] = 0xFE; out[1] = 0xEE;
    out[2] = (uint8_t)((id & 0x0Fu) | ((status & 0x07u) << 4));
    put_i16(&out[3],  (int16_t)lrintf(c->tau_nm * 256.0f));     /* q8  */
    put_i16(&out[5],  (int16_t)lrintf(c->vel_rad_s * 128.0f));  /* q7  */
    put_i32(&out[7],  (int32_t)lrintf(c->pos_rad * 32768.0f));  /* q15 */
    put_i16(&out[11], (int16_t)lrintf(clampf(c->k_pos, 0.f, 1.f) * 32767.f));
    put_i16(&out[13], (int16_t)lrintf(clampf(c->k_spd, 0.f, 1.f) * 32767.f));
    uint16_t crc = crc16_ccitt(out, 15);
    out[15] = (uint8_t)crc; out[16] = (uint8_t)(crc >> 8);      /* 低字节在前 */
}

解包即反向除法：力矩除 256、速度除 128、位置除 32768，字节 11 直接作为 int8 温度，状态字低三位为错误码，位 3~14 为 12 bit 足端气压（可选）。CRC 使用按位实现即可，17 字节在 550 MHz 下仅需 1~2 µs，无需查表；多项式 0x1021、初值 0xFFFF，控制帧计算前 15 字节、反馈帧计算前 14 字节：

uint16_t crc16_ccitt(const uint8_t *p, size_t n)
{
    uint16_t crc = 0xFFFFu;
    while (n--) {
        crc ^= (uint16_t)(*p++) << 8;
        for (int i = 0; i < 8; i++)
            crc = (crc & 0x8000u) ? (uint16_t)((crc << 1) ^ 0x1021u)
                                  : (uint16_t)(crc << 1);
    }
    return crc;
}

量程、力矩上限这类随型号变化的参数全部放入 cfg/motor_cfg.c（TODO(按型号填)，数值见第 3 篇），驱动只读配置；将来更换 IM6014（20 字节控制帧、26 字节反馈、CRC32）时，驱动层用一张 proto_ops 函数表切换 pack/unpack/长度/CRC，上层完全无感知。此处有一个反直觉的点必须说明：失能不是发 status=0，status=0 是锁定，会将关节抱死，摔倒时抱死一条腿只会使机器摔得更重。卸力的正确做法是 status=1 加 Kp=Kd=0 加零力矩，连续发几帧之后再决定是否锁定，急停与摔倒均走此路径。使能时 Kp 用 200 毫秒斜坡加上去，避免突然增加刚度引起冲击。温度方面，模组在 90 摄氏度自保护，软件的降额阈值应设在其之前，80 度即开始降额。

上电扫描也需说明：对 ID 0 到 14 逐个发送零力矩帧（status=1、Kp=Kd=0），收到 CRC 正确的反馈才算在线，15 为广播地址不作扫描对象；每帧 10 毫秒超时，12 个电机 120 毫秒扫完，同时带回每个电机的状态与温度。扫描出的在线表立即写日志并上报，线接反、ID 冲突这类问题在扫描阶段即暴露，比进入 STAND 后再发现成本更低。

状态机与保护：把安全写进结构

最后是状态机与保护，这是"安全"从口号落实为结构的环节。状态机共六个状态：INIT 自检、CALIB 标定、STAND 站立、WALK 行走、ESTOP 急停、FAULT 故障，迁移图如下：

stateDiagram-v2
    [*] --> INIT
    INIT --> CALIB: 自检通过
    INIT --> FAULT: 自检失败
    CALIB --> STAND: 零位写入 + 姿态水平
    CALIB --> FAULT: 标定失败
    STAND --> WALK: 使能 + 行走指令
    WALK --> STAND: 零速持续 500 ms
    STAND --> ESTOP: 急停 / 摔倒
    WALK --> ESTOP: 急停 / 摔倒
    STAND --> FAULT: 保护触发
    WALK --> FAULT: 保护触发
    ESTOP --> INIT: 手动复位 + 检查
    FAULT --> INIT: 手动复位 + 检查

六个状态各有进入动作与退出条件：INIT 关使能、清故障、运行自检，12 关节在线且 IMU 正常才放行；CALIB 锁零位、记录 IMU 零偏，零位写入成功且机身水平才进入 STAND；STAND 保持站立位形 q_stand，行走指令且速度大于 0.05 m/s 才起步；WALK 起步态、相位清零，速度归零持续 500 ms 才退回 STAND；ESTOP 立即卸力、记录原因；FAULT 卸力、记录错误码。规则明确：ESTOP 与 FAULT 均不允许自动恢复，必须人工确认机械无卡死、故障码清除后才回 INIT；急停是卸力而非锁定，"按一下复位"绝不能做成自动回 WALK。两者区别需分清：ESTOP 由人工按下或摔倒触发，第一优先级是卸力；FAULT 由保护触发，先读错误码再决定能否复位。

保护项	检测方法	响应	时间
过流	反馈力矩 >1.2×上限或错误位 bit2	该关节卸力，连续 3 次进 ESTOP	<5 ms
过压/欠压	母线分压 ADC，>26.5 V / <19.8 V（示例）	停发指令/降功率停机	<20 ms
过温	反馈温度 >80 °C 降额、>90 °C 停机	降额 50% → 卸力	<100 ms
堵转	|vel|<0.1 rad/s 且 |tau|>0.6×上限持续 200 ms	该关节卸力	200 ms
通信超时	10 ms 无反馈帧	卸力进 FAULT	10 ms
摔倒	横滚/俯仰超 45° 持续 100 ms	卸力进 ESTOP	50 ms
急停	EXTI 下降沿	断使能进 ESTOP	<1 ms
看门狗	IWDG 200 ms 未喂	复位回 INIT	200 ms

保护按级别分层，将上述响应归纳为 L1 到 L4 四级：

级别	触发例子	动作	恢复
L1 提示	单关节 80 °C	降额 50%，继续跑	温度回落自动恢复
L2 降级	单关节过流 1 次、偶发 CRC 错	该腿卸力，其余保持	人工确认后复位
L3 停机	通信超时、摔倒、急停	全部卸力，进 ESTOP	人工复位
L4 断电	过压、严重过流、硬件急停	断使能/断母线	排查后上电

四级的用意是使代价与概率相匹配：L1 为常态小偏差，降额继续运行，机器无需停止；L2 只影响一条腿，卸掉该腿的力，其余三条腿仍可维持到人员介入；L3 意味着机器人已不安全，全部卸力、姿态交由重力，这是使跌倒损失最小的唯一选择；L4 是电气层面的最后手段，直接断使能断母线。一项保护归入哪一级，取决于其是否会连锁放大——单关节过热降额即可，母线过压若不处理会连同电池与驱动一起损坏，因此直接归入 L4。

task_safety 的执行路径值得展开：1 ms 一次，先读急停 GPIO（EXTI 下降沿同时直接断硬件使能），再查估计器给出的摔倒标志，然后遍历 12 个电机的 online 与温度，将结论写入事件组与故障码。它全程只读不改，不写电机、不接触总线，真正的卸力由 task_control 看到事件位后统一执行——卸力只有一条路径，不会出现两个任务争用总线的竞态。软件侧急停延迟控制在两个 1 kHz 节拍之内，加上硬件断使能，从按下到卸力小于 5 毫秒。

阈值的确定也有方法：先用第 3 篇的手册值作初值，运行 30 分钟常温、10 分钟满载，将降额点定在实测峰值的八成，每改一次记录一条日志。保护完成后必须做故障注入，四种故障各做三次，确认每次均在表内时间内进入安全态：

1. 拔 A/B 线：模拟通信断线，10 ms 内应卸力进 FAULT；
2. 换欠压源或拔电源：<20 ms 内降功率停机；
3. 在 RS485 路径注入 20 ms 延迟：验证超时重发与 online 置零路径；
4. 把两个电机 ID 改成一样：总线应答全乱，验证 CRC 计数与卸力路径。

未做注入测试的保护等同于没有保护。四个测试各查一条链路：拔 A/B 查通信超时保护，拔电源查欠压检测与掉电保存，注入延迟查超时重发状态机与 online 置零，双 ID 查 CRC 校验与异常帧丢弃。每项完成后观察三方面：进入安全态的时间是否在表内、日志中的错误码与现场是否一致、恢复流程能否使机器回到正常状态。三方面均通过才算合格——只测"能进入安全态"而不测恢复，复位路径未经验证，同样会出问题。看门狗只在 task_monitor 中喂，喂之前确认所有任务心跳到齐；把喂狗放在高优先级任务中，若卡死的是其他任务，即为新手最常见的假保护。

参数、日志与上位机：让机器可观测

其余配套组件分几部分说明。参数存 Flash A/B 双区加 CRC32 加版本号：写入时先写备用区再写主区，任一步骤掉电后上电都能从另一区读回，格式不兼容即回默认值并报 ERR_PARAM；标定参数（零位、IMU 零偏、腿长）与运行参数（kp_stand/kd_stand、限幅、速度上限）均在其中。调试期用上位机 YAML 覆盖，冲突以 Flash 为准并打印版本号，整机脱离上位机依靠 Flash 自启动——YAML 只是覆盖层，Flash 才是最终保障。

参数结构体本身也需说明：magic 0x474F4F44（“GOOD”）置于头四个字节，读出的值与其不符即判为损坏；版本号随格式变化，升级后不兼容即回默认值。标定顺序固定四步：机械零位（工装或限位）→ IMU 六面静置取零偏 → 站立高度 → 站立增益 kp_stand/kd_stand，每步完成后立即 param_save 并记录日志。顺序不可颠倒：零位与零偏是后续一切换算的基准，先标增益后标零位等于在错误的坐标系中调参，全部无效。

日志的帧格式固定，帧头 0xA5 0x5A 开头便于上位机同步：

偏移	长度	字段
0	2	帧头 0xA5 0x5A
2	1	版本
3	1	类型（0x01 关节、0x02 IMU、0x03 事件、0x04 统计）
4	2	负载长度
6	4	时间戳（µs，DWT）
10	N	负载
10+N	2	CRC16-CCITT

分级按 DEBUG/INFO/WARN/ERROR 四级过滤，默认 INFO，调试期开启 DEBUG、量产关闭；日志量突然翻倍通常是有任务在反复报错，应先查原因，不应靠加大缓冲来应对。带宽需提前核算：1 kHz × 12 关节 × 8 字节约 96 kB/s，加帧头约 100 kB/s，921600 的 UART 仅 92 kB/s 不够用，日志口应上 2 Mbps（200 kB/s）或将关节日志降至 500 Hz。发送使用环形缓冲加 DMA：不拷贝数据，直接将当前段地址与长度交给 UART DMA，完成中断再启动下一段，回绕时一次最多发到缓冲末尾，写指针留安全余量。

环形缓冲还需设置两个水位：低水位，低于它 DMA 无数据可发、链路空转；高水位，接近它说明发送受阻，开始丢弃最旧数据并计数。丢帧需区分"主动丢弃"与"溢出"——前者是带宽不足的预案，后者是缺陷，统计中分开记录。时间戳全部来自同一个 DWT 计数器，与上位机对时之后，控制环每一拍的耗时、RS485 每一帧的往返均可按微秒对齐复盘。

上位机协议使用 0xAA 0x55 帧头加长度、命令码、序号与 CRC16-CCITT，命令 100 Hz 下发，ESTOP 走独立最高优先级不排队。命令表如下：

命令	码	负载	说明
ENABLE	0x01	0/1	使能/失能
STAND	0x02	高度（可选）	站立
WALK	0x03	vx, vy, wz（float×3）	速度指令
ESTOP	0x04	无	立即卸力
GET_PARAM	0x05	参数 ID	读参数
SET_PARAM	0x06	ID + 值	写参数
CALIB	0x07	类型	零位/IMU

解析状态机按 WAIT_AA → WAIT_55 → LEN → CMD → SEQ → PAYLOAD → CRC 推进，长度字段先做上限检查（≤64），超出即复位重新找帧头，避免一个错误长度撑爆缓冲；命令带 seq 序号做幂等，上位机 100 毫秒未收到 ACK 才重发，主控对重复序号只执行一次，不会重复使能或重复急停，ACK 中带当前状态与故障码，上位机无需再轮询；200 毫秒未收到任何有效帧，即把速度按 0.5 m/s² 减到零再进入 STAND，绝不因上位机卡死而继续执行旧速度。电机不走 CAN 的原因也在此说明：经典 CAN 一帧最多 8 字节，无法容纳 17 字节的控制帧，CAN FD 虽可容纳但宇树电机固件不支持，因此板上三路 FDCAN 留给遥控、扩展传感器与上位机备用。

上电流程也有明确的时间核算：时钟与外设初始化 50 毫秒，读参数加 CRC 10 毫秒，12 个电机扫描 120 毫秒（每个 10 毫秒），零位与 IMU 检查 300 毫秒，Kp 斜坡进入 STAND 200 毫秒，全程一秒内完成。任一步失败即进 FAULT 点红灯，不应带故障进入 STAND；日志中记录每步耗时，更换板卡、修改时钟之后一经对比，即可迅速发现"变慢"。先扫描再标定的顺序也不应颠倒——零位标定需驱动关节，前提是 12 个电机都能通信，顺序颠倒时标定失败将无法区分机械问题与通信问题。

这五步的耗时在日志中分开记录，任一步偏离基线即可立即发现。扫描为何每个电机给 10 毫秒？最坏情况是一帧无应答，42.5 µs 发完加 200 µs 超时重发，几百微秒即可，10 毫秒是为总线竞争与电机上电延迟预留的二十倍余量。零位与 IMU 检查的 300 毫秒主要用于等待机器静止——搬动与上电冲击会使 IMU 短暂饱和，需读到连续几十毫秒稳定的输出才继续。

验收共四项硬性指标：1 kHz 周期抖动 1 ms±50 µs 连续十分钟、两路 RS485 各运行十分钟 CRC 错误小于 0.01%、急停按下到卸力小于 5 毫秒、CPU 空闲大于 30%。四项指标各有测量方法：抖动靠 GPIO 翻转接示波器，看分布而非平均值；CRC 错误率应将重发也计入，只看最终成功率会掩盖一半错误；急停延迟用逻辑分析仪从按钮边沿量到电流跌落；CPU 空闲率取 task_monitor 十分钟的统计。全部达标后再谈步态调参，否则后续第 8 篇的行走实验出现问题时，将无法区分是控制算法问题还是底层不稳定。均通过后，这台机器即具备可靠的运动底层。下一篇（四足开发实战（七）：建模篇——运动学与 URDF 从零写起）从固件向上，讨论运动学与 URDF——机器需要感知自身足端位置，依赖的正是坐标系与雅可比矩阵。