CANopenNode 接口及 CANopenLinux 完整实现

一、CANopenNode 核心接口详解

1.1 对象创建与初始化接口

CO_new() - 创建 CANopen 主对象

CO_t* CO = CO_new(NULL, &heapMemoryUsed);
if (CO == NULL) {
    log_printf(LOG_CRIT, DBG_GENERAL, "CO_new(), heapMemoryUsed=", heapMemoryUsed);
    exit(EXIT_FAILURE);
}

功能：

• 在堆上分配 CANopen 对象内存
• 初始化所有子模块的内存空间
• 返回 CO_t* 指针，这是整个协议栈的核心对象

参数：

• heapMemoryUsed: 返回实际使用的堆内存大小（字节）

CO_CANmodule_init() - 初始化 CAN 驱动

CO_ReturnError_t err = CO_CANmodule_init(
    CO->CANmodule,          // CAN 模块对象
    (void*)&CANptr,         // 驱动特定参数（Linux socketCAN）
    NULL,                   // RX array (由内部分配)
    0U,                     // RX array size
    NULL,                   // TX array
    0U,                     // TX array size
    0U                      // 波特率表索引（Linux 下不使用）
);

功能：

• 打开 SocketCAN 设备（如 can0）
• 配置 CAN 接收和发送缓冲区
• 注册 CAN 文件描述符到 epoll

CO_LSSinit() - 初始化层设置服务

err = CO_LSSinit(CO, &lssAddress, &mlStorage.pendingNodeId, &mlStorage.pendingBitRate);

功能：

• 支持动态配置节点 ID 和波特率
• 通过 CAN 网络远程配置未配置的设备
• 存储配置到非易失性存储

应用场景：

• 出厂设备默认无节点 ID
• 通过 LSS master 动态分配 ID
• 系统维护时更改波特率

CO_CANopenInit() - 核心协议栈初始化

err = CO_CANopenInit(
    CO,                          // CANopen 对象
    NULL,                        // 使用默认 NMT 对象
    NULL,                        // 使用默认 Emergency 对象
    OD,                          // 对象字典（Object Dictionary）
    OD_STATUS_BITS,              // 可选的状态位
    NMT_CONTROL,                 // NMT 控制标志
    FIRST_HB_TIME,               // 首次心跳时间 (500ms)
    SDO_SRV_TIMEOUT_TIME,        // SDO 服务器超时 (1000ms)
    SDO_CLI_TIMEOUT_TIME,        // SDO 客户端超时 (500ms)
    SDO_CLI_BLOCK,               // SDO 块传输 (false)
    CO_activeNodeId,             // 当前节点 ID
    &errInfo                     // 错误信息输出
);

初始化的模块：

1. NMT (Network Management)：网络管理，控制设备状态（初始化/预操作/操作/停止）
2. SDO (Service Data Object)：服务数据对象，用于配置参数
3. Emergency：紧急消息，报告设备错误
4. Heartbeat Producer：心跳生产者，定期发送设备存��信号
5. Heartbeat Consumer：心跳消费者，监控其他设备

NMT_CONTROL 标志详解：

#define NMT_CONTROL \
    CO_NMT_STARTUP_TO_OPERATIONAL        \  // 启动后自动进入操作态
    | CO_NMT_ERR_ON_ERR_REG              \  // 错误寄存器置位时进入预操作态
    | CO_ERR_REG_GENERIC_ERR             \  // 启用通用错误
    | CO_ERR_REG_COMMUNICATION              // 启用通信错误

CO_CANopenInitPDO() - 初始化过程数据对象

err = CO_CANopenInitPDO(
    CO,                     // CANopen 对象
    CO->em,                 // Emergency 对象
    OD,                     // 对象字典
    CO_activeNodeId,        // 节点 ID
    &errInfo                // 错误信息
);

PDO 类型：

• RPDO (Receive PDO)：接收过程数据，用于输入（如传感器数据）
• TPDO (Transmit PDO)：发送过程数据，用于输出（如控制命令）

PDO 配置来源：对象字典 0x1400-0x17FF (RPDO)、0x1800-0x1BFF (TPDO)

1.2 Linux 特定接口 - epoll 事件驱动

CO_epoll_create() - 创建 epoll 实例

CO_epoll_t epMain;
err = CO_epoll_create(&epMain, MAIN_THREAD_INTERVAL_US);  // 100ms 间隔
if (err != CO_ERROR_NO) {
    log_printf(LOG_CRIT, DBG_GENERAL, "CO_epoll_create(main), err=", err);
    exit(EXIT_FAILURE);
}

监控的事件：

1. timerfd：定时器事件（100ms 主线程，1ms 实时线程）
2. eventfd：线程间通知事件
3. socketfd：CAN socket 接收事件
4. gateway socket：网关客户端连接事件

CO_epoll_wait() - 等待事件发生

while (reset == CO_RESET_NOT && CO_endProgram == 0) {
    // 阻塞等待事件
    CO_epoll_wait(&epMain);
    
    // 处理事件...
}

内部实现：

void CO_epoll_wait(CO_epoll_t* ep) {
    // 调用 Linux epoll_wait 系统调用
    int n = epoll_wait(ep->epoll_fd, &ep->ev, 1, -1);  // 阻塞等待
    
    if (n == 1) {
        ep->epoll_new = true;  // 标记有新事件
        
        // 如果是定时器事件，读取并清除
        if (ep->ev.data.fd == ep->timer_fd) {
            uint64_t tmr;
            read(ep->timer_fd, &tmr, sizeof(tmr));
        }
    }
    
    // 计算时间差
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    uint64_t now_us = ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
    ep->timeDifference_us = now_us - ep->timePrevious_us;
    ep->timePrevious_us = now_us;
}

CO_epoll_processRT() - 实时任务处理

#ifdef CO_SINGLE_THREAD
CO_epoll_processRT(&epMain, CO, false);
#endif

处理的实时任务：

1. CAN 接收：从 SocketCAN 读取并分发 CAN 消息
2. SYNC 处理：同步对象处理
3. RPDO 处理：接收 PDO 解析
4. TPDO 处理：发送 PDO 准备和发送

实现细节：

void CO_epoll_processRT(CO_epoll_t* ep, CO_t* co, bool_t syncWas) {
    // 1. 检查 CAN 接收事件
    if (ep->epoll_new && (ep->ev.events & EPOLLIN) != 0) {
        if (ep->ev.data.fd == co->CANmodule->fd) {
            // 从 socketCAN 读取 CAN 帧
            CO_CANrxWait(co->CANmodule, ...);
        }
    }
    
    // 2. 加锁对象字典（多线程模式）
    CO_LOCK_OD(co->CANmodule);
    
    // 3. 处理 SYNC 和 PDO
    if (!co->nodeIdUnconfigured && co->CANmodule->CANnormal) {
        bool_t syncWas = CO_process_SYNC(co, ep->timeDifference_us, NULL);
        CO_process_RPDO(co, syncWas, ep->timeDifference_us, NULL);
        CO_process_TPDO(co, syncWas, ep->timeDifference_us, NULL);
    }
    
    // 4. 解锁对象字典
    CO_UNLOCK_OD(co->CANmodule);
}

CO_epoll_processMain() - 主线程任务处理

CO_epoll_processMain(&epMain, CO, GATEWAY_ENABLE, &reset);

处理的非实时任务：

1. SDO 服务器：响应参数读写请求
2. SDO 客户端：发起参数读写
3. Heartbeat Consumer：检测其他设备超时
4. LSS slave：处理 LSS 配置命令
5. Gateway：如果启用，处理 ASCII 命令

内部调用 CO_process()：

void CO_epoll_processMain(CO_epoll_t* ep, CO_t* co, bool_t enableGateway, CO_NMT_reset_cmd_t* reset) {
    uint32_t timerNext_us = ep->timerNext_us;
    
    // 调用核心处理函数
    *reset = CO_process(
        co, 
        enableGateway, 
        ep->timeDifference_us, 
        &timerNext_us
    );
    
    // 更新下次定时器触发时间
    if (timerNext_us < ep->timerNext_us) {
        ep->timerNext_us = timerNext_us;
    }
}

CO_epoll_processLast() - 最后处理

CO_epoll_processLast(&epMain);

功能：

• 根据 timerNext_us 重新配置定时器
• 如果需要提前触发，更新 timerfd

实现：

void CO_epoll_processLast(CO_epoll_t* ep) {
    // 如果下次触发时间小于默认间隔，重新配置定时器
    if (ep->timerNext_us < ep->timerInterval_us) {
        struct itimerspec its;
        its.it_value.tv_sec = ep->timerNext_us / 1000000;
        its.it_value.tv_nsec = (ep->timerNext_us % 1000000) * 1000;
        its.it_interval.tv_sec = 0;
        its.it_interval.tv_nsec = 0;
        
        timerfd_settime(ep->timer_fd, 0, &its, NULL);
    }
    
    // 重置为默认值
    ep->timerNext_us = ep->timerInterval_us;
    ep->epoll_new = false;
}

1.3 应用接口钩子

// 程序启动时调用（在 CANopen 初始化前）
CO_ReturnError_t app_programStart(
    uint16_t* bitRate,      // [输入/输出] 波特率
    uint8_t* nodeId,        // [输入/输出] 节点 ID
    uint32_t* errInfo       // [输出] 错误信息
);

// 通信复位后调用
void app_communicationReset(CO_t* co);

// 主线程循环调用（非实时）
void app_programAsync(
    CO_t* co,
    uint32_t timer1msDiff_us    // 距上次调用的时间差
);

// 实时线程循环调用（每 1ms）
void app_programRt(
    CO_t* co,
    uint32_t timer1msDiff_us
);

// 程序结束前调用
void app_programEnd();

二、canopend 实现详解

2.1 程序架构

┌─────────────────────────────────────────────┐
│              canopend 进程                   │
├─────────────────────────────────────────────┤
│  主线程 (Mainline Thread)                    │
│  - epoll 事件循环                            │
│  - SDO 处理                                  │
│  - 心跳消费者                                │
│  - 网关处理                                  │
│  - 存储管理                                  │
├─────────────────────────────────────────────┤
│  实时线程 (RT Thread) [可选]                 │
│  - 1ms 定时器                                │
│  - CAN 接收                                  │
│  - SYNC/PDO 处理                             │
│  - 应用实时代码                              │
└─────────────────────────────────────────────┘
         ↓                    ↓
    SocketCAN           Unix Socket
     (can0)        (/tmp/CO_command_socket)

2.2 主函数流程详解

int main(int argc, char* argv[]) {
    int programExit = EXIT_SUCCESS;
    CO_epoll_t epMain;
#ifndef CO_SINGLE_THREAD
    pthread_t rt_thread_id;
    int rtPriority = -1;
#endif
    CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
    CO_ReturnError_t err;
    CO_CANptrSocketCan_t CANptr = {0};
    
    // 步骤 1: 解析命令行参数
    char* CANdevice = argv[1];
    uint8_t nodeId = 0xFF;
    char* localSocketPath = NULL;
    int32_t commandInterface = -100;
    
    while ((opt = getopt(argc, argv, "i:p:c:T:s:r")) != -1) {
        switch (opt) {
            case 'i': nodeId = strtol(optarg, NULL, 0); break;
            case 'c': 
                if (strcmp(optarg, "stdio") == 0) {
                    commandInterface = CO_COMMAND_IF_STDIO;
                } else if (strncmp(optarg, "local-", 6) == 0) {
                    commandInterface = CO_COMMAND_IF_LOCAL_SOCKET;
                    localSocketPath = &optarg[6];
                } else if (strncmp(optarg, "tcp-", 4) == 0) {
                    uint16_t port;
                    sscanf(&optarg[4], "%hu", &port);
                    commandInterface = port;
                }
                break;
            case 'p': rtPriority = strtol(optarg, NULL, 0); break;
        }
    }
    
    // 步骤 2: 创建 CANopen 对象
    uint32_t heapMemoryUsed = 0;
    CO = CO_new(NULL, &heapMemoryUsed);
    
    // 步骤 3: 初始化存储
    CO_storageLinux_init(&storage, CO->CANmodule, ...);
    
    // 步骤 4: 调用应用启动钩子
#ifdef CO_USE_APPLICATION
    app_programStart(&mlStorage.pendingBitRate, 
                     &mlStorage.pendingNodeId, 
                     &errInfo_app_programStart);
#endif
    
    // 步骤 5: 初始化 CAN 驱动
    CANptr.can_ifindex = if_nametoindex(CANdevice);
    CO_CANmodule_init(CO->CANmodule, (void*)&CANptr, NULL, 0U, NULL, 0U, 0U);
    
    // 步骤 6: 创建 epoll 实例
    CO_epoll_create(&epMain, MAIN_THREAD_INTERVAL_US);  // 100ms
#ifndef CO_SINGLE_THREAD
    CO_epoll_create(&epRT, TMR_THREAD_INTERVAL_US);     // 1ms
    CANptr.epoll_fd = epRT.epoll_fd;
#else
    CANptr.epoll_fd = epMain.epoll_fd;
#endif
    
    // 步骤 7: 创建网关（如果启用）
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
    CO_epoll_createGtw(&epGtw, epMain.epoll_fd, 
                       commandInterface, socketTimeout_ms, 
                       localSocketPath);
#endif
    
    // 步骤 8: 启动实时线程（多线程模式）
#ifndef CO_SINGLE_THREAD
    pthread_create(&rt_thread_id, NULL, rt_thread, NULL);
    
    if (rtPriority > 0 && rtPriority < 100) {
        struct sched_param param = {.sched_priority = rtPriority};
        pthread_setschedparam(rt_thread_id, SCHED_FIFO, &param);
    }
#endif
    
    // 步骤 9: 进入通信复位循环
    while (reset != CO_RESET_APP && CO_endProgram == 0) {
        // 详见 2.3 节
    }
    
    // 步骤 10: 清理资源
    CO_storageLinux_auto_process(&storage, true);
    CO_epoll_close(&epMain);
#ifndef CO_SINGLE_THREAD
    CO_epoll_close(&epRT);
#endif
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
    CO_epoll_closeGtw(&epGtw);
#endif
    CO_CANsetConfigurationMode((void*)&CANptr);
    CO_delete(CO);
    
    exit(programExit);
}

2.3 通信复位循环详解

while (reset != CO_RESET_APP && reset != CO_RESET_QUIT && CO_endProgram == 0) {
    // 步骤 1: 初始化 LSS
    err = CO_LSSinit(CO, &lssAddress, 
                     &mlStorage.pendingNodeId, 
                     &mlStorage.pendingBitRate);
    CO_activeNodeId = mlStorage.pendingNodeId;
    
    // 步骤 2: 初始化 CANopen 核心功能
    errInfo = 0;
    err = CO_CANopenInit(
        CO,                          // CANopen 对象
        NULL,                        // 默认 NMT
        NULL,                        // 默认 Emergency
        OD,                          // 对象字典
        OD_STATUS_BITS,              // 状态位
        NMT_CONTROL,                 // NMT 控制
        FIRST_HB_TIME,               // 首次心跳 500ms
        SDO_SRV_TIMEOUT_TIME,        // SDO 服务器超时 1000ms
        SDO_CLI_TIMEOUT_TIME,        // SDO 客户端超时 500ms
        SDO_CLI_BLOCK,               // SDO 块传输：禁用
        CO_activeNodeId,             // 节点 ID
        &errInfo
    );
    
    // 步骤 3: 调用应用通信复位钩子
#ifdef CO_USE_APPLICATION
    app_communicationReset(CO);
#endif
    
    // 步骤 4: 初始化 PDO
    errInfo = 0;
    err = CO_CANopenInitPDO(CO, CO->em, OD, CO_activeNodeId, &errInfo);
    
    // 步骤 5: 初始化网关
    CO_epoll_initCANopenMain(&epMain, CO);
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
    CO_epoll_initCANopenGtw(&epGtw, CO);
#endif
    
    // 步骤 6: 启动 CAN
    CO_CANsetNormalMode(CO->CANmodule);
    reset = CO_RESET_NOT;
    
    log_printf(LOG_INFO, "Node ID=%d running...", CO_activeNodeId);
    
    // 主循环
    while (reset == CO_RESET_NOT && CO_endProgram == 0) {
        // 1. 等待事件
        CO_epoll_wait(&epMain);
        
        // 2. 处理实时任务（单线程模式）
#ifdef CO_SINGLE_THREAD
        CO_epoll_processRT(&epMain, CO, false);
#endif
        
        // 3. 处理网关
#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
        CO_epoll_processGtw(&epGtw, CO, &epMain);
#endif
        
        // 4. 处理主线程任务
        CO_epoll_processMain(&epMain, CO, GATEWAY_ENABLE, &reset);
        
        // 5. 最后处理
        CO_epoll_processLast(&epMain);
        
        // 6. 调用应用异步代码
#ifdef CO_USE_APPLICATION
        app_programAsync(CO, epMain.timeDifference_us);
#endif
        
        // 7. 自动存储参数
#if (CO_CONFIG_STORAGE) & CO_CONFIG_STORAGE_ENABLE
        if (storageIntervalTimer >= CO_STORAGE_AUTO_INTERVAL) {  // 60秒
            CO_storageLinux_auto_process(&storage, false);
            storageIntervalTimer = 0;
        } else {
            storageIntervalTimer += epMain.timeDifference_us;
        }
#endif
    }
    
    log_printf(LOG_INFO, "Finished, reset=%d", reset);
}

2.4 实时线程实现

static void* rt_thread(void* arg) {
    CO_epoll_t* ep = &epRT;
    
    log_printf(LOG_INFO, "Real-time thread started");
    
    while (CO_endProgram == 0) {
        // 等待定时器事件（每 1ms）
        CO_epoll_wait(ep);
        
        // 处理实时任务
        CO_epoll_processRT(ep, CO, false);
        
        // 调用应用实时代码
#ifdef CO_USE_APPLICATION
        app_programRt(CO, ep->timeDifference_us);
#endif
        
        // 更新定时器
        CO_epoll_processLast(ep);
    }
    
    log_printf(LOG_INFO, "Real-time thread stopped");
    return NULL;
}

实时线程特点：

• 固定周期：1ms 精确触发（通过 timerfd）
• 高优先级：SCHED_FIFO 调度策略（通过 -p 参数设置 1-99）
• 最小延迟：直接处理 CAN 接收，无需等待主线程
• 对象字典保护：使用 CO_LOCK_OD/CO_UNLOCK_OD 互斥锁

2.5 单线程 vs 多线程模式对比

特性	单线程模式	多线程模式
编译选项	CO_SINGLE_THREAD 定义	未定义
资源占用	低（一个线程）	高（两个线程）
实时性	较差（100ms 主循环）	好（1ms 实时线程）
PDO 延迟	最大 100ms	最大 1ms
线程同步	无需	需要互斥锁
适用场景	简单设备、低速通信	I/O 设备、高速 PDO

2.6 关键接口调用流程图

启动阶段:
  CO_new()
    ↓
  CO_storageLinux_init()
    ↓
  app_programStart()  [可选]
    ↓
  CO_epoll_create()
    ↓
  CO_CANmodule_init()

通信循环:
  CO_LSSinit()
    ↓
  CO_CANopenInit()
    ↓
  CO_CANopenInitPDO()
    ↓
  app_communicationReset()  [可选]
    ↓
  CO_CANsetNormalMode()
    ↓
  ┌─────────────────────────┐
  │  主循环（事件驱动）      │
  │  CO_epoll_wait()        │
  │    ↓                    │
  │  CO_epoll_processRT()   │
  │    ↓                    │
  │  CO_epoll_processGtw()  │
  │    ↓                    │
  │  CO_epoll_processMain() │
  │    ↓                    │
  │  CO_epoll_processLast() │
  │    ↓                    │
  │  app_programAsync()     │
  └────────┬────────────────┘
           │
        循环直至复位命令

实时线程（多线程模式）:
  ┌─────────────────────┐
  │ CO_epoll_wait()     │
  │   ↓                 │
  │ CO_epoll_processRT()│
  │   ↓                 │
  │ app_programRt()     │
  │   ↓                 │
  │ CO_epoll_processLast()│
  └─────────┬───────────┘
            │
      每 1ms 循环

三、网关接口实现

3.1 网关架构

  cocomm 客户端                  canopend                     CAN 网络
       │                           │                              │
       │  1. 连接 Unix Socket       │                              │
       ├─────────────────────────> │                              │
       │  "/tmp/CO_command_socket" │                              │
       │                           │                              │
       │  2. 发送 ASCII 命令        │                              │
       │  "[1] 4 read 0x1017 0 u16"│                              │
       ├─────────────────────────> │                              │
       │                           │  3. 解析命令                  │
       │                           │  -> CO_SDOclientUpload()     │
       │                           │                              │
       │                           │  4. 发送 SDO 请求             │
       │                           ├────────────────────────────> │
       │                           │  COB-ID=0x604 (node 4)       │
       │                           │                              │
       │                           │  5. 接收 SDO 响应             │
       │                           │<──────────────────────────── │
       │                           │  COB-ID=0x584                │
       │                           │                              │
       │                           │  6. 转换为 ASCII              │
       │                           │  "[1] 1000\r\n"              │
       │  7. 接收响应               │                              │
       │<───────────────────────── │                              │
       │                           │                              │

3.2 网关接口类型

typedef enum {
    CO_COMMAND_IF_DISABLED = -100,        // 禁用
    CO_COMMAND_IF_STDIO = -2,             // 标准输入输出
    CO_COMMAND_IF_LOCAL_SOCKET = -1,      // Unix 本地 socket
    CO_COMMAND_IF_TCP_SOCKET_MIN = 0,     // TCP socket (端口号)
    CO_COMMAND_IF_TCP_SOCKET_MAX = 0xFFFF
} CO_commandInterface_t;

typedef struct {
    int epoll_fd;                    // epoll 文件描述符
    int32_t commandInterface;        // 命令接口类型
    uint32_t socketTimeout_us;       // Socket 超时（微秒）
    uint32_t socketTimeoutTmr_us;    // Socket 超时计时器
    char* localSocketPath;           // 本地 socket 路径
    int gtwa_fdSocket;               // 监听 socket fd
    int gtwa_fd;                     // I/O socket fd
    bool_t freshCommand;             // 新命令标志
} CO_epoll_gtw_t;

3.3 网关初始化

CO_ReturnError_t CO_epoll_createGtw(
    CO_epoll_gtw_t* epGtw, 
    int epoll_fd, 
    int32_t commandInterface, 
    uint32_t socketTimeout_ms, 
    char* localSocketPath
) {
    epGtw->epoll_fd = epoll_fd;
    epGtw->commandInterface = commandInterface;
    epGtw->socketTimeout_us = socketTimeout_ms * 1000;
    
    // STDIO 模式
    if (commandInterface == CO_COMMAND_IF_STDIO) {
        epGtw->gtwa_fd = STDIN_FILENO;
        
        struct epoll_event ev = {0};
        ev.events = EPOLLIN;
        ev.data.fd = STDIN_FILENO;
        epoll_ctl(epoll_fd, EPOLL_CTL_ADD, STDIN_FILENO, &ev);
        
        log_printf(LOG_INFO, "Command interface: stdio");
    }
    
    // Unix 本地 Socket 模式
    else if (commandInterface == CO_COMMAND_IF_LOCAL_SOCKET) {
        struct sockaddr_un addr;
        
        epGtw->gtwa_fdSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
        
        memset(&addr, 0, sizeof(struct sockaddr_un));
        addr.sun_family = AF_UNIX;
        strncpy(addr.sun_path, localSocketPath, sizeof(addr.sun_path) - 1);
        bind(epGtw->gtwa_fdSocket, (struct sockaddr*)&addr, sizeof(struct sockaddr_un));
        
        listen(epGtw->gtwa_fdSocket, 5);
        
        struct epoll_event ev = {0};
        ev.events = EPOLLIN | EPOLLONESHOT;
        ev.data.fd = epGtw->gtwa_fdSocket;
        epoll_ctl(epoll_fd, EPOLL_CTL_ADD, epGtw->gtwa_fdSocket, &ev);
        
        signal(SIGPIPE, SIG_IGN);
        
        log_printf(LOG_INFO, "Command interface: local socket %s", localSocketPath);
    }
    
    // TCP Socket 模式
    else if (commandInterface >= 0 && commandInterface <= 0xFFFF) {
        struct sockaddr_in addr;
        
        epGtw->gtwa_fdSocket = socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0);
        
        const int yes = 1;
        setsockopt(epGtw->gtwa_fdSocket, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int));
        
        memset(&addr, 0, sizeof(struct sockaddr_in));
        addr.sin_family = AF_INET;
        addr.sin_port = htons(commandInterface);
        addr.sin_addr.s_addr = INADDR_ANY;
        bind(epGtw->gtwa_fdSocket, (struct sockaddr*)&addr, sizeof(struct sockaddr_in));
        
        listen(epGtw->gtwa_fdSocket, 5);
        
        struct epoll_event ev = {0};
        ev.events = EPOLLIN | EPOLLONESHOT;
        ev.data.fd = epGtw->gtwa_fdSocket;
        epoll_ctl(epoll_fd, EPOLL_CTL_ADD, epGtw->gtwa_fdSocket, &ev);
        
        signal(SIGPIPE, SIG_IGN);
        
        log_printf(LOG_INFO, "Command interface: TCP port %d", commandInterface);
    }
    
    return CO_ERROR_NO;
}

3.4 网关处理流程

void CO_epoll_processGtw(CO_epoll_gtw_t* epGtw, CO_t* co, CO_epoll_t* ep) {
    // 处理新连接
    if (ep->epoll_new && ep->ev.data.fd == epGtw->gtwa_fdSocket) {
        epGtw->gtwa_fd = accept4(epGtw->gtwa_fdSocket, NULL, NULL, SOCK_NONBLOCK);
        
        if (epGtw->gtwa_fd >= 0) {
            struct epoll_event ev2 = {0};
            ev2.events = EPOLLIN;
            ev2.data.fd = epGtw->gtwa_fd;
            epoll_ctl(ep->epoll_fd, EPOLL_CTL_ADD, epGtw->gtwa_fd, &ev2);
            
            epGtw->socketTimeoutTmr_us = 0;
            log_printf(LOG_DEBUG, "Gateway client connected, fd=%d", epGtw->gtwa_fd);
        }
        
        struct epoll_event ev = {0};
        ev.events = EPOLLIN | EPOLLONESHOT;
        ev.data.fd = epGtw->gtwa_fdSocket;
        epoll_ctl(epGtw->epoll_fd, EPOLL_CTL_MOD, epGtw->gtwa_fdSocket, &ev);
        
        ep->epoll_new = false;
    }
    
    // 处理命令输入
    else if (ep->epoll_new && ep->ev.data.fd == epGtw->gtwa_fd) {
        char buf[CO_CONFIG_GTWA_COMM_BUF_SIZE];
        size_t space = CO_GTWA_write_getSpace(co->gtwa);
        
        ssize_t s = read(epGtw->gtwa_fd, buf, space);
        
        if (s > 0) {
            // STDIO 模式：自动添加 "[0] " 前缀
            if (epGtw->commandInterface == CO_COMMAND_IF_STDIO) {
                const char sequence[] = "[0] ";
                if (buf[0] != '[' && buf[0] != '#') {
                    CO_fifo_write(&co->gtwa->commFifo, sequence, strlen(sequence), NULL);
                }
            }
            
            CO_fifo_write(&co->gtwa->commFifo, buf, s, NULL);
            
            epGtw->freshCommand = true;
            epGtw->socketTimeoutTmr_us = 0;
        }
        
        ep->epoll_new = false;
    }
    
    // 处理命令
    if (epGtw->freshCommand) {
        CO_GTWA_process(co->gtwa);
        
        if (CO_GTWA_write_getOccupied(co->gtwa) == 0) {
            epGtw->freshCommand = false;
        }
    }
    
    // 检查超时
    if (epGtw->socketTimeout_us > 0 && 
        epGtw->gtwa_fdSocket > 0 && 
        epGtw->gtwa_fd > 0) {
        
        if (epGtw->socketTimeoutTmr_us > epGtw->socketTimeout_us) {
            epoll_ctl(ep->epoll_fd, EPOLL_CTL_DEL, epGtw->gtwa_fd, NULL);
            close(epGtw->gtwa_fd);
            epGtw->gtwa_fd = -1;
            
            log_printf(LOG_DEBUG, "Gateway connection timeout");
            
            struct epoll_event ev = {0};
            ev.events = EPOLLIN | EPOLLONESHOT;
            ev.data.fd = epGtw->gtwa_fdSocket;
            epoll_ctl(epGtw->epoll_fd, EPOLL_CTL_MOD, epGtw->gtwa_fdSocket, &ev);
        } else {
            epGtw->socketTimeoutTmr_us += ep->timeDifference_us;
        }
    }
}

3.5 网关响应写回

static size_t gtwa_write_response(
    void* object, 
    const char* buf, 
    size_t count, 
    uint8_t* connectionOK
) {
    int* fd = (int*)object;
    size_t nWritten = count;
    
    if (fd != NULL && *fd >= 0) {
        ssize_t n = write(*fd, (const void*)buf, count);
        
        if (n >= 0) {
            nWritten = (size_t)n;
        } else {
            if (errno == EAGAIN || errno == EWOULDBLOCK) {
                log_printf(LOG_DEBUG, "Socket write EAGAIN, will retry");
                nWritten = 0;
            } else {
                *connectionOK = 0;
                nWritten = 0;
            }
        }
    } else {
        *connectionOK = 0;
    }
    
    return nWritten;
}

void CO_epoll_initCANopenGtw(CO_epoll_gtw_t* epGtw, CO_t* co) {
    CO_GTWA_initRead(co->gtwa, gtwa_write_response, (void*)&epGtw->gtwa_fd);
    epGtw->freshCommand = true;
}

3.6 支持的 CiA309-3 命令

命令	说明	示例
help	显示帮助	help
read	读取对象字典	4 read 0x1017 0 u16
write	写入对象字典	4 write 0x1017 0 u16 1000
start	启动节点 (NMT)	4 start
stop	停止节点	4 stop
preop	预操作模式	4 preop
reset node	复位节点	4 reset node
reset comm	复位通信	4 reset comm
set sdo_timeout	设置 SDO 超时	set sdo_timeout 1000
lss	LSS 命令	_lss_switch_glob 1

四、cocomm 工具实现详解

4.1 cocomm 架构

 用户输入                 cocomm                    canopend
    │                       │                           │
    │  命令行参数             │                           │
    │  "4 read 0x1017 0 u16" │                           │
    └──────────────────────> │                           │
                            │  1. 解析参数               │
                            │  2. 连接 socket            │
                            ├─────────────────────────> │
                            │  "/tmp/CO_command_socket" │
                            │                           │
                            │  3. 发送命令               │
                            │  "[1] 4 read 0x1017 0 u16\n"│
                            ├─────────────────────────> │
                            │                           │
                            │  4. 接收响应               │
                            │<─────────────────────────  │
                            │  "[1] 1000\r\n"           │
                            │                           │
                            │  5. 解析并打印             │
                            │  stdout: "1000"           │
                            │  stderr: "[1] "           │
                            │                           │

4.2 main 函数流程

int main(int argc, char* argv[]) {
    enum { out_all, out_data, out_flat } outputType = out_all;
    
    char* inputFilePath = NULL;
    char* socketPath = "/tmp/CO_command_socket";
    char hostname[HOST_NAME_MAX];
    char tcpPort[20] = "60000";
    int additionalReadStdin = 0;
    int fd_gtw;
    struct sockaddr_un addr_un;
    sa_family_t addrFamily = AF_UNIX;
    
    // 解析命令行参数
    while ((opt = getopt(argc, argv, "f:s:t:p:io:gh")) != -1) {
        switch (opt) {
            case 'f': inputFilePath = optarg; break;
            case 's': socketPath = optarg; break;
            case 't': 
                strncpy(hostname, optarg, HOST_NAME_MAX - 1);
                addrFamily = AF_INET;
                break;
            case 'p': strncpy(tcpPort, optarg, 19); break;
            case 'i': additionalReadStdin = 1; break;
            case 'o': 
                if (strcmp(optarg, "data") == 0) {
                    outputType = out_data;
                } else if (strcmp(optarg, "flat") == 0) {
                    outputType = out_flat;
                }
                break;
        }
    }
    
    // 从环境变量读取配置
    if (socketPath == NULL) {
        socketPath = getenv("cocomm_socket");
        if (socketPath == NULL) {
            socketPath = "/tmp/CO_command_socket";
        }
    }
    
    // 设置颜色输出
    if (outputType == out_all) {
        greenC = "\x1B[32m";
        redC = "\x1B[31m";
        resetC = "\x1B[0m";
    } else {
        greenC = redC = resetC = "";
    }
    
    // 创建并连接 socket
    signal(SIGPIPE, SIG_IGN);
    
    if (addrFamily == AF_INET) {
        // TCP 连接模式
        struct addrinfo hints, *res, *rp;
        
        memset(&hints, 0, sizeof(hints));
        hints.ai_family = AF_INET;
        hints.ai_socktype = SOCK_STREAM;
        hints.ai_flags |= AI_CANONNAME;
        
        getaddrinfo(hostname, tcpPort, &hints, &res);
        
        for (rp = res; rp != NULL; rp = rp->ai_next) {
            fd_gtw = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
            if (fd_gtw == -1) continue;
            
            if (connect(fd_gtw, rp->ai_addr, rp->ai_addrlen) != -1) {
                break;
            }
            
            close(fd_gtw);
        }
        
        freeaddrinfo(res);
    } 
    else {
        // Unix Domain Socket 连接模式
        fd_gtw = socket(AF_UNIX, SOCK_STREAM, 0);
        
        memset(&addr_un, 0, sizeof(struct sockaddr_un));
        addr_un.sun_family = AF_UNIX;
        strncpy(addr_un.sun_path, socketPath, sizeof(addr_un.sun_path) - 1);
        
        connect(fd_gtw, (struct sockaddr*)&addr_un, sizeof(struct sockaddr_un));
    }
}

4.3 命令发送与响应处理

从命令行参数读取命令

if (optind < argc) {
    for (int i = optind; i < argc; i++) {
        char* comm = argv[i];
        commBuf[0] = 0;
        
        // 自动添加序列号（如果用户没有提供）
        if (comm[0] != '[' && comm[0] != '#') {
            sprintf(commBuf, "[%d] ", i - optind + 1);
        }
        
        strcat(commBuf, comm);
        
        if (additionalReadStdin == 0) {
            strcat(commBuf, "\n");
            size_t len = strlen(commBuf);
            
            // 发送命令到网关
            write(fd_gtw, commBuf, len);
        } 
        else {
            // 从 stdin 读取额外参数
            strcat(commBuf, " ");
            size_t len = strlen(commBuf);
            char lastChar;
            
            do {
                if (fgets(commBuf + len, BUF_SIZE - 1 - len, stdin) == NULL) {
                    strcat(commBuf, "\n");
                }
                
                len = strlen(commBuf);
                lastChar = commBuf[len - 1];
                
                if (len < BUF_SIZE - 2 && lastChar != '\n') {
                    continue;
                }
                
                write(fd_gtw, commBuf, len);
                
                commBuf[0] = 0;
                len = 0;
            } while (lastChar != '\n');
        }
        
        printReply(fd_gtw);
    }
}

从文件读取命令

if (inputFilePath != NULL) {
    FILE* fp = fopen(inputFilePath, "r");
    
    while (fgets(commBuf, BUF_SIZE, fp) != NULL) {
        size_t len = strlen(commBuf);
        if (len < 1) continue;
        
        write(fd_gtw, commBuf, len);
        
        if (commBuf[len - 1] == '\n') {
            printReply(fd_gtw);
        }
    }
    
    fclose(fp);
}

从 stdin 读取命令

else {
    while (fgets(commBuf, BUF_SIZE, stdin) != NULL) {
        size_t len = strlen(commBuf);
        if (len < 1) continue;
        
        write(fd_gtw, commBuf, len);
        
        if (commBuf[len - 1] == '\n') {
            printReply(fd_gtw);
        }
    }
}

4.4 响应解析和打印

static int printReply(int fd_gtw) {
    char* replyBuf = malloc(BUF_SIZE + BUF_LAG);
    int ret = EXIT_SUCCESS;
    size_t bufFill = 0;
    
    while (1) {
        ssize_t n = read(fd_gtw, &replyBuf[bufFill], BUF_SIZE - bufFill);
        
        if (n > 0) {
            bufFill += n;
            replyBuf[bufFill] = 0;
            
            char* loc = strchr(replyBuf, '\n');
            while (loc != NULL) {
                *loc = 0;
                
                char* responseStart = replyBuf;
                char* sequenceEnd = strstr(replyBuf, "] ");
                
                if (sequenceEnd != NULL) {
                    *sequenceEnd = 0;
                    
                    if (outputType == out_all) {
                        char* responseSequence = replyBuf;
                        
                        if (strstr(sequenceEnd + 2, "ERROR:") == (sequenceEnd + 2)) {
                            fprintf(errStream, "%s%s]%s ", redC, responseSequence, resetC);
                        } else {
                            fprintf(errStream, "%s%s]%s ", greenC, responseSequence, resetC);
                        }
                        
                        printf("%s\n", sequenceEnd + 2);
                    } 
                    else if (outputType == out_data) {
                        printf("%s\n", sequenceEnd + 2);
                    } 
                    else {
                        printf("%s] %s\n", responseSequence, sequenceEnd + 2);
                    }
                } 
                else {
                    printf("%s\n", responseStart);
                }
                
                if (strstr(replyBuf, "ERROR:") != NULL) {
                    ret = EXIT_FAILURE;
                }
                
                size_t len = (loc - replyBuf) + 1;
                bufFill -= len;
                memmove(replyBuf, loc + 1, bufFill);
                replyBuf[bufFill] = 0;
                
                loc = strchr(replyBuf, '\n');
            }
            
            if (bufFill >= BUF_SIZE) {
                fprintf(errStream, "%sResponse too long!%s\n", redC, resetC);
                ret = EXIT_FAILURE;
                break;
            }
        } 
        else if (n == 0) {
            fprintf(errStream, "%sConnection closed%s\n", redC, resetC);
            ret = EXIT_FAILURE;
            break;
        }
        
        fflush(stdout);
    }
    
    free(replyBuf);
    return ret;
}

4.5 cocomm 使用示例

基本用法

# 读取心跳时间
cocomm "1 read 0x1017 0 u16"

# 写入心跳时间为 1000ms
cocomm "1 write 0x1017 0 u16 1000"

# 保存参数
cocomm "1 write 0x1010 1 vs save"

# 复位节点
cocomm "1 reset node"

从文件执行命令

创建 commands.txt:

[1] 4 read 0x1000 0 u32
[2] 4 read 0x1017 0 u16
[3] 4 write 0x1017 0 u16 2000
[4] 4 write 0x1010 1 vs save

执行：

cocomm -f commands.txt

环境变量配置

# 设置默认 socket 路径
export cocomm_socket="/var/run/canopen.sock"

# 设置 TCP 连接
export cocomm_host="192.168.1.100"
export cocomm_port="60000"

# 使用配置
cocomm "4 read 0x1017 0 u16