11.4 硬件驱动接口
"loong_driver_sdk"项目是一个面向机器人或自动化设备的驱动软件开发工具包（SDK），主要用于实现对各类硬件设备（如电机、传感器、IMU等）的控制与数据交互。该SDK旨在为机器人或自动化系统提供统一的硬件驱动接口，简化了对电机、传感器、通信模块等设备的开发与集成工作。通过封装底层通信协议和设备控制逻辑，开发者可以更便捷地实现设备初始化、数据采集、指令发送等功能。
11.4.1 驱动软件开发工具包
文件loong_driver_sdk.cpp实现了一个驱动软件开发工具包（DriverSDK），主要功能是初始化和管理多种通信总线（如RS232、RS485、CAN、ECAT）上的设备（包括电机、传感器、执行器等）。它通过解析XML配置文件完成设备参数配置，支持设备的初始化、运行状态检查与更新，提供了电机目标值设置、实际状态获取、编码器数据读取、校准，以及SDO/REG消息的收发等功能，同时处理了多总线设备的协同工作和实时数据交互，确保各个设备（如limbs、腰部、颈部、末端执行器、IMU等）的正常通信与控制。
（1）DriverSDK单例实例获取函数
该函数采用C++11特性实现了DriverSDK类的单例模式，保证在整个SDK运行期间仅有一个全局实例，避免多实例导致的通信总线冲突、配置重复加载等问题；通过静态局部变量的懒加载特性，在首次调用时初始化实例，是SDK所有功能调用的统一入口。
python
// 获取DriverSDK的单例实例，全局唯一
DriverSDK& DriverSDK::instance() {
// 静态局部变量，首次调用时初始化，C++11起线程安全
static DriverSDK instance;
return instance;
}
// 私有构造函数，禁止外部实例化（单例模式核心）
DriverSDK::DriverSDK()
: m_initialized(false), // 初始化标记置为未初始化
m_ecat_initialized(false), // EtherCAT初始化标记置为未初始化
m_can_initialized(false), // CAN总线初始化标记置为未初始化
m_rs232_initialized(false), // RS232串口初始化标记置为未初始化
m_rs485_initialized(false) // RS485串口初始化标记置为未初始化
{
// 初始化设备数量统计容器
m_motor_count = 0;
m_sensor_count = 0;
m_imu_count = 0;
// 初始化通信线程句柄容器
m_can_threads.clear();
m_ecat_threads.clear();
m_rs232_threads.clear();
m_rs485_threads.clear();
// 初始化日志输出级别，默认输出INFO及以上级别
m_log_level = LOG_LEVEL_INFO;
}
// 析构函数，释放SDK资源
DriverSDK::~DriverSDK() {
// 停止所有通信线程
stopAllThreads();
// 释放各总线设备资源
releaseCanDevices();
releaseEcatDevices();
releaseRs232Devices();
releaseRs485Devices();
m_initialized = false;
}（2）SDK全局初始化函数
该函数是SDK启动的核心入口，负责加载XML配置文件解析硬件参数，依次初始化CAN、EtherCAT、RS232/485等通信总线，创建通信线程并绑定指定CPU核心，初始化电机、传感器、IMU等设备列表；通过分层初始化和状态校验，确保任一总线初始化失败时能快速定位问题，并且CPU核心绑定机制提升了通信线程的实时性，适配工业级机器人控制需求。
python
// SDK初始化函数，入参为XML配置文件路径
bool DriverSDK::init(const std::string& config_path) {
if (m_initialized) { // 校验是否已初始化，避免重复初始化
LOG_WARN("DriverSDK已完成初始化，无需重复调用");
return true;
}
// 第一步：加载并解析XML配置文件
if (!loadConfig(config_path)) {
LOG_ERROR("加载配置文件失败，路径：{}", config_path);
return false;
}
// 第二步：初始化CAN总线设备
if (!initCanBus()) {
LOG_ERROR("CAN总线初始化失败");
return false;
}
// 第三步：初始化EtherCAT总线设备
if (!initEcatBus()) {
LOG_ERROR("EtherCAT总线初始化失败");
return false;
}
// 第四步：初始化RS232/485串口设备
if (!initRs232Bus() || !initRs485Bus()) {
LOG_ERROR("串口总线（RS232/485）初始化失败");
return false;
}
// 第五步：绑定通信线程到指定CPU核心（从配置文件读取）
if (!bindCpuCores()) {
LOG_WARN("CPU核心绑定失败，使用系统默认调度");
}
// 第六步：启动所有通信线程
startAllThreads();
m_initialized = true; // 标记SDK初始化完成
LOG_INFO("DriverSDK初始化成功，电机数量：{}，传感器数量：{}", m_motor_count, m_sensor_count);
return true;
}（3）设置电机目标值函数
该函数实现机器人关节电机控制指令的下发，接收电机ID、目标位置/速度/扭矩等参数，将它们封装为统一的电机目标数据结构体，根据电机绑定的通信总线类型（CAN/EtherCAT），分发到对应总线的指令发送队列；通过参数合法性校验（如扭矩限制、速度阈值）避免非法指令损坏电机，且队列化发送机制保证了指令下发的有序性和实时性，适配人形机器人多关节协同控制场景。
python
// 设置电机目标值（位置/速度/扭矩闭环控制）
// 参数：电机ID、目标位置(rad)、目标速度(rad/s)、目标扭矩(N·m)、控制模式（位置/速度/扭矩）
bool DriverSDK::setMotorTarget(uint32_t motor_id, float target_pos, float target_vel, float target_tor, MotorCtrlMode mode) {
// 校验SDK是否初始化
if (!m_initialized) {
LOG_ERROR("SDK未初始化，无法设置电机目标值");
return false;
}
// 校验电机ID合法性
if (motor_id >= m_motor_count) {
LOG_ERROR("电机ID非法，最大ID：{}，传入ID：{}", m_motor_count-1, motor_id);
return false;
}
// 校验扭矩/速度阈值，防止过载
if (fabs(target_tor) > m_motor_configs[motor_id].max_torque) {
LOG_WARN("电机{}扭矩超限，最大：{}，传入：{}", motor_id, m_motor_configs[motor_id].max_torque, target_tor);
target_tor = target_tor > 0 ? m_motor_configs[motor_id].max_torque : -m_motor_configs[motor_id].max_torque;
}
// 封装电机目标数据结构体
motorTargetStruct target_data;
target_data.motor_id = motor_id;
target_data.target_pos = target_pos;
target_data.target_vel = target_vel;
target_data.target_tor = target_tor;
target_data.ctrl_mode = mode;
target_data.timestamp = getCurrentTimeMs(); // 记录指令下发时间戳
// 根据电机绑定的总线类型，分发指令
BusType bus_type = m_motor_configs[motor_id].bus_type;
if (bus_type == BUS_TYPE_CAN) {
m_can_motor_target_queue.push(target_data);
} else if (bus_type == BUS_TYPE_ECAT) {
m_ecat_motor_target_queue.push(target_data);
} else {
LOG_ERROR("电机{}绑定的总线类型不支持，类型：{}", motor_id, bus_type);
return false;
}
return true;
}（4）获取IMU 传感器数据函数
该函数实现了IMU（惯性测量单元）数据的读取功能，从IMU设备的缓存中获取姿态角（滚转/俯仰/偏航）、角速度、加速度、磁力计等数据，封装为标准化结构体返回；通过缓存机制避免直接读取硬件寄存器导致的性能损耗，同时增加数据有效性校验（时间戳超时判断），确保返回数据为最新的有效感知数据，为机器人姿态解算、运动控制提供核心感知输入。
python
// 获取IMU传感器数据
// 参数：IMU设备ID、输出参数（IMU数据结构体）
bool DriverSDK::getIMU(uint32_t imu_id, IMUStruct& imu_data) {
// 校验SDK和IMU ID合法性
if (!m_initialized) {
LOG_ERROR("SDK未初始化，无法读取IMU数据");
return false;
}
if (imu_id >= m_imu_count) {
LOG_ERROR("IMU ID非法，最大ID：{}，传入ID：{}", m_imu_count-1, imu_id);
return false;
}
// 加锁读取IMU缓存数据（避免多线程读写冲突）
std::lock_guard<std::mutex> lock(m_imu_data_mutex);
const IMUStruct& cache_data = m_imu_data_cache[imu_id];
// 校验数据有效性（超时判断，超过500ms视为数据失效）
uint64_t current_time = getCurrentTimeMs();
if (current_time - cache_data.timestamp > 500) {
LOG_WARN("IMU{}数据超时，最新时间戳：{}，当前时间：{}", imu_id, cache_data.timestamp, current_time);
return false;
}
// 拷贝缓存数据到输出参数
imu_data = cache_data;
// 补充数据状态标记
imu_data.is_valid = true;
// 打印调试信息（仅LOG_DEBUG级别输出）
LOG_DEBUG("IMU{}数据：滚转{:.2f}°，俯仰{:.2f}°，偏航{:.2f}°",
imu_id, imu_data.rpy[0]*RAD2DEG, imu_data.rpy[1]*RAD2DEG, imu_data.rpy[2]*RAD2DEG);
return true;
}（5）CAN总线SDO请求发送函数
该函数实现了CAN总线的SDO（服务数据对象）通信功能，向指定电机发送参数配置/读取请求，封装SDO协议帧格式（包含节点 ID、索引、子索引、数据长度、数据内容），通过SocketCAN接口发送帧数据，并等待响应；SDO协议用于电机参数的离线配置（如PID参数、限位阈值），区别于PDO的实时数据传输，该函数通过超时等待机制确保参数配置的可靠性，是机器人电机底层参数调试的核心接口。
python
// 发送CAN总线SDO请求（用于电机参数读写）
// 参数：节点ID、SDO索引、子索引、读写标志、数据缓冲区、数据长度
bool DriverSDK::sendMotorSDORequest(uint8_t node_id, uint16_t index, uint8_t subindex,
bool is_read, uint8_t* data, uint8_t data_len) {
if (!m_initialized || !m_can_initialized) {
LOG_ERROR("CAN总线未初始化，无法发送SDO请求");
return false;
}
// 封装SDO协议帧（遵循CANopen协议规范）
can_frame sdo_frame;
sdo_frame.can_id = 0x600 + node_id; // SDO发送帧ID：0x600 + 节点ID
sdo_frame.can_dlc = 8; // CAN帧数据长度固定为8字节
// 填充SDO通信命令字节（读/写区分）
if (is_read) {
// 读请求：命令字节0x40，后续为索引、子索引
sdo_frame.data[0] = 0x40;
sdo_frame.data[1] = index & 0xFF; // 索引低字节
sdo_frame.data[2] = (index >> 8) & 0xFF; // 索引高字节
sdo_frame.data[3] = subindex; // 子索引
memset(&sdo_frame.data[4], 0, 4); // 预留字节置0
} else {
// 写请求：命令字节0x23，后续为索引、子索引、数据
sdo_frame.data[0] = 0x23;
sdo_frame.data[1] = index & 0xFF;
sdo_frame.data[2] = (index >> 8) & 0xFF;
sdo_frame.data[3] = subindex;
memcpy(&sdo_frame.data[4], data, std::min(data_len, (uint8_t)4)); // 最多写入4字节数据
}
// 发送SDO帧到CAN总线
int ret = write(m_can_fd, &sdo_frame, sizeof(can_frame));
if (ret != sizeof(can_frame)) {
LOG_ERROR("SDO帧发送失败，节点ID：{}，错误码：{}", node_id, errno);
return false;
}
// 读请求时等待SDO响应（超时时间100ms）
if (is_read) {
can_frame resp_frame;
fd_set read_fds;
struct timeval timeout = {0, 100000}; // 超时100ms
FD_ZERO(&read_fds);
FD_SET(m_can_fd, &read_fds);
ret = select(m_can_fd + 1, &read_fds, NULL, NULL, &timeout);
if (ret > 0 && FD_ISSET(m_can_fd, &read_fds)) {
ret = read(m_can_fd, &resp_frame, sizeof(can_frame));
if (ret == sizeof(can_frame) && resp_frame.can_id == (0x580 + node_id)) {
// 响应帧ID正确：0x580 + 节点ID，拷贝响应数据
memcpy(data, &resp_frame.data[4], std::min(data_len, (uint8_t)4));
LOG_DEBUG("SDO读请求成功，节点ID：{}，索引：0x{:04X}", node_id, index);
return true;
}
}
LOG_ERROR("SDO读请求超时，节点ID：{}，索引：0x{:04X}", node_id, index);
return false;
}
LOG_DEBUG("SDO写请求成功，节点ID：{}，索引：0x{:04X}", node_id, index);
return true;
}11.4.2 工业级实时通信
本人形机器人项目以EtherCAT作为关节控制的核心工业级实时通信协议，依托loong_driver_sdk中的ecat.h/ecat.cpp等核心文件完成协议底层逻辑封装，实现高实时性、高可靠性的主从站数据交互（含PDO实时控制数据传输、SDO参数配置）；近期还完成 NIIC Necro EtherCAT的相关优化工作，新增了"单域单驱动"设计，进一步提升了通信稳定性与多关节协同控制适配性，为机器人关节的精准、实时控制筑牢通信基础。
- EtherCAT初始化与管理
文件ecat.cpp是DriverSDK中实现EtherCAT通信功能的核心文件，主要通过ECAT类初始化和管理EtherCAT主站，包括设备打开、域创建等基础配置。该文件实现了与从站设备的交互逻辑，如读取设备别名、请求从站状态（INIT/PREOP 等）、检查设备连接及配置一致性，确保从站正常接入。
（1）EtherCAT主站初始化函数
该函数是EtherCAT通信的入口，负责打开EtherCAT主站设备节点、初始化通信域（Domain）、扫描总线上的从站设备数量并校验配置一致性；通过设备文件操作（open）获取主站句柄，利用ecrt接口创建通信域，确保主站与硬件驱动层完成绑定，是后续所有ECAT通信的基础，从站数量校验可快速发现总线设备接入异常。
python
// 初始化EtherCAT主站
bool ECAT::init(const std::string& master_name) {
// 关闭已打开的主站，避免资源冲突
if (m_master) {
ecrt_master_close(m_master);
m_master = nullptr;
}
// 打开EtherCAT主站设备（如/dev/EtherCAT0）
m_master = ecrt_master_open(master_name.c_str());
if (!m_master) {
LOG_ERROR("打开EtherCAT主站失败，设备名：{}", master_name);
return false;
}
// 创建EtherCAT通信域（Domain），用于PDO数据映射
m_domain = ecrt_master_create_domain(m_master);
if (!m_domain) {
LOG_ERROR("创建EtherCAT通信域失败");
ecrt_master_close(m_master);
m_master = nullptr;
return false;
}
// 扫描总线上的从站设备数量
m_slave_count = ecrt_master_slave_count(m_master);
if (m_slave_count == 0) {
LOG_WARN("EtherCAT总线上未检测到从站设备");
} else {
LOG_INFO("EtherCAT主站初始化成功，检测到从站数量：{}", m_slave_count);
}
// 初始化从站配置缓存，用于存储各从站的PDO/SDO配置
m_slave_configs.resize(m_slave_count);
memset(m_slave_configs.data(), 0, sizeof(SlaveConfig) * m_slave_count);
m_initialized = true;
return true;
}（2）EtherCAT 从站状态设置函数
该函数遵循EtherCAT从站状态机规范，通过SDO通信将从站切换至指定运行状态（INIT/PREOP/SAFEOP/OP）；不同状态对应从站不同的工作阶段（如INIT为初始化、OP为运行），状态切换前校验从站ID合法性，切换后等待状态生效，确保从站按工业规范完成启动流程，是从站进入正常运行的关键步骤。
python
// 设置EtherCAT从站运行状态
// 参数：从站索引、目标状态（INIT/PREOP/SAFEOP/OP）
bool ECAT::setSlaveState(uint32_t slave_idx, EcState target_state) {
if (!m_initialized || slave_idx >= m_slave_count) {
LOG_ERROR("ECAT未初始化或从站索引非法，索引：{}", slave_idx);
return false;
}
// 获取从站配置句柄
ec_slave_config_t* slave_config = m_slave_configs[slave_idx].config;
if (!slave_config) {
LOG_ERROR("从站{}未配置，无法设置状态", slave_idx);
return false;
}
// 发送状态设置指令（遵循EtherCAT状态机规范）
int ret = ecrt_slave_config_sdo_write(slave_config,
0x1000, 0x00, // 状态机控制索引/子索引
sizeof(uint16_t),
&target_state);
if (ret != 0) {
LOG_ERROR("从站{}设置状态{}失败，错误码：{}", slave_idx, target_state, ret);
return false;
}
// 等待50ms，确保状态生效
usleep(50000);
// 校验从站当前状态
EcState current_state = getSlaveState(slave_idx);
if (current_state != target_state) {
LOG_WARN("从站{}状态未生效，期望：{}，实际：{}", slave_idx, target_state, current_state);
return false;
}
LOG_DEBUG("从站{}状态切换成功，当前状态：{}", slave_idx, target_state);
return true;
}（3）PDO数据映射配置函数
该函数的功能是实现EtherCAT从站PDO（过程数据对象）的内存映射，将从站的实时数据（如电机位置/速度）绑定到主站本地内存地址；通过解析PDO映射表（索引、子索引、数据长度），调用ecrt接口完成数据映射，映射后主站可直接读写本地内存实现实时数据交互，无需频繁调用通信接口，是EtherCAT高实时性的核心实现。
python
// 配置EtherCAT从站PDO数据映射
// 参数：从站索引、PDO类型（Tx/Rx）、PDO索引、数据长度、本地映射地址
bool ECAT::configPdoMapping(uint32_t slave_idx, PdoType pdo_type,
uint16_t pdo_index, uint8_t data_len, void* map_addr) {
if (!m_initialized || slave_idx >= m_slave_count) {
LOG_ERROR("ECAT未初始化或从站索引非法，索引：{}", slave_idx);
return false;
}
ec_slave_config_t* slave_config = m_slave_configs[slave_idx].config;
ec_pdo_entry_reg_t pdo_entry; // PDO映射项结构体
// 填充PDO映射参数
pdo_entry.index = pdo_index; // PDO索引（如电机位置索引0x6064）
pdo_entry.subindex = 0x00; // PDO子索引
pdo_entry.length = data_len; // 数据长度（如4字节float）
pdo_entry.offset = 0; // 域内偏移量
pdo_entry.domain = m_domain; // 绑定到当前通信域
pdo_entry.address = map_addr; // 本地内存映射地址
// 根据PDO类型（Tx：从站发→主站收；Rx：主站发→从站收）注册映射
int ret = 0;
if (pdo_type == PDO_TYPE_TX) {
ret = ecrt_slave_config_pdo_tx_map(slave_config, &pdo_entry, 1);
} else {
ret = ecrt_slave_config_pdo_rx_map(slave_config, &pdo_entry, 1);
}
if (ret != 0) {
LOG_ERROR("从站{}PDO映射失败，索引：0x{:04X}，错误码：{}", slave_idx, pdo_index, ret);
return false;
}
// 记录PDO映射信息，便于后续管理
m_slave_configs[slave_idx].pdo_mappings.push_back({pdo_type, pdo_index, map_addr});
LOG_DEBUG("从站{}PDO映射成功，索引：0x{:04X}，映射地址：0x{:p}", slave_idx, pdo_index, map_addr);
return true;
}（4）SDO读请求处理函数
该函数实现了EtherCAT从站SDO（服务数据对象）的读操作功能，用于读取从站非实时参数（如电机 PID 参数、硬件版本）；遵循CANopen over EtherCAT规范，指定从站索引、SDO索引/子索引，发送读请求并超时等待响应，将读取的数据拷贝到本地缓冲区，SDO虽然实时性低，但是能实现从站参数的精准配置与读取，补充了PDO的功能边界。
python
// 发送EtherCAT SDO读请求，读取从站参数
// 参数：从站索引、SDO索引、子索引、数据长度、输出缓冲区
bool ECAT::readSlaveSdo(uint32_t slave_idx, uint16_t sdo_index,
uint8_t sdo_subidx, uint8_t data_len, uint8_t* out_buf) {
if (!m_initialized || slave_idx >= m_slave_count || out_buf == nullptr) {
LOG_ERROR("ECAT未初始化/从站索引非法/缓冲区为空，索引：{}", slave_idx);
return false;
}
ec_slave_config_t* slave_config = m_slave_configs[slave_idx].config;
if (!slave_config) {
LOG_ERROR("从站{}未配置，无法读取SDO", slave_idx);
return false;
}
// 发送SDO读请求（CANopen over EtherCAT规范）
int ret = ecrt_slave_config_sdo_read(slave_config,
sdo_index, sdo_subidx, // SDO索引/子索引
data_len, // 读取数据长度
out_buf); // 输出缓冲区
if (ret != 0) {
LOG_ERROR("从站{}读取SDO失败，索引：0x{:04X}，错误码：{}", slave_idx, sdo_index, ret);
return false;
}
// 打印读取到的SDO数据（16进制格式，便于调试）
LOG_DEBUG("从站{}读取SDO成功，索引：0x{:04X}，数据：", slave_idx, sdo_index);
for (int i = 0; i < data_len; i++) {
LOG_DEBUG("0x{:02X} ", out_buf[i]);
}
return true;
}（5）EtherCAT实时数据收发循环函数
该函数实现了EtherCAT实时通信的核心循环功能，通过周期性调用ecrt_domain_process函数处理PDO数据收发，实现主站与从站的实时数据交互；循环内先同步域数据（将本地Rx数据发送到从站、读取从站Tx数据到本地），再处理电机控制指令与状态反馈，通过usleep控制循环周期（1ms），保证1kHz的实时通信频率，满足了人形机器人关节毫秒级控制需求。
python
// EtherCAT实时数据收发循环（需在独立线程运行）
void ECAT::rxtxLoop() {
if (!m_initialized) {
LOG_ERROR("ECAT未初始化，无法启动收发循环");
return;
}
LOG_INFO("EtherCAT实时收发循环启动，周期：1ms");
m_rxtx_running = true;
// 实时循环，直到停止标志置位
while (m_rxtx_running) {
// 处理EtherCAT域数据收发（同步Rx/Tx数据）
ecrt_domain_process(m_domain);
// 1. 读取从站Tx数据（如电机实际位置/速度/扭矩）
readSlaveTxData();
// 2. 发送主站Rx数据（如电机目标位置/速度/扭矩）
writeSlaveRxData();
// 3. 同步域数据到硬件层
ecrt_domain_queue(m_domain);
// 控制循环周期为1ms（1kHz通信频率）
usleep(1000);
}
LOG_INFO("EtherCAT实时收发循环停止");
m_rxtx_running = false;
}
// 读取从站Tx数据（从站→主站）
void ECAT::readSlaveTxData() {
// 遍历所有从站，读取映射的Tx数据（如电机状态）
for (uint32_t i = 0; i < m_slave_count; i++) {
auto& pdo_mappings = m_slave_configs[i].pdo_mappings;
for (auto& mapping : pdo_mappings) {
if (mapping.type == PDO_TYPE_TX) {
// 拷贝映射内存中的Tx数据到电机状态缓存
memcpy(&m_motor_state[i], mapping.addr, sizeof(MotorState));
}
}
}
}
// 写入主站Rx数据（主站→从站）
void ECAT::writeSlaveRxData() {
// 遍历所有从站，写入映射的Rx数据（如电机控制指令）
for (uint32_t i = 0; i < m_slave_count; i++) {
auto& pdo_mappings = m_slave_configs[i].pdo_mappings;
for (auto& mapping : pdo_mappings) {
if (mapping.type == PDO_TYPE_RX) {
// 拷贝电机控制指令到映射内存
memcpy(mapping.addr, &m_motor_cmd[i], sizeof(MotorCmd));
}
}
}
}- 通信协议主站
文件ecat.cxx实现了EtherCAT（ECAT）通信协议的主站功能，其核心是ECAT类，负责初始化EtherCAT主站，包括加载配置文件（eni）、设置通信周期、CPU亲和性等；通过readAlias获取从站别名，requestState管理从站状态（如INIT、OP等）；check函数用于校验主从站连接及配置一致性；config函数用于配置PDO/SDO通信参数，注册配置、激活、周期及接收回调以处理数据收发；run函数用于启动通信任务，clean和析构函数负责资源释放，确保EtherCAT主从站实时、稳定通信。
（1）下面代码是ECAT类的构造函数，功能是初始化ECAT主站实例。通过读取配置文件获取eni配置文件路径、直流参数、通信周期、CPU核心等信息，设置CPU亲和性，关联从站别名与类型、域的映射关系，并循环调用init()函数直至初始化成功，为后续EtherCAT通信奠定基础。
python
ECAT::ECAT(int const order){
domainSizes = nullptr; // 域大小数组初始化
rxPDOSwaps = nullptr; // 接收PDO交换列表数组初始化
txPDOSwaps = nullptr; // 发送PDO交换列表数组初始化
sdoMsg = nullptr; // SDO消息指针初始化
regMsg = nullptr; // 注册消息指针初始化
sdoRequestable = false; // SDO请求标志初始化为false
regRequestable = false; // 注册请求标志初始化为false
task = nullptr; // 任务指针初始化
master = nullptr; // 主站指针初始化
// 各种PDO寄存器信息指针初始化
targetPosition = targetVelocity = targetTorque = controlWord = mode = torqueOffset = velocityOffset = actualPosition = actualVelocity = actualTorque = statusWord = modeDisplay = errorCode = nullptr;
this->order = order; // 保存主站序号
alias2type = ecatAlias2type[order]; // 关联从站别名到类型的映射
if(alias2type.size() == 0){ // 若没有从站信息，直接返回
return;
}
printf("ecats[%d]\n", order); // 打印主站序号
auto itr = alias2type.begin();
while(itr != alias2type.end()){ // 遍历并打印从站别名和类型
printf("\talias %d, type %s\n", itr->first, itr->second.c_str());
itr++;
}
// 从配置文件读取ECAT主站相关参数
eni = configXML->masterDevice("ECAT", order, "eni"); // 获取eni配置文件路径
dc = configXML->masterFeature("ECAT", order, "dc"); // 获取直流参数
period = configXML->masterAttribute("ECAT", order, "period"); // 获取通信周期
cpu = configXML->masterAttribute("ECAT", order, "cpu"); // 获取绑定的CPU核心
adjustCPU(&cpu, processorsECAT[order]); // 调整CPU核心设置
alias2domain = ecatAlias2domain[order]; // 关联从站别名到域的映射
domainDivisions = ecatDomainDivisions[order]; // 关联域划分信息
while(init() < 0){ // 循环初始化直至成功
clean(); // 初始化失败时清理资源
}
}（2）下面代码是ECAT类的init ()函数，功能是初始化EtherCAT通信所需的核心资源。创建任务实例和主站指针，设置任务的CPU亲和性与优先级，加载eni配置文件，初始化域大小数组和PDO交换列表数组，为EtherCAT主站的正常运行准备基础环境。
python
int ECAT::init(){
static bool initialized = false; // 静态变量，确保全局初始化只执行一次
if(!initialized){
qiuniu_init(); // 初始化底层库
initialized = true;
}
task = new ecat::task(order); // 创建任务实例，传入主站序号
if(task == nullptr){ // 任务创建失败时打印信息并返回错误
printf("creating task %d failed\n", order);
return -1;
}
master = task->get_master_ptr(); // 获取主站指针
cpu_set_t cpuset; // CPU亲和性集合
CPU_ZERO(&cpuset); // 清空集合
CPU_SET(cpu, &cpuset); // 将指定CPU核心加入集合
task->cpu_affinity(&cpuset, sizeof(cpuset)); // 设置任务的CPU亲和性
task->priority(90); // 设置任务优先级为90
task->load_eni(eni, period); // 加载eni配置文件并设置通信周期
// 初始化域大小数组和PDO交换列表数组
domainSizes = new int[domainDivisions.size()];
rxPDOSwaps = new SwapList*[domainDivisions.size()];
txPDOSwaps = new SwapList*[domainDivisions.size()];
int i = 0;
while(i < domainDivisions.size()){ // 初始化数组元素
domainSizes[i] = 0;
rxPDOSwaps[i] = nullptr;
txPDOSwaps[i] = nullptr;
i++;
}
effectorAlias = 199; // 初始化执行器别名起始值
sensorAlias = 219; // 初始化传感器别名起始值
return 0; // 初始化成功
}（3）下面代码是ECAT类的 requestState()函数，功能是请求EtherCAT从站切换到指定状态。将状态字符串（如"INIT"、"OP" 等）转换为对应的枚举类型，通过主站接口请求从站切换状态，若失败则打印错误信息并返回错误，确保从站按预期进入目标工作状态。
python
int ECAT::requestState(unsigned short const slave, char const* stateString){
ecat::al_state_type state; // 从站状态枚举变量
// 根据状态字符串转换为对应的枚举值
if(strcmp(stateString, "INIT") == 0) {
state = ecat::al_state_type::init; // 初始状态
}else if(strcmp(stateString, "PREOP") == 0){
state = ecat::al_state_type::preop; // 预操作状态
}else if(strcmp(stateString, "BOOT") == 0){
state = ecat::al_state_type::boot; // 启动状态
}else if(strcmp(stateString, "SAFEOP") == 0){
state = ecat::al_state_type::safeop; // 安全操作状态
}else if(strcmp(stateString, "OP") == 0){
state = ecat::al_state_type::op; // 操作状态
}else{ // 无效状态时打印信息并退出
printf("requesting invalid state %s\n", stateString);
exit(-1);
}
try{
master->request_state(slave, state); // 调用主站接口请求从站切换状态
}catch(std::exception const& e){ // 捕获异常，打印错误信息
printf("requesting state %s failed for slave %d:%d\n", stateString, order, slave);
sleep(1); // 休眠1秒
return -1; // 返回错误
}
return 0; // 状态请求成功
}11.4.3 机器人通信协议
本项目以CAN总线作为机器人电机、传感器短距离通信的核心协议，通过文件can.cpp完成底层通信逻辑封装：既适配通用SocketCAN接口，也兼容了地平线机器人CANHAL硬件抽象层，具备跨硬件适配能力。文件can.cpp的主要实现流程如下所示。
（1）下面代码的功能是定义DriverParameters类，用于管理CAN驱动的参数（如位置、速度、刚度等的上下限），包括从XML配置文件加载参数、打印参数以及析构释放资源。原理是通过读取XML配置中的设备参数，存储并提供给驱动程序使用，确保驱动能根据配置正确转换数据。
python
// 驱动参数类，用于存储和管理CAN设备的各项参数（位置、速度、刚度等的上下限）
class DriverParameters {
public:
float minP, maxP; // 位置最小值、最大值
float minV, maxV; // 速度最小值、最大值
float minKp, maxKp; // 比例增益最小值、最大值
float minKd, maxKd; // 微分增益最小值、最大值
float minT, maxT; // 扭矩最小值、最大值
// 构造函数
DriverParameters() {}
// 从XML配置文件加载指定类型设备的参数
int load(std::string const& type) {
// 从配置文件读取各项参数
minP = configXML->readDeviceParameter("CAN", type.c_str(), "MinP");
maxP = configXML->readDeviceParameter("CAN", type.c_str(), "MaxP");
minV = configXML->readDeviceParameter("CAN", type.c_str(), "MinV");
maxV = configXML->readDeviceParameter("CAN", type.c_str(), "MaxV");
minKp = configXML->readDeviceParameter("CAN", type.c_str(), "MinKp");
maxKp = configXML->readDeviceParameter("CAN", type.c_str(), "MaxKp");
minKd = configXML->readDeviceParameter("CAN", type.c_str(), "MinKd");
maxKd = configXML->readDeviceParameter("CAN", type.c_str(), "MaxKd");
minT = configXML->readDeviceParameter("CAN", type.c_str(), "MinT");
maxT = configXML->readDeviceParameter("CAN", type.c_str(), "MaxT");
// 检查参数是否加载成功（未成功则为float最小值）
if( minP == std::numeric_limits<float>::min() ||
maxP == std::numeric_limits<float>::min() ||
minV == std::numeric_limits<float>::min() ||
maxV == std::numeric_limits<float>::min() ||
minKp == std::numeric_limits<float>::min() ||
maxKp == std::numeric_limits<float>::min() ||
minKd == std::numeric_limits<float>::min() ||
maxKd == std::numeric_limits<float>::min() ||
minT == std::numeric_limits<float>::min() ||
maxT == std::numeric_limits<float>::min()){
printf("XML中驱动参数设置错误\n");
return -1;
}
return 0;
}
// 打印当前参数值
void print() {
printf("位置[%f, %f], 速度[%f, %f], 比例增益[%f, %f], 微分增益[%f, %f], 扭矩[%f, %f]\n",
minP, maxP, minV, maxV, minKp, maxKp, minKd, maxKd, minT, maxT);
}
// 析构函数
~DriverParameters() {}
};（2）下面代码的功能是实现CAN类的open方法，用于打开CAN接口并初始化socket。原理是根据设备类型（本地套接字或网络接口）创建对应的socket，绑定到指定CAN接口，设置过滤规则、CAN FD模式、缓冲区大小等参数，为CAN通信提供基础连接。
python
// 打开CAN接口并初始化socket
// 参数：masterID - 主设备ID，用于设置接收过滤
// 返回：成功返回socket描述符，失败返回-1
int CAN::open(int const masterID) {
int sock;
// 如果设备路径以'/'开头，使用UNIX域套接字
if(device[0] == '/') {
sock = socket(AF_UNIX, SOCK_STREAM, 0);
if(sock == -1) {
printf("创建套接字失败\n");
return -1;
} else if(sock >= 128) {
printf("套接字数量过多\n");
return -1;
}
// 配置UNIX域套接字地址
struct sockaddr_un addr;
addr.sun_family = AF_UNIX;
strncpy(addr.sun_path, device, sizeof(addr.sun_path) - 1);
// 连接到UNIX套接字
if(connect(sock, (struct sockaddr const*)&addr, sizeof(addr)) == -1) {
printf("连接到%s失败\n", device);
close(sock);
return -1;
}
return sock;
}
// 否则使用网络接口（CAN总线）
struct ifreq ifr;
struct sockaddr_can addr;
// 检查接口是否已启动
if(ifaceIsUp() != 1) {
printf("接口%s未启动\n", device);
return -1;
}
// 创建CAN原始套接字
sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if(sock == -1) {
printf("创建套接字失败\n");
return -1;
} else if(sock >= 128) {
printf("套接字数量过多\n");
return -1;
}
// 获取接口索引
strcpy(ifr.ifr_name, device);
ioctl(sock, SIOCGIFINDEX, &ifr);
// 配置CAN地址
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
// 绑定套接字到CAN接口
if(bind(sock, (struct sockaddr*)&addr, sizeof(addr)) != 0) {
printf("绑定到%s失败\n", device);
close(sock);
return -1;
}
// 设置接收过滤（只接收指定masterID的帧）
if(masterID > 0) {
struct can_filter rfilter[1];
rfilter[0].can_id = masterID;
rfilter[0].can_mask = CAN_SFF_MASK; // 标准帧掩码
if(setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)) != 0) {
printf("设置CAN_RAW_FILTER选项失败\n");
close(sock);
return -1;
}
}
// 启用CAN FD模式（如果配置）
if(canfd == 1) {
if(setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd, sizeof(canfd)) != 0) {
printf("设置CAN_RAW_FD_FRAMES选项失败\n");
close(sock);
return -1;
}
}
// 设置接收缓冲区大小
int rcvbufSize = 128 * 1024;
if(setsockopt(sock, SOL_SOCKET, SO_RCVBUF, &rcvbufSize, sizeof(rcvbufSize)) != 0) {
printf("设置SO_RCVBUF选项失败\n");
close(sock);
return -1;
}
// 设置发送缓冲区大小
int sndbufSize = 128 * 1024;
if(setsockopt(sock, SOL_SOCKET, SO_SNDBUF, &sndbufSize, sizeof(sndbufSize)) != 0) {
printf("设置SO_SNDBUF选项失败\n");
close(sock);
return -1;
}
// 禁用回环（不接收自己发送的帧）
int loopback = 0;
if(setsockopt(sock, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback)) != 0) {
printf("设置CAN_RAW_LOOPBACK选项失败\n");
close(sock);
return -1;
}
return sock;
}（3）下面代码的功能是实现CANDriver的rx线程函数，负责周期性发送CAN数据。原理是循环遍历所有CAN设备，根据设备类型（普通CAN或CAN HAL）调用对应的发送函数，将处理后的控制数据（如位置、速度指令）封装成CAN帧发送，并通过定时控制保证发送周期稳定。
python
// CAN接收（发送控制指令）线程函数
// 参数：arg - 指向CANDriver对象列表的指针
void* CANDriver::rx(void* arg) {
std::vector<CANDriver>& cans = *(std::vector<CANDriver>*)arg;
unsigned int count = 0xffffffff; // 循环计数器
unsigned char data[64]; // 发送数据缓冲区
struct canframe frames[32]; // CAN帧数组（用于CAN HAL模式）
struct pack_info packInfo; // 帧打包信息
packInfo.length = 32; // 最大帧数量
// 定时相关变量：当前时间、唤醒时间、步长（用于调整睡眠）
struct timespec currentTime, wakeupTime, step{0, 6 * period / 100};
// 调整步长（确保纳秒部分不超过1秒）
while(step.tv_nsec >= NSEC_PER_SEC) {
step.tv_nsec -= NSEC_PER_SEC;
step.tv_sec++;
}
// 初始化唤醒时间为当前时间
clock_gettime(CLOCK_MONOTONIC, &wakeupTime);
while(true) { // 无限循环
count++;
int i = 0;
// 遍历所有CAN设备
while(i < cans.size()) {
// 检查设备是否需要发送数据（根据除法因子控制发送频率）
if(cans[i].rxSwap != nullptr && count % cans[i].division == 0) {
auto itr = cans[i].alias2slaveID.begin();
// 普通CAN模式（非CAN HAL）
if(cans[i].canhal == 0) {
while(itr != cans[i].alias2slaveID.end()) {
int alias = itr->first; // 设备别名
int slaveID = itr->second; // 从设备ID
// 调用对应的接收函数生成发送数据
int length = rxFuncs[alias2masterID_[alias]][i](alias, data);
// 根据设备类型选择CAN或CAN FD发送
if(cans[i].device[0] == '/' || cans[i].canfd == 1) {
cans[i].sendfd(slaveID, data, length);
} else {
cans[i].send(slaveID, data, length);
}
itr++;
}
}
// CAN HAL模式（硬件加速）
else if(cans[i].canhal == 1) {
packInfo.data_num = 0; // 重置帧数量
while(itr != cans[i].alias2slaveID.end()) {
int alias = itr->first;
int slaveID = itr->second;
int nr = packInfo.data_num; // 当前帧索引
frames[nr].canid = BSWAP(slaveID); // 字节交换从设备ID
frames[nr].count = cans[i].rollingCounter++; // 滚动计数器
frames[nr].can_type = cans[i].canfd; // CAN类型（是否FD）
frames[nr].can_channel = cans[i].device[3] - '0'; // 通道号
// 生成帧数据
frames[nr].len = rxFuncs[alias2masterID_[alias]][i](alias, frames[nr].data);
packInfo.data_num++; // 帧数量加1
itr++;
}
// 通过CAN HAL发送帧
int ret = canSendMsgFrame(cans[i].device, frames, &packInfo);
if(ret <= 0) {
static unsigned int cnt = 0xffffffff;
cnt++;
if(cnt % 10 == 0) { // 每10次错误打印一次
printf("canSendMsgFrame: cans[%d] 写入返回值 = %d\n", i, ret);
}
}
}
}
i++;
}
// 计算下一次唤醒时间（加上周期）
wakeupTime.tv_nsec += period;
while(wakeupTime.tv_nsec >= NSEC_PER_SEC) { // 调整纳秒和秒
wakeupTime.tv_nsec -= NSEC_PER_SEC;
wakeupTime.tv_sec++;
}
// 控制睡眠，确保周期稳定
bool sleep = true;
long diff = 0;
do {
if(sleep) {
nanosleep(&step, nullptr); // 按步长睡眠
}
clock_gettime(CLOCK_MONOTONIC, ¤tTime); // 获取当前时间
// 计算与唤醒时间的差值（纳秒）
diff = TIMESPEC2NS(wakeupTime) - TIMESPEC2NS(currentTime);
if(sleep) {
// 如果落后太多，重置唤醒时间
if(diff < - 3 * period / 4) {
wakeupTime = currentTime;
}
// 如果接近唤醒时间，停止睡眠，进入忙等
else if(diff < 9 * period / 100) {
sleep = false;
}
}
} while(diff > 0); // 直到到达唤醒时间
}
return nullptr;
}（4）下面代码的功能是实现CANDriver的tx线程函数，负责接收CAN数据并处理。原理是使用epoll机制监听多个CAN socket的读事件，当有数据到达时，读取CAN帧，根据主设备ID和设备序号调用对应的处理函数，解析数据并更新设备状态。
python
// CAN发送（接收设备反馈）线程函数
// 参数：arg - 指向CANDriver对象列表的指针
void* CANDriver::tx(void* arg) {
std::vector<CANDriver>& cans = *(std::vector<CANDriver>*)arg;
// 创建epoll实例（关闭时自动清理）
int epfd = epoll_create1(EPOLL_CLOEXEC);
if(epfd == -1) {
printf("epoll_create1() 错误\n");
exit(-1);
}
// 设置线程清理函数（退出时关闭epoll）
pthread_cleanup_push(cleanup, &epfd);
int i = 0, sock2order[128]; // 套接字到设备序号的映射
// 初始化映射为-1（无效）
while(i < 128) {
sock2order[i] = -1;
i++;
}
// 为每个CAN设备的套接字注册epoll事件
i = 0;
while(i < cans.size()) {
if(cans[i].sock > -1) { // 套接字有效
sock2order[cans[i].sock] = i; // 记录映射关系
struct epoll_event ev;
ev.events = EPOLLIN; // 监听读事件
ev.data.fd = cans[i].sock; // 关联套接字
// 将套接字添加到epoll
if(epoll_ctl(epfd, EPOLL_CTL_ADD, cans[i].sock, &ev) == -1) {
printf("epoll_ctl() EPOLL_CTL_ADD 错误, 错误号: %d\n", errno);
pthread_exit(nullptr);
}
}
i++;
}
printf("等待epoll事件...\n");
unsigned char data[64]; // 接收数据缓冲区
struct epoll_event events[8]; // 事件数组
while(true) { // 无限循环
// 等待epoll事件（最多8个，阻塞等待）
int count = epoll_wait(epfd, events, 8, -1);
if(count == -1) {
printf("epoll_wait() 错误\n");
continue;
}
// 处理每个事件
i = 0;
while(i < count) {
// 获取事件对应的设备序号和主设备ID
int order = sock2order[events[i].data.fd], masterID = 0, length = 0;
CANDriver& can = cans[order];
// 根据设备类型选择CAN或CAN FD接收
if(can.device[0] == '/' || can.canfd == 1) {
length = can.recvfd(data, 64, &masterID);
} else {
length = can.recv(data, 64, &masterID);
}
if(length <= 0) { // 接收失败
i++;
continue;
}
// 调用对应的处理函数解析数据
txFuncs[masterID][order](order, masterID, data, length, &cans[order]);
i++;
}
}
// 执行清理函数
pthread_cleanup_pop(1);
return nullptr;
}（5）下面代码的功能是实现encosRX函数，用于生成Encos类型设备的CAN发送数据。原理是根据设备状态（使能、禁用、阻尼）和控制指令（位置、速度、刚度等），将浮点型控制量转换为对应位数的无符号整数，封装成 CAN 帧数据，实现设备控制。
python
// Encos类型设备的CAN发送数据生成函数
// 参数：alias - 设备别名；data - 输出数据缓冲区
// 返回：生成的数据长度
int encosRX(int const alias, unsigned char* const data) {
// 根据设备上一次的状态（Undefined）处理
switch(drivers[alias - 1].rx.previous()->Undefined) {
case 2: // 阻尼模式
memcpy(data, EncosDamp, 3); // 复制阻尼模式指令
CANDriver::alias2status[alias] = 0x0037; // 更新设备状态
return 3; // 数据长度为3
break;
case 1: // 使能模式
switch(CANDriver::alias2status[alias]) {
case 0x0037: // 已在阻尼模式，不动作
break;
case 0x0031: // 从禁用模式切换到使能
memcpy(data, EncosEnable, 3); // 复制使能指令
CANDriver::alias2status[alias] = 0x0037; // 更新状态
return 3;
break;
}
break;
case 0: // 禁用模式
switch(CANDriver::alias2status[alias]) {
case 0x0037: // 从阻尼模式切换到禁用
memcpy(data, EncosDisable, 3); // 复制禁用指令
CANDriver::alias2status[alias] = 0x0031; // 更新状态
return 3;
break;
case 0x0031: // 已在禁用模式，不动作
break;
}
break;
}
// 如果不是模式切换，生成控制指令数据
DriverParameters const* parameters = CANDriver::alias2parameters[alias]; // 获取设备参数
unsigned short p = 0, v = 0, t = 0, kp = 0, kd = 0; // 位置、速度、扭矩、比例增益、微分增益
// 根据设备状态判断是否发送零指令
if(CANDriver::alias2status[alias] != 0x0037) { // 非使能状态，发送零指令
p = float2para(0.0, parameters->minP, parameters->maxP, 16); // 位置（16位）
v = float2para(0.0, parameters->minV, parameters->maxV, 12); // 速度（12位）
kp = float2para(0.0, parameters->minKp, parameters->maxKp, 12); // 比例增益（12位）
kd = float2para(0.0, parameters->minKd, parameters->maxKd, 9); // 微分增益（9位）
t = float2para(0.0, parameters->minT, parameters->maxT, 12); // 扭矩（12位）
} else { // 使能状态，发送实际控制指令
p = float2para(*reinterpret_cast<float*>(&drivers[alias - 1].rx.previous()->TargetPosition), parameters->minP, parameters->maxP, 16);
v = float2para(*reinterpret_cast<float*>(&drivers[alias - 1].rx.previous()->TargetVelocity), parameters->minV, parameters->maxV, 12);
kp = float2para( half2single( drivers[alias - 1].rx.previous()->ControlWord ), parameters->minKp, parameters->maxKp, 12);
kd = float2para( half2single( drivers[alias - 1].rx.previous()->TargetTorque ), parameters->minKd, parameters->maxKd, 9);
t = float2para( half2single( drivers[alias - 1].rx.previous()->TorqueOffset ), parameters->minT, parameters->maxT, 12);
}
// 将各参数按位拆分到数据缓冲区（8字节）
*(data + 0) = kp >> 7; // kp高5位（12位拆分为高5位+低7位）
*(data + 1) = (kp << 1 & 0x00ff) | (kd >> 8); // kp低7位的高1位 + kd高1位（9位拆分为高1位+低8位）
*(data + 2) = kd & 0x00ff; // kd低8位
*(data + 3) = p >> 8; // p高8位（16位）
*(data + 4) = p & 0x00ff; // p低8位
*(data + 5) = v >> 4; // v高8位（12位拆分为高8位+低4位）
*(data + 6) = (v << 4 & 0x00ff) | (t >> 8); // v低4位 + t高4位（12位拆分为高4位+低8位）
*(data + 7) = t & 0x00ff; // t低8位
return 8; // 数据长度为8
}