帮我用C写一段CANopen SDO读写函数

下面是一个简单的CANopen SDO读写函数的示例，使用C语言实现：

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include "can.h"

#define CAN_ID_SDO_TX           0x600        // SDO传输ID
#define CAN_ID_SDO_RX           0x580        // SDO接收ID

#define SDO_COMMAND_READ        0x40        // 读SDO命令
#define SDO_COMMAND_WRITE       0x23        // 写SDO命令

CAN_HandleTypeDef hcan1;                    // CAN总线句柄

// 发送SDO消息
int sdo_send(uint16_t index, uint8_t subindex, uint8_t *data, uint32_t len, uint32_t timeout)
{
    CAN_TxHeaderTypeDef tx_header = {0};
    uint8_t tx_data[8] = {0};

    tx_header.StdId = CAN_ID_SDO_TX;
    tx_header.RTR = CAN_RTR_DATA;
    tx_header.IDE = CAN_ID_STD;
    tx_header.DLC = 8;

    // 构建SDO数据
    tx_data[0] = SDO_COMMAND_WRITE;                 // 写SDO命令
    tx_data[1] = (uint8_t)(index & 0xFF);           // SDO索引低8位
    tx_data[2] = (uint8_t)(index >> 8);             // SDO索引高8位
    tx_data[3] = subindex;                          // SDO子索引
    memcpy(&tx_data[4], data, len);                 // SDO数据

    // 发送CAN消息
    if(HAL_CAN_Transmit(&hcan1, &tx_header, tx_data, timeout) != HAL_OK)
    {
        return -1;
    }

    return 0;
}

// 接收SDO消息
int sdo_recv(uint16_t index, uint8_t subindex, uint8_t *data, uint32_t len, uint32_t timeout)
{
    CAN_RxHeaderTypeDef rx_header = {0};
    uint8_t rx_data[8] = {0};

    while(1)
    {
        // 接收CAN消息
        if(HAL_CAN_Receive(&hcan1, CAN_FIFO0, timeout) != HAL_OK)
        {
            return -1;
        }

        // 判断是否为SDO消息
        if(rx_header.StdId == CAN_ID_SDO_RX && rx_header.RTR == CAN_RTR_DATA && rx_header.IDE == CAN_ID_STD && rx_header.DLC == 8)
        {
            // 判断SDO索引是否匹配
            if(rx_data[1] == (uint8_t)(index & 0xFF) && rx_data[2] == (uint8_t)(index >> 8) && rx_data[3] == subindex)
            {
                // 读取SDO数据
                memcpy(data, &rx_data[4], len);
                return 0;
            }
        }
    }

    return -1;
}

这是一个简单的示例，需要根据具体应用进行修改和优化。同时，需要注意以下几点：

1. `hcan1` 是一个CAN总线的句柄，需要在初始化CAN总线之后进行赋值。
2. `sdo_send` 和 `sdo_recv` 函数都需要设置超时时间，以防止函数一直阻塞。
3. 在 `sdo_recv` 函数中，需要在一个循环中不断接收CAN消息，直到接收到匹配的SDO消息为止。