SDO Client/Server 实现
目录
架构概述
SDO 通信采用典型的 Client/Server 架构：
┌──────────────┐ ┌──────────────┐
│ SDO Client │ │ SDO Server │
│ (主站/PC) │ │ (设备/节点) │
├──────────────┤ ├──────────────┤
│ 命令行工具 │ │ 对象字典 │
│ 配置工具 │ │ 服务循环 │
│ 上位机软件 │ │ 数据访问层 │
└──────┬───────┘ └──────┬───────┘
│ │
│ CAN Frame (COB-ID 0x600+N) │
│──────────────────────────────────> │
│ │
│ CAN Frame (COB-ID 0x580+N) │
│◀────────────────────────────────── │
│ │
└────────── CAN Bus (vcan0) ──────────┘职责分离：
- Client：发起请求、等待响应、解析数据、处理超时
- Server：监听请求、查找对象字典、读取/写入数据、返回响应
SDO Client 实现
状态机
┌─────────┐
│ IDLE │
└────┬────┘
│
send_request │
▼
┌──────────────────┐
│ WAIT_RESPONSE │◀── 500ms 超时
└────────┬─────────┘
│
┌────────┼────────┐
│ │ │
success│ timeout│ abort│
▼ ▼ ▼
┌────────┐┌────────┐┌────────┐
│COMPLETE││ ERROR ││ ERROR │
└────────┘└────────┘└────────┘核心数据结构
typedef struct {
int sock; /* CAN socket */
uint8_t node_id; /* 目标节点 ID */
uint32_t timeout_ms; /* 超时时间（默认 500ms） */
canid_t req_cobid; /* 请求 COB-ID (0x600 + node_id) */
canid_t resp_cobid; /* 响应 COB-ID (0x580 + node_id) */
} sdo_client_t;
typedef enum {
SDO_STATE_IDLE,
SDO_STATE_WAIT_RESPONSE,
SDO_STATE_COMPLETE,
SDO_STATE_ERROR
} sdo_state_t;
typedef struct {
sdo_state_t state;
uint8_t data[256]; /* 接收缓冲区 */
size_t data_len; /* 接收到的数据长度 */
uint32_t abort_code; /* Abort 码（如果有） */
} sdo_transfer_t;关键函数
1. 初始化
int sdo_client_init(sdo_client_t *client, int sock, uint8_t node_id) {
client->sock = sock;
client->node_id = node_id;
client->timeout_ms = 500;
client->req_cobid = 0x600 + node_id;
client->resp_cobid = 0x580 + node_id;
return 0;
}2. Expedited Upload
int sdo_client_upload_expedited(sdo_client_t *client,
uint16_t index, uint8_t sub_index,
uint8_t *data, size_t *data_len) {
struct can_frame req, resp;
fd_set fds;
struct timeval tv;
/* 构建请求 */
memset(&req, 0, sizeof(req));
req.can_id = client->req_cobid;
req.can_dlc = 8;
req.data[0] = 0x40;
req.data[1] = index & 0xFF;
req.data[2] = (index >> 8) & 0xFF;
req.data[3] = sub_index;
/* 发送 */
if (write(client->sock, &req, sizeof(req)) < 0) {
return -1;
}
/* 等待响应（带超时） */
FD_ZERO(&fds);
FD_SET(client->sock, &fds);
tv.tv_sec = client->timeout_ms / 1000;
tv.tv_usec = (client->timeout_ms % 1000) * 1000;
if (select(client->sock + 1, &fds, NULL, NULL, &tv) <= 0) {
fprintf(stderr, "SDO timeout\n");
return -1;
}
/* 接收 */
if (read(client->sock, &resp, sizeof(resp)) < 0) {
return -1;
}
/* 验证 COB-ID */
if (resp.can_id != client->resp_cobid) {
return -1;
}
/* 检查 Abort */
if (resp.data[0] == 0x80) {
client->abort_code = resp.data[4] | (resp.data[5] << 8) |
(resp.data[6] << 16) | (resp.data[7] << 24);
return -1;
}
/* 解析 Expedited 数据 */
if ((resp.data[0] & 0x03) == 0x03) { /* e=1, s=1 */
int n = (resp.data[0] >> 2) & 0x07;
*data_len = 4 - n;
memcpy(data, &resp.data[4], *data_len);
return 0;
}
/* 不是 Expedited 模式 */
return -2;
}3. Expedited Download
int sdo_client_download_expedited(sdo_client_t *client,
uint16_t index, uint8_t sub_index,
const uint8_t *data, size_t data_len) {
struct can_frame req, resp;
int n = 4 - data_len;
memset(&req, 0, sizeof(req));
req.can_id = client->req_cobid;
req.can_dlc = 8;
req.data[0] = 0x20 | (n << 1) | 0x03; /* CCS=0, e=1, s=1 */
req.data[1] = index & 0xFF;
req.data[2] = (index >> 8) & 0xFF;
req.data[3] = sub_index;
memcpy(&req.data[4], data, data_len);
if (write(client->sock, &req, sizeof(req)) < 0) {
return -1;
}
/* 等待并验证响应 */
/* ... (与 Upload 类似的超时处理) ... */
if (resp.data[0] != 0x60) {
return -1; /* 非预期响应 */
}
return 0;
}超时机制
int sdo_wait_response(int sock, uint32_t timeout_ms) {
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(sock, &fds);
tv.tv_sec = timeout_ms / 1000;
tv.tv_usec = (timeout_ms % 1000) * 1000;
int ret = select(sock + 1, &fds, NULL, NULL, &tv);
if (ret < 0) {
return SDO_ERROR_SELECT;
}
if (ret == 0) {
return SDO_ERROR_TIMEOUT;
}
return 0; /* 数据可读 */
}SDO Server 实现
架构
┌───────────────────────────────────────────┐
│ SDO Server │
├───────────────────────────────────────────┤
│ │
│ ┌─────────┐ ┌──────────┐ │
│ │ CAN │───>│ Request │ │
│ │ Socket │ │ Parser │ │
│ └─────────┘ └────┬─────┘ │
│ │ │
│ ┌──────┴──────┐ │
│ │ OD Lookup │ │
│ └──────┬──────┘ │
│ │ │
│ ┌─────────┐ ┌────┴─────┐ │
│ │ CAN │<───│ Response │ │
│ │ Socket │ │ Builder │ │
│ └─────────┘ └──────────┘ │
│ │
│ ┌──────────────────────────────┐ │
│ │ Object Dictionary (OD) │ │
│ │ 0x1000: Device Type │ │
│ │ 0x1001: Error Register │ │
│ │ 0x1008: Device Name │ │
│ │ ... │ │
│ └──────────────────────────────┘ │
└───────────────────────────────────────────┘核心数据结构
typedef struct {
int sock;
uint8_t node_id;
canid_t req_cobid; /* 0x600 + node_id */
canid_t resp_cobid; /* 0x580 + node_id */
od_t *od; /* 对象字典指针 */
} sdo_server_t;请求处理循环
int sdo_server_run(sdo_server_t *server) {
struct can_frame req, resp;
while (1) {
/* 1. 接收请求 */
if (read(server->sock, &req, sizeof(req)) < 0) {
continue;
}
/* 2. 验证 COB-ID */
if (req.can_id != server->req_cobid) {
continue; /* 不是发给我的 */
}
/* 3. 解析命令 */
uint8_t ccs = req.data[0] >> 5;
switch (ccs) {
case 1: /* Initiate Upload */
handle_upload_initiate(server, &req, &resp);
break;
case 0: /* Initiate Download / Segment */
if (req.data[0] == 0x40) {
handle_upload_initiate(server, &req, &resp);
} else {
handle_download_initiate(server, &req, &resp);
}
break;
case 3: /* Segment Upload */
handle_segment_upload(server, &req, &resp);
break;
default:
send_abort(server, &req, 0x05040001);
break;
}
/* 4. 发送响应 */
write(server->sock, &resp, sizeof(resp));
}
}对象字典查找
od_entry_t *od_find(od_t *od, uint16_t index, uint8_t sub_index) {
for (int i = 0; i < od->entry_count; i++) {
if (od->entries[i].index == index) {
if (od->entries[i].max_sub == 0) {
/* VAR 类型，sub_index 必须为 0 */
if (sub_index == 0) {
return &od->entries[i];
}
} else {
/* ARRAY/RECORD 类型 */
if (sub_index <= od->entries[i].max_sub) {
return &od->entries[i];
}
}
}
}
return NULL; /* 未找到 */
}Upload 处理
static void handle_upload_initiate(sdo_server_t *server,
struct can_frame *req,
struct can_frame *resp) {
uint16_t index = req->data[1] | (req->data[2] << 8);
uint8_t sub_index = req->data[3];
/* 查找对象字典 */
od_entry_t *entry = od_find(server->od, index, sub_index);
if (!entry) {
send_abort(server, req, 0x06020000); /* 对象不存在 */
return;
}
/* 检查访问权限 */
if (entry->access == OD_ACCESS_WO) {
send_abort(server, req, 0x06010002); /* 只写对象 */
return;
}
/* 获取数据 */
uint8_t *data = entry->data;
size_t data_len = entry->data_size;
if (data_len <= 4) {
/* Expedited 传输 */
resp->can_id = server->resp_cobid;
resp->can_dlc = 8;
int n = 4 - data_len;
resp->data[0] = 0x40 | (n << 2) | 0x03; /* SCS=011, e=1, s=1 */
resp->data[1] = index & 0xFF;
resp->data[2] = (index >> 8) & 0xFF;
resp->data[3] = sub_index;
memcpy(&resp->data[4], data, data_len);
} else {
/* Segmented 传输 */
/* 实现分段发送逻辑 */
/* ... */
}
}Abort 发送
static void send_abort(sdo_server_t *server,
struct can_frame *req,
uint32_t abort_code) {
uint16_t index = req->data[1] | (req->data[2] << 8);
uint8_t sub_index = req->data[3];
struct can_frame resp;
memset(&resp, 0, sizeof(resp));
resp.can_id = server->resp_cobid;
resp.can_dlc = 8;
resp.data[0] = 0x80;
resp.data[1] = index & 0xFF;
resp.data[2] = (index >> 8) & 0xFF;
resp.data[3] = sub_index;
resp.data[4] = abort_code & 0xFF;
resp.data[5] = (abort_code >> 8) & 0xFF;
resp.data[6] = (abort_code >> 16) & 0xFF;
resp.data[7] = (abort_code >> 24) & 0xFF;
write(server->sock, &resp, sizeof(resp));
}集成演示
模块 2 的示例 7（examples/07-sdo-demo/）演示了完整的 SDO Client/Server 交互：
- Server 启动：创建模拟对象字典，监听 vcan0
- Client 启动：连接到同一 vcan0
- 自动执行 5 步演示：
- Expedited Upload 0x1000（设备类型）
- Expedited Upload 0x1001（错误寄存器）
- Expedited Download 0x1017（心跳周期）
- Segmented Upload 0x1008（设备名称）
- Segmented Upload 0x100A（软件版本）
# 终端 1：启动 Server
./sdo_server vcan0 3
# 终端 2：运行 Client
./sdo_client upload-exp vcan0 3 0x1000 0错误处理最佳实践
| 场景 | 处理方式 |
|---|---|
| 超时 | 返回错误码，不重试（由上层决定是否重试） |
| Abort 响应 | 记录 abort_code，返回错误 |
| Toggle 位不匹配 | 发送 Abort，终止传输 |
| COB-ID 不匹配 | 忽略帧（可能发给其他节点） |
| 对象不存在 | 返回 0x06020000 Abort |
| 访问权限错误 | 返回 0x06010002 Abort |
总结
| 组件 | 关键特性 |
|---|---|
| Client | 状态机、超时机制、Expedited/Segmented 支持 |
| Server | 请求解析、OD 查找、权限检查、Abort 处理 |
| 通信 | COB-ID 计算、帧格式解析、大小端处理 |
| 错误 | 超时、Abort、Toggle 位校验 |
如果这篇文章对你有帮助，欢迎请我喝杯咖啡 ☕
支付宝
微信