帮我用C写一段CANopen SDO收发函数

CANopen SDO（Service Data Object）是一种用于在CAN总线上进行数据传输的协议。下面是一个简单的CANopen SDO收发函数的示例，使用C语言编写：

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "canopen.h"

#define CANOPEN_COBID_SDO_TX 0x600
#define CANOPEN_COBID_SDO_RX 0x580

typedef struct {
    uint16_t index;
    uint8_t subindex;
    uint8_t data[4];
} canopen_sdo_t;

int canopen_sdo_write(canopen_sdo_t *sdo) {
    uint8_t tx_buf[8];
    uint8_t rx_buf[8];
    int ret;

    tx_buf[0] = 0x23;
    tx_buf[1] = sdo->index & 0xFF;
    tx_buf[2] = (sdo->index >> 8) & 0xFF;
    tx_buf[3] = sdo->subindex;
    memcpy(&tx_buf[4], sdo->data, 4);

    ret = can_send(CANOPEN_COBID_SDO_TX, tx_buf, 8);
    if (ret < 0) {
        return ret;
    }

    ret = can_recv(CANOPEN_COBID_SDO_RX, rx_buf, 8, 1000);
    if (ret < 0) {
        return ret;
    }

    if (rx_buf[0] != 0x60) {
        return -1;
    }

    return 0;
}

int canopen_sdo_read(canopen_sdo_t *sdo) {
    uint8_t tx_buf[8];
    uint8_t rx_buf[8];
    int ret;

    tx_buf[0] = 0x40;
    tx_buf[1] = sdo->index & 0xFF;
    tx_buf[2] = (sdo->index >> 8) & 0xFF;
    tx_buf[3] = sdo->subindex;
    memset(&tx_buf[4], 0, 4);

    ret = can_send(CANOPEN_COBID_SDO_TX, tx_buf, 8);
    if (ret < 0) {
        return ret;
    }

    ret = can_recv(CANOPEN_COBID_SDO_RX, rx_buf, 8, 1000);
    if (ret < 0) {
        return ret;
    }

    if (rx_buf[0] != 0x43) {
        return -1;
    }

    memcpy(sdo->data, &rx_buf[4], 4);

    return 0;
}

这个函数包含两个函数：

- `canopen_sdo_write`：用于将数据写入CANopen SDO对象。
- `canopen_sdo_read`：用于从CANopen SDO对象读取数据。

这个函数假定CAN总线的接口已经设置好了，其中`can_send`和`can_recv`函数用于实现CAN数据发送和接收。

使用这个函数的示例代码如下：

#include <stdio.h>
#include "canopen.h"

int main() {
    canopen_sdo_t sdo;
    int ret;

    // 写入数据
    sdo.index = 0x1017;
    sdo.subindex = 0x00;
    sdo.data[0] = 0x01;
    sdo.data[1] = 0x00;
    sdo.data[2] = 0x00;
    sdo.data[3] = 0x00;
    ret = canopen_sdo_write(&sdo);
    if (ret < 0) {
        printf("Error writing SDO: %d\n", ret);
        return -1;
    }

    // 读取数据
    sdo.index = 0x1018;
    sdo.subindex = 0x01;
    memset(sdo.data, 0, 4);
    ret = canopen_sdo_read(&sdo);
    if (ret < 0) {
        printf("Error reading SDO: %d\n", ret);
        return -1;
    }

    printf("SDO data: %02x %02x %02x %02x\n", sdo.data[0], sdo.data[1], sdo.data[2], sdo.data[3]);

    return 0;
}