本文档深入解析 LroMotorDriver 的实现原理与协议细节，涵盖其继承架构、CAN-FD 帧格式、控制模式编解码、多电机批量控制机制以及错误码体系。阅读前建议先了解 MotorDriver抽象基类与接口设计 和 CAN/CANFD协议抽象接口，以便理解驱动在整个 SDK 中的位置。

架构定位与继承关系

LroMotorDriver 继承自 MotorDriver 抽象基类，通过 MotorDriver::create_motor() 工厂方法统一实例化。它在架构中定位于"电机驱动实现层"，直接依赖 MotorsCANFD 抽象接口完成帧收发，当前仅支持 CAN-FD 物理层（EtherCAT 预留接口但尚未实现）。与 DM、EVO 驱动并列，LRO 驱动是 SDK 支持的第三种品牌电机实现。

classDiagram
    class MotorDriver {
        +create_motor()$ shared_ptr~MotorDriver~
        +lock_motor()* void
        +unlock_motor()* void
        +init_motor()* uint8_t
        +motor_mit_cmd()* void
        +motor_pos_cmd()* void
        +motor_spd_cmd()* void
        #motor_id_: uint16_t
        #comm_type_: CommType
        #can_interface_: string
    }
    class MotorsCANFD {
        +transmit(frame) void
        +add_canfd_callback(cb, id) void
        +remove_canfd_callback(id) void
    }
    class LroMotorDriver {
        +LroMotorDriver(id, interface_type, can, model, offset)
        +canfd_rx_cbk(frame) void
        -write_register_lro(rid, value) void
        -save_register_lro() void
        -bus_registry_: unordered_map~string, vector~LroMotorDriver*~~$
        -motor_index_: uint8_t
        -limit_param_: LRO_Limit_Param
    }
    MotorDriver <|-- LroMotorDriver
    LroMotorDriver --> MotorsCANFD : uses (CAN-FD)

LRO 驱动在构造时通过 MotorsCANFD::get(can_interface) 获取单例总线后端，并将自身实例注册到 bus_registry_ 静态映射中。该注册机制是实现"一帧多机"批量 MIT 控制的基础：同一 can_interface 下的所有 LroMotorDriver 指针被收集到同一个向量中，在调用数组版 motor_mit_cmd(float*, ...) 时按 motor_index_ 填充 64 字节 CAN-FD 负载。

Sources: motor_driver.hpp, lro_motor_driver.hpp, motor_driver.cpp

支持的电机型号与参数限位

LeadRobot 系列当前支持四款电机，每款具有独立的物理限位参数，存储于全局静态数组 lro_limit_param 中。这些限位值同时用于 MIT 控制指令的量化和反馈数据的反量化。

在构造函数中，motor_model_ 决定 limit_param_ 的取值，后续所有 range_map 调用均依赖此结构完成浮点物理量与固定位宽整型之间的双向映射。值得注意的是，LRO 驱动的位置控制指令内部使用角度（degree）和转速（RPM）作为单位，与 SDK 公开的 rad / rad/s 接口存在显式换算。

Sources: lro_motor_driver.hpp, lro_motor_driver.cpp

CAN-FD 通信协议与帧格式

LRO 驱动采用标准 CAN-FD 帧进行通信，所有数据帧均置位 CANFD_BRS（比特率切换）标志以提升传输效率。协议中存在两种 CAN ID 语义：以电机独立 ID（1–0x7FF）为标识的目标控制帧，以及以固定 0x7FF 为标识的全局管理帧。

控制帧结构（以 MIT 模式为例）

MIT 阻抗控制帧长度为 8 字节，采用紧凑位域打包，总布局如下：

block-beta
    columns 8
    block:row1:8
        b63["bit 63-61<br/>motor_mode<br/>uint3"]
        b60["bit 60-49<br/>Kp<br/>uint12"]
        b48["bit 48-40<br/>Kd<br/>uint9"]
        b39["bit 39-24<br/>pos<br/>uint16"]
        b23["bit 23-12<br/>spd<br/>uint12"]
        b11["bit 11-0<br/>torque<br/>uint12"]
    end

量化规则为线性映射：pos 映射到 [-PosMax, PosMax] 区间占满 16 位；spd 映射到 [-SpdMax, SpdMax] 占满 12 位；Kp 映射到 [0, OKpMax] 占满 12 位；Kd 映射到 [0, OKdMax] 占满 9 位；torque 映射到 [-TauMax, TauMax] 占满 12 位。发送前驱动会先通过 limit() 将浮点输入裁剪到对应物理范围，再通过 range_map() 完成量化。

位置控制帧（POS 模式）采用 8 字节长度，位域分布为：模式位 3bit、目标位置 32bit（IEEE-754 float，但以 8 字节中的特定位存放）、限速 15bit、电流限制 12bit、应答位 2bit。速度控制帧（SPD 模式）为 7 字节，直接以大端序存放 RPM 浮点值的 4 字节原始内存，后接 16bit 电流限制。

Sources: lro_motor_driver.cpp, utils.hpp

全局管理命令（0x7FF 帧）

0x7FF 是 LRO 协议中预留的全局配置 CAN ID，用于执行不针对单条电机 ID 的管理操作。所有管理帧均携带 CANFD_BRS 标志。

init_motor() 的实现流程体现了典型的 LRO 上电初始化序列：先发送禁用指令进入只读状态，再切至 MIT 控制模式，随后使能电机，最后刷新一次状态并返回错误码。clear_motor_error() 则采用"先禁用再使能"的软复位策略，而非发送独立清错码，这是 LRO 协议与其他品牌电机的显著差异。

Sources: lro_motor_driver.hpp, lro_motor_driver.cpp

控制模式详解

MIT 阻抗控制模式

MIT 模式是 LRO 驱动的核心控制模式。单电机指令通过 motor_mit_cmd(float f_p, float f_v, float f_kp, float f_kd, float f_t) 发送。在首次进入 MIT 模式或从其他模式切换回来时，set_motor_control_mode() 会自动发送一条零指令（位置、速度、Kp、Kd、力矩均为 0）作为同步缓冲，防止模式跳变引发电机抖动。所有参数在量化前均会减去 motor_zero_offset_ 并经过 limit() 裁剪，确保不会溢出固定位宽。

位置控制模式

motor_pos_cmd(float pos, float spd, bool ignore_limit) 将目标位置从 rad 转换为 degree，速度从 rad/s 转换为 RPM（取绝对值后放大 10 倍并以 uint15 存放），然后按位域打包发送。ignore_limit 参数在 LRO 实现中未参与逻辑，属于基类接口的保留签名。

速度控制模式

motor_spd_cmd(float spd) 将目标速度转换为 RPM 后，以大端字节序写入帧中，并附带最大电流限制 0xFFFF。与位置模式类似，单位转换发生在驱动层，对上层用户透明。

Sources: lro_motor_driver.cpp, lro_motor_driver.cpp

多电机批量控制（One-to-Many）

LRO 驱动支持一种高效的多电机同步控制机制：当用户调用数组版 motor_mit_cmd(float* f_p, float* f_v, float* f_kp, float* f_kd, float* f_t) 时，驱动会构造一帧长度为 64 字节的 CAN-FD 报文，CAN ID 为 0x8080 | CAN_EFF_FLAG（扩展帧）。该帧被划分为 8 个 8 字节槽位（slot 0–7），每个槽位对应 motor_index_ 为 0–7 的电机。

sequenceDiagram
    participant User
    participant LroMotorDriver as 任意LRO实例
    participant Registry as bus_registry_
    participant Bus as MotorsCANFD

    User->>LroMotorDriver: motor_mit_cmd(arrays)
    LroMotorDriver->>Registry: 按 can_interface 查找同总线所有实例
    loop 遍历同总线电机
        LroMotorDriver->>LroMotorDriver: 按 motor_index_ 选择 slot
        LroMotorDriver->>LroMotorDriver: 量化该电机的 p/v/kp/kd/t
        LroMotorDriver->>LroMotorDriver: 将 8 字节打包进 data[slot*8 : slot*8+7]
    end
    LroMotorDriver->>Bus: transmit(64B frame, id=0x8080)

该机制要求同一总线上的电机 ID 必须落在 1–8 范围内，以便 motor_index_ = motor_id - 1 正确映射到帧槽位。若 motor_index_ >= 8，该实例在批量发送时会被静默跳过。由于整个填充过程受 bus_registry_mutex_ 保护，多线程环境下调用数组版指令是线程安全的。

Sources: lro_motor_driver.cpp, lro_motor_driver.hpp

反馈数据解析

canfd_rx_cbk() 是 LRO 驱动的接收回调入口，由 MotorsCANFD 后台线程在收到匹配 motor_id_ 的帧时触发。当前实现仅完整解析了 Type-1 反馈（0x01，即 Byte0 的高 3 位），其布局如下：

温度字段采用 "原始值减 25" 的偏移编码，因此接收到的无符号数值 25 对应实际 0°C。每次回调还会将 response_count_ 清零，用于上层判断通信是否超时。若 error_id_ > 0，驱动会通过 spdlog 输出错误日志，包含总线名、电机 ID 和十六进制错误码。

Sources: lro_motor_driver.cpp

错误码体系

LRO 协议将错误信息存放在反馈帧 Byte0 的低 5 位，定义如下：

init_motor() 在初始化末尾会专门对 error_id_ 进行 switch-case 映射，将上述错误码原样返回给调用方，便于上层在上电自检阶段识别硬件故障。refresh_motor_status() 的实现策略是发送一条零力矩 MIT 指令并等待 Type-1 反馈，从而在不改变电机运行状态的前提下刷新 telemetry。

Sources: lro_motor_driver.hpp, lro_motor_driver.cpp

参数配置与查询

配置模式（Mode 0x06）

LRO 支持通过配置模式写入电机运行参数，帧格式为：Byte0 = (0x06 << 5)，Byte1 = config_code，Byte2-5 = float / int32 value。驱动封装了 write_register_lro(uint8_t rid, float value) 和 write_register_lro(uint8_t index, int32_t value) 两个重载，内部自动完成 float 内存reinterpret 或 int32 大端拆分。

与 DM 和 EVO 驱动不同，LRO 协议没有独立的"写入 Flash"命令；参数在配置帧发送后即自动持久化。因此 write_motor_flash() 直接返回 true，save_register_lro() 仅输出警告日志说明该行为。

查询模式（Mode 0x07）

get_motor_param(uint8_t param_cmd) 发送查询帧请求电机内部状态。查询码涵盖制造商信息、实时位置/速度/电流、功率、环路与限位参数、MCU UUID、软件版本等。由于当前回调仅解析 Type-1 反馈，查询响应（Type-5）的详细解码需要用户在上层或后续版本中扩展。

Sources: lro_motor_driver.hpp, lro_motor_driver.cpp

与其他品牌驱动的差异对比

将 LRO 驱动与 DM、EVO 并列对比，可以帮助开发者快速选型并避免协议层面的混淆：

Sources: lro_motor_driver.hpp

下一步阅读

理解 LRO 驱动的协议细节后，建议继续阅读以下页面以形成完整的控制闭环认知：

• 若需深入理解 MIT 控制的数学原理与参数调参方法，请参阅 MIT阻抗控制原理与实现
• 若需了解位置与速度控制模式的通用接口语义与使用示例，请参阅 位置与速度控制模式
• 若需了解状态反馈与所有品牌电机通用的错误处理策略，请参阅 状态反馈与错误码解析
• 若需通过 Python 调用本文所述的 LRO 接口，请参阅 Python pybind11绑定机制