From 12b2577d6b9b6202916b0dc9a706f00688cdac59 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Fri, 15 May 2026 23:04:04 +0800 Subject: [PATCH] =?UTF-8?q?feat(can):=20=E5=AE=9E=E7=8E=B0CANFD=E5=8D=8F?= =?UTF-8?q?=E8=AE=AE=E6=A0=88=E5=8F=8ADMA=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 添加CANFD DMA接收功能，使用32位宽数据传输提升效率 - 实现CANFD收发队列管理，支持异步消息处理防止丢帧 - 重构CAN中断处理机制，采用FIFO0中断配合DMA完成数据接收 - 添加CANFD DLC长度转换表，规范化数据长度处理 - 实现临界区保护机制，确保多线程环境下的数据安全 - 优化发送队列机制，使用邮箱空闲中断进行消息调度 - 添加DMA错误处理，增强系统稳定性 - 移除原有8字节限制，支持CANFD最大64字节数据传输 --- Robot_V2/device/Can.cpp | 530 ++++--- Robot_V2/device/Can.h | 1 + .../Robot_V2/device/Can.cpp | 1246 +++++++++++++++++ .../Robot_V2/device/Can.h | 169 +++ 4 files changed, 1756 insertions(+), 190 deletions(-) create mode 100644 backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.cpp create mode 100644 backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.h diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 1322d0f..9e7b12f 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -37,26 +37,38 @@ extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式 // 添加 DMA 相关变量 dma_parameter_struct dma_rx_parameter; -dma_parameter_struct dma_tx_parameter; uint8_t can_rx_buffer[64]; // CAN 接收缓冲区（支持CAN FD最大64字节） -uint8_t can_tx_buffer[64]; // CAN 发送缓冲区（支持CAN FD最大64字节） +static uint32_t can_rx_dma_words[16]; // CAN FD接收DMA按32位搬运，最多64字节 +static volatile bool can_rx_dma_busy = false; +static can_receive_message_struct can_rx_dma_message; + +// CAN接收/发送队列结构 +#define CAN_RX_QUEUE_SIZE 32 +#define CAN_TX_QUEUE_SIZE 32 -// CAN发送队列结构 -#define CAN_TX_QUEUE_SIZE 8 typedef struct { - u32 id; - u8 length; - u8 data[64]; - u8 valid; + u32 id; + u8 length; + u8 data[64]; +} can_rx_queue_item_t; + +typedef struct { + can_trasnmit_message_struct message; + u8 valid; } can_tx_queue_item_t; +static can_rx_queue_item_t rx_queue[CAN_RX_QUEUE_SIZE]; +static volatile u8 rx_queue_head = 0; +static volatile u8 rx_queue_tail = 0; +static volatile u8 rx_queue_count = 0; + static can_tx_queue_item_t tx_queue[CAN_TX_QUEUE_SIZE]; static volatile u8 tx_queue_head = 0; static volatile u8 tx_queue_tail = 0; static volatile u8 tx_queue_count = 0; -// 发送队列使用的独立消息结构体 -static can_trasnmit_message_struct queue_transmit_message; +// CAN FD DLC 9~15对应的非线性payload长度表 +static constexpr u8 CANFD_DLC_LENGTH_TABLE[7] = {12, 16, 20, 24, 32, 48, 64}; CAN_transmiter CAN0_transmiter; // CAN 初始化 @@ -288,36 +300,21 @@ void can_dma_init(void) { dma_struct_para_init(&dma_rx_parameter); dma_rx_parameter.periph_addr = (uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0)); // CAN 接收数据寄存器地址 - dma_rx_parameter.memory_addr = (uint32_t)can_rx_buffer; // 接收缓冲区地址 - dma_rx_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; // 外设到内存 - dma_rx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度 8 位 - dma_rx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度 8 位 - dma_rx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级 - dma_rx_parameter.number = 8; // 传输字节数 - dma_rx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // 外设地址不递增 + dma_rx_parameter.memory_addr = (uint32_t)can_rx_dma_words; // 接收 DMA 缓冲区地址 + dma_rx_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; // 外设到内存 + dma_rx_parameter.memory_width = DMA_MEMORY_WIDTH_32BIT; // 内存宽度 32 位 + dma_rx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_32BIT; // 外设宽度 32 位 + dma_rx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级 + dma_rx_parameter.number = 16; // 最多 16 个 word = 64 字节 + dma_rx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // CAN FIFO 数据端口不递增 dma_rx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增 - dma_init(DMA0, DMA_CH4, &dma_rx_parameter); // 使用 DMA 通道 4 + dma_init(DMA0, DMA_CH4, &dma_rx_parameter); // 使用 DMA 通道 4 + dma_memory_to_memory_enable(DMA0, DMA_CH4); // CAN 无独立 DMA request 时，按帧启动 M2M 读取 FIFO 数据端口 - // 配置发送 DMA - dma_struct_para_init(&dma_tx_parameter); - dma_tx_parameter.periph_addr = - (uint32_t)(&CAN_TMI0(CAN0)); // CAN 发送数据寄存器地址 - dma_tx_parameter.memory_addr = (uint32_t)can_tx_buffer; // 发送缓冲区地址 - dma_tx_parameter.direction = DMA_MEMORY_TO_PERIPHERAL; // 内存到外设 - dma_tx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度 8 位 - dma_tx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度 8 位 - dma_tx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级 - dma_tx_parameter.number = 8; // 传输字节数 - dma_tx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // 外设地址不递增 - dma_tx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增 - dma_init(DMA0, DMA_CH5, &dma_tx_parameter); // 使用 DMA 通道 5 - - // 使能 DMA 通道 - dma_channel_enable(DMA0, DMA_CH4); - dma_channel_enable(DMA0, DMA_CH5); - - // 使能 CAN 接收 DMA - // can_dma_enable(CAN0, CAN_DMA_RECEIVE); + // RX DMA 按帧启动，初始化时不使能 CH4，避免无有效帧时搬运垃圾数据 + dma_channel_disable(DMA0, DMA_CH4); + can_rx_dma_busy = false; + memset(can_rx_dma_words, 0, sizeof(can_rx_dma_words)); } /*! @@ -390,7 +387,7 @@ void can_config(void) { */ void canfd_config(void) { can_gpio_init(); - can_dma_init(); // 初始化 DMA + can_dma_init(); can_parameter_struct can_parameter; can_fdframe_struct can_fd_parameter; can_fd_tdc_struct can_fd_tdc_parameter; @@ -451,11 +448,12 @@ void canfd_config(void) { /* configure CAN0 NVIC */ nvic_irq_enable(CAN0_RX0_IRQn, 1, 0); + nvic_irq_enable(CAN0_TX_IRQn, 1, 1); /* enable can receive FIFO0 not empty interrupt */ can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); // 使能 DMA 中断 nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1); - dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF); + dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF | DMA_INT_ERR); } /*! @@ -651,9 +649,19 @@ u8 CAN_transmiter::init() { /* initialize receive message */ can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message); + // 初始化收发队列和标志位 + rx_queue_head = 0; + rx_queue_tail = 0; + rx_queue_count = 0; + tx_queue_head = 0; + tx_queue_tail = 0; + tx_queue_count = 0; + can0_send_flag = RESET; + // 初始化 DMA 缓冲区 memset(can_rx_buffer, 0, sizeof(can_rx_buffer)); - memset(can_tx_buffer, 0, sizeof(can_tx_buffer)); + memset(can_rx_dma_words, 0, sizeof(can_rx_dma_words)); + can_rx_dma_busy = false; // 初始化节点 ID 为默认值 node_id = DEFAULT_NODE_ID; @@ -663,6 +671,216 @@ u8 CAN_transmiter::init() { return 0; } +static uint32_t CAN_EnterCritical(void) { + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + return primask; +} + +static void CAN_ExitCritical(uint32_t primask) { __set_PRIMASK(primask); } + +static u8 CANFD_DlcToLength(u8 dlc) { + return (dlc <= 8U) ? dlc + : ((dlc <= 15U) ? CANFD_DLC_LENGTH_TABLE[dlc - 9U] : 0U); +} + +static u8 CANFD_NormalizeLength(u8 length) { + if (length <= 8U) + return length; + for (u8 i = 0; i < sizeof(CANFD_DLC_LENGTH_TABLE); i++) { + if (length <= CANFD_DLC_LENGTH_TABLE[i]) { + return CANFD_DLC_LENGTH_TABLE[i]; + } + } + return 0U; +} + +static u8 CANFD_WordCount(u8 length) { return (length + 3U) / 4U; } + +static void CAN_RX_QueuePushFromIsr(const can_receive_message_struct &message) { + if (rx_queue_count < CAN_RX_QUEUE_SIZE) { + rx_queue[rx_queue_tail].id = message.rx_efid; + rx_queue[rx_queue_tail].length = message.rx_dlen; + if (rx_queue[rx_queue_tail].length > 64) { + rx_queue[rx_queue_tail].length = 64; + } + for (int i = 0; i < rx_queue[rx_queue_tail].length; i++) { + rx_queue[rx_queue_tail].data[i] = message.rx_data[i]; + } + rx_queue_tail = (rx_queue_tail + 1) % CAN_RX_QUEUE_SIZE; + rx_queue_count++; + } else { + CAN_DEBUG_PRINT("RX queue full, message dropped\r\n"); + } +} + +static void CAN_RX_ReleaseFifo0(void) { CAN_RFIFO0(CAN0) |= CAN_RFIFO0_RFD0; } + +static u8 CAN_RX_Fifo0PendingCount(void) { + return can_receive_message_length_get(CAN0, CAN_FIFO0); +} + +static bool CAN_TX_HasEmptyMailbox(void) { + return (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET) || + (can_flag_get(CAN0, CAN_FLAG_TME1) != RESET) || + (can_flag_get(CAN0, CAN_FLAG_TME2) != RESET); +} + +static void CAN_RX_ReadMetadata(can_receive_message_struct *message) { + uint8_t dlc; + uint32_t rfifomi = CAN_RFIFOMI(CAN0, CAN_FIFO0); + uint32_t rfifomp = CAN_RFIFOMP(CAN0, CAN_FIFO0); + uint32_t canfd_en = CAN_FDCTL(CAN0) & CAN_FDCTL_FDEN; + + can_struct_para_init(CAN_RX_MESSAGE_STRUCT, message); + message->rx_ff = (uint8_t)(CAN_RFIFOMI_FF & rfifomi); + if (CAN_FF_STANDARD == message->rx_ff) { + message->rx_sfid = (uint32_t)(GET_RFIFOMI_SFID(rfifomi)); + } else { + message->rx_efid = (uint32_t)(GET_RFIFOMI_EFID(rfifomi)); + } + message->rx_ft = (uint8_t)(CAN_RFIFOMI_FT & rfifomi); + message->rx_fi = (uint8_t)(GET_RFIFOMP_FI(rfifomp)); + message->fd_flag = (uint8_t)((CAN_RFIFOMP_FDF & rfifomp) >> 7); + dlc = (uint8_t)(GET_RFIFOMP_DLENC(rfifomp)); + if ((canfd_en != 0U) && (message->fd_flag != CAN_FDF_CLASSIC)) { + message->rx_dlen = CANFD_DlcToLength(dlc); + message->fd_brs = (uint8_t)((rfifomp & CAN_RFIFOMP_BRS) >> 5); + message->fd_esi = (uint8_t)((rfifomp & CAN_RFIFOMP_ESI) >> 4); + } else { + message->rx_dlen = (dlc <= 8U) ? dlc : 8U; + } +} + +static void CAN_RX_StartPayloadDma(const can_receive_message_struct &metadata) { + can_rx_dma_message = metadata; + if (can_rx_dma_message.rx_dlen > 64) { + can_rx_dma_message.rx_dlen = 64; + } + + u8 word_count = CANFD_WordCount(can_rx_dma_message.rx_dlen); + if (word_count == 0) { + CAN_RX_QueuePushFromIsr(can_rx_dma_message); + CAN_RX_ReleaseFifo0(); + return; + } + + can_rx_dma_busy = true; + dma_channel_disable(DMA0, DMA_CH4); + dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G); + dma_periph_address_config(DMA0, DMA_CH4, + (uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0))); + dma_memory_address_config(DMA0, DMA_CH4, (uint32_t)can_rx_dma_words); + dma_transfer_number_config(DMA0, DMA_CH4, word_count); + dma_channel_enable(DMA0, DMA_CH4); +} + +static void CAN_RX_PumpFifo0(void) { + while (!can_rx_dma_busy) { + if (CAN_RX_Fifo0PendingCount() == 0U) { + can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); + return; + } + + can_receive_message_struct temp_message; + CAN_RX_ReadMetadata(&temp_message); + + if (temp_message.rx_ff == CAN_FF_EXTENDED) { + CAN_RX_StartPayloadDma(temp_message); + } else { + CAN_RX_ReleaseFifo0(); + CAN_DEBUG_PRINT("Drop standard CAN frame in CANFD path\r\n"); + } + } +} + +static u8 CAN_RX_QueuePop(can_rx_queue_item_t *item) { + u8 popped = 0U; + uint32_t primask = CAN_EnterCritical(); + if (rx_queue_count == 0) { + CAN_ExitCritical(primask); + return 0U; + } + *item = rx_queue[rx_queue_head]; + rx_queue_head = (rx_queue_head + 1) % CAN_RX_QUEUE_SIZE; + rx_queue_count--; + popped = 1U; + CAN_ExitCritical(primask); + + return popped; +} + +static u8 CAN_TX_QueuePush(const can_trasnmit_message_struct &message) { + u8 queued = 0U; + uint32_t primask = CAN_EnterCritical(); + if (tx_queue_count < CAN_TX_QUEUE_SIZE) { + tx_queue[tx_queue_tail].message = message; + tx_queue[tx_queue_tail].valid = 1U; + tx_queue_tail = (tx_queue_tail + 1U) % CAN_TX_QUEUE_SIZE; + tx_queue_count++; + queued = 1U; + } + CAN_ExitCritical(primask); + + if (queued != 0U) { + can_interrupt_enable(CAN0, CAN_INTEN_TMEIE); + return 1U; + } + + CAN_DEBUG_PRINT("TX queue full, message dropped\r\n"); + return 0U; +} + +static u8 CAN_TX_QueuePeek(can_trasnmit_message_struct *message) { + u8 has_message = 0U; + uint32_t primask = CAN_EnterCritical(); + if (tx_queue_count != 0U) { + *message = tx_queue[tx_queue_head].message; + has_message = 1U; + } + CAN_ExitCritical(primask); + return has_message; +} + +static u8 CAN_TX_QueuePop(can_trasnmit_message_struct *message) { + u8 popped = 0U; + uint32_t primask = CAN_EnterCritical(); + if (tx_queue_count != 0U) { + *message = tx_queue[tx_queue_head].message; + tx_queue[tx_queue_head].valid = 0U; + tx_queue_head = (tx_queue_head + 1U) % CAN_TX_QUEUE_SIZE; + tx_queue_count--; + popped = 1U; + } + CAN_ExitCritical(primask); + return popped; +} + +static void CAN_TX_DisableTmeIfQueueEmpty(void) { + uint32_t primask = CAN_EnterCritical(); + if (tx_queue_count == 0U) { + can_interrupt_disable(CAN0, CAN_INTEN_TMEIE); + } + CAN_ExitCritical(primask); +} + +static void CAN_TX_ServiceQueue(void) { + can_trasnmit_message_struct queued_message; + while (CAN_TX_HasEmptyMailbox()) { + if (CAN_TX_QueuePeek(&queued_message) == 0U) { + CAN_TX_DisableTmeIfQueueEmpty(); + return; + } + if (can_message_transmit(CAN0, &queued_message) == CAN_NOMAILBOX) { + CAN_DEBUG_PRINT( + "Mailbox became unavailable while draining TX queue\r\n"); + can_interrupt_enable(CAN0, CAN_INTEN_TMEIE); + return; + } + CAN_TX_QueuePop(&queued_message); + } +} + /*! \brief CAN 数据更新 */ @@ -757,12 +975,18 @@ void CAN_transmiter::communication_check() { can0_send_flag = RESET; } - // 处理所有接收到的 CAN 消息（从环形缓冲区中） - can_receive_message_struct current_message; - while (dequeue_rx_message(current_message)) { + // 处理所有接收到的 CANFD 消息（从底层中断队列中取出） + can_rx_queue_item_t rx_item; + while (CAN_RX_QueuePop(&rx_item)) { + can_receive_message_struct current_message; + can_struct_para_init(CAN_RX_MESSAGE_STRUCT, ¤t_message); + current_message.rx_efid = rx_item.id; + current_message.rx_dlen = rx_item.length; + memcpy(current_message.rx_data, rx_item.data, rx_item.length); + // 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分) u32 base_id = current_message.rx_efid & 0xFFFF; - + // 提取发送方的节点 ID u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF); @@ -771,11 +995,14 @@ void CAN_transmiter::communication_check() { // 判断是广播消息还是其他节点的命令 if (sender_node_id == BROADCAST_NODE_ID || sender_node_id != node_id) { // 广播消息或其他节点的命令 → 进入静默模式（停发 10 秒） - CAN_DEBUG_PRINT("Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n", sender_node_id); + CAN_DEBUG_PRINT( + "Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n", + sender_node_id); enter_silent_mode(); } else { // 自己节点的命令 → 执行 BOOT 功能 - CAN_DEBUG_PRINT("Received base_id=0x0000 from self, jumping to BOOT\r\n"); + CAN_DEBUG_PRINT( + "Received base_id=0x0000 from self, jumping to BOOT\r\n"); motor_sleep(); flashEEP.save_system_config(false); fmc_unlock(); @@ -800,8 +1027,7 @@ void CAN_transmiter::communication_check() { if (current_message.rx_dlen >= 2) { new_node_id = unpack_bytes_to_u16(current_message.rx_data, 0); } else { - new_node_id = - current_message.rx_data[0]; // 只发送 1 字节时直接使用 + new_node_id = current_message.rx_data[0]; // 只发送 1 字节时直接使用 } u16 old_node_id = node_id; // 保存旧节点 ID 用于构建响应 ID CAN_DEBUG_PRINT( @@ -811,44 +1037,47 @@ void CAN_transmiter::communication_check() { if (new_node_id <= MAX_NODE_ID && new_node_id != BROADCAST_NODE_ID) { // 先发送响应（使用旧节点ID构建CAN ID） // 响应数据格式：[status:1byte][new_node_id:2byte] = 3 字节 - transmit_data[0] = 0x01; // 成功 + transmit_data[0] = 0x01; // 成功 transmit_data[1] = (u8)(new_node_id & 0xFF); // 新 ID 低字节 transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF); // 新 ID 高字节 - + // 使用旧节点 ID 发送响应 node_id = old_node_id; u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, - transmit_data); - + transmit_data); + // 然后保存新节点 ID 到 Flash u8 save_status = save_node_id_to_flash(new_node_id); - - CAN_DEBUG_PRINT("SET_NODE_ID: save_status=%d, send_result=%d\r\n", - save_status, send_result); - + + CAN_DEBUG_PRINT( + "SET_NODE_ID: save_status=%d, send_result=%d\r\n", + save_status, send_result); + if (save_status == FLASH_EEPROM_OK) { - CAN_DEBUG_PRINT("SET_NODE_ID: SUCCESS, node_id updated from %d to %d\r\n", - old_node_id, new_node_id); + CAN_DEBUG_PRINT( + "SET_NODE_ID: SUCCESS, node_id updated from %d to %d\r\n", + old_node_id, new_node_id); } else { - CAN_DEBUG_PRINT("SET_NODE_ID: Flash write FAILED, status=%d\r\n", - save_status); + CAN_DEBUG_PRINT( + "SET_NODE_ID: Flash write FAILED, status=%d\r\n", + save_status); // 发送错误响应 transmit_data[0] = 0x00; // 失败 node_id = old_node_id; CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, - transmit_data); + transmit_data); } } else { // 发送错误响应 - 使用旧节点 ID 构建响应 ID CAN_DEBUG_PRINT("SET_NODE_ID: invalid new_id=%d (max=%d)\r\n", - new_node_id, MAX_NODE_ID); + new_node_id, MAX_NODE_ID); transmit_data[0] = 0x00; // 失败 transmit_data[1] = (u8)(new_node_id & 0xFF); transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF); node_id = old_node_id; // 临时恢复旧 ID 用于构建响应 CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, - transmit_data); + transmit_data); // 保持原 ID 不变 } continue; // SET_NODE_ID 命令处理完跳过后续逻辑 @@ -919,7 +1148,7 @@ void CAN_transmiter::communication_check() { if (first_byte >> 3 & 0x01) { KTH7823.angle_set(0); Angle_kalman.init(0); // 角度滤波 - Vel_kalman.init(0); // 角度滤波 + Vel_kalman.init(0); // 角度滤波 } // 休眠 if (eigth_byte & 0x80 >> 7) { @@ -939,7 +1168,7 @@ void CAN_transmiter::communication_check() { delay_1ms(1); if (app_enable == OTA_AVAL && app_check == OTA_AVAL) { CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2, - transmit_data); + transmit_data); } } // 查询节点 ID 命令 (基础 ID: 0x0402) @@ -948,7 +1177,7 @@ void CAN_transmiter::communication_check() { CANFD_AsyncSend(build_can_id(CANFD_GET_NODE_ID), 2, transmit_data); } else { CAN_DEBUG_PRINT("Unknown CAN ID: %X (node_id=%d)\n", - current_message.rx_efid, node_id); + current_message.rx_efid, node_id); } } } @@ -982,17 +1211,10 @@ u8 CAN_transmiter::CAN_Wakeup() { * @retval 无 */ void CAN0_RX0_IRQHandler(void) { - // 检查是否是 FIFO0 消息挂起中断 - if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFL0) != RESET) { - // 清除中断标志 - can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0); - - // 直接读取消息（包括数据），不依赖DMA - can_receive_message_struct temp_message; - can_message_receive(CAN0, CAN_FIFO0, &temp_message); - - // 将接收到的消息加入环形缓冲区 - CAN0_transmiter.enqueue_rx_message_from_isr(temp_message); + // FIFO0 非空时，关中断并持续抽空 FIFO；DMA 完成后由 DMA ISR 继续续传。 + if (CAN_RX_Fifo0PendingCount() != 0U) { + can_interrupt_disable(CAN0, CAN_INTEN_RFNEIE0); + CAN_RX_PumpFifo0(); } // 检查其他可能的中断源 @@ -1014,14 +1236,32 @@ void CAN0_RX0_IRQHandler(void) { * @retval 无 */ void DMA0_Channel4_IRQHandler(void) { + if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_ERR)) { + dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G); + dma_channel_disable(DMA0, DMA_CH4); + if (can_rx_dma_busy) { + CAN_RX_ReleaseFifo0(); + can_rx_dma_busy = false; + } + CAN_DEBUG_PRINT("CAN RX DMA error, frame dropped\r\n"); + CAN_RX_PumpFifo0(); + return; + } + if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_FTF)) { // 清除 DMA 传输完成中断标志 dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G); - // 将 DMA 缓冲区的数据复制到接收消息结构体 - for (int i = 0; i < CAN0_transmiter.receive_message.rx_dlen && i < 8; - i++) { - CAN0_transmiter.receive_message.rx_data[i] = can_rx_buffer[i]; + dma_channel_disable(DMA0, DMA_CH4); + if (!can_rx_dma_busy) { + CAN_RX_PumpFifo0(); + return; } + memcpy(can_rx_dma_message.rx_data, can_rx_dma_words, + can_rx_dma_message.rx_dlen); + CAN_RX_QueuePushFromIsr(can_rx_dma_message); + CAN_RX_ReleaseFifo0(); + can_rx_dma_busy = false; + CAN_RX_PumpFifo0(); } } @@ -1030,12 +1270,7 @@ void DMA0_Channel4_IRQHandler(void) { * @retval 无 */ void DMA0_Channel5_IRQHandler(void) { - if (dma_interrupt_flag_get(DMA0, DMA_CH5, DMA_INT_FLAG_FTF)) { - // DMA 传输完成，可以在此处执行后续处理 - // 清除 DMA 传输完成中断标志 - dma_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_FTF); - // 可以在此添加后续处理逻辑，如发送下一个数据包等 - } + // 保留空实现：发送链路改用 CAN mailbox + TME 中断队列。 } /** @@ -1087,50 +1322,6 @@ u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) { return 1; } -/** - * @brief 向发送队列添加消息 - * @param id CAN消息ID - * @param length CAN消息长度 - * @param data CAN消息数据 - * @retval 1=成功, 0=队列满 - */ -static u8 CAN_TX_QueuePush(u32 id, u8 length, u8 *data) { - if (tx_queue_count >= CAN_TX_QUEUE_SIZE) { - CAN_DEBUG_PRINT("TX queue full, message dropped\r\n"); - return 0; // 队列满，丢弃消息 - } - - tx_queue[tx_queue_tail].id = id; - tx_queue[tx_queue_tail].length = length; - for (int i = 0; i < length && i < 64; i++) { - tx_queue[tx_queue_tail].data[i] = data[i]; - } - tx_queue[tx_queue_tail].valid = 1; - - tx_queue_tail = (tx_queue_tail + 1) % CAN_TX_QUEUE_SIZE; - tx_queue_count++; - - return 1; -} - -/** - * @brief 从发送队列取出消息 - * @param item 指向队列项的指针 - * @retval 1=成功, 0=队列空 - */ -static u8 CAN_TX_QueuePop(can_tx_queue_item_t *item) { - if (tx_queue_count == 0) { - return 0; // 队列空 - } - - *item = tx_queue[tx_queue_head]; - tx_queue[tx_queue_head].valid = 0; - tx_queue_head = (tx_queue_head + 1) % CAN_TX_QUEUE_SIZE; - tx_queue_count--; - - return 1; -} - /** * @brief 异步 CANFD 数据发送 (使用队列机制防止丢帧) * @param id CAN 消息 ID @@ -1138,42 +1329,32 @@ static u8 CAN_TX_QueuePop(can_tx_queue_item_t *item) { * @param data CAN 消息数据 */ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) { - u8 valid_length = length; + u8 valid_length = 0U; // 检查 CANFD 模式是否启用 - if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) { + if ((CAN_FDCTL(CAN0) & CAN_FDCTL_FDEN) == 0U) { CAN_DEBUG_PRINT("CANFD mode not enabled\r\n"); return 0; } // 检查数据指针是否有效 - if (data == nullptr) { + if (data == nullptr && length != 0U) { CAN_DEBUG_PRINT("Invalid data pointer\r\n"); return 0; } - // 检查数据长度是否符合 CANFD 规范 - if (length > 8) { - if (length > 8 and length < 13) { - valid_length = 12; - } else if (length > 12 and length < 17) { - valid_length = 16; - } else if (length > 16 and length < 21) { - valid_length = 20; - } else if (length > 20 and length < 25) { - valid_length = 24; - } else if (length > 24 and length < 33) { - valid_length = 32; - } else if (length > 32 and length < 49) { - valid_length = 48; - } else if (length > 48 and length < 65) { - valid_length = 64; - } else { - CAN_DEBUG_PRINT("CANFD data length error\r\n"); - return 0; - } + // 检查数据长度是否符合 CANFD 规范，并映射到合法 DLC 长度 + if (length == 0U) { + valid_length = 0U; + } else { + valid_length = CANFD_NormalizeLength(length); + } + if ((length != 0U) && (valid_length == 0U)) { + CAN_DEBUG_PRINT("CANFD data length error\r\n"); + return 0; } /* 配置发送消息参数 */ + can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message); transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式 transmit_message.tx_efid = id; // 扩展帧 ID @@ -1183,12 +1364,10 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) { transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD 帧格式 transmit_message.fd_brs = ENABLE; // 使能比特率切换 transmit_message.fd_esi = DISABLE; // 错误状态指示 - // 复制数据到发送缓冲区 + memset(transmit_message.tx_data, 0, sizeof(transmit_message.tx_data)); for (int i = 0; i < valid_length; i++) { if (i < length) { transmit_message.tx_data[i] = *(data + i); - } else { - transmit_message.tx_data[i] = 0; } } @@ -1199,7 +1378,7 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) { if (transmit_mailbox == CAN_NOMAILBOX) { // 没有可用的 mailbox，将消息加入发送队列 CAN_DEBUG_PRINT("No available mailbox, adding to TX queue\r\n"); - return CAN_TX_QueuePush(id, valid_length, data); + return CAN_TX_QueuePush(transmit_message); } // 启用 CAN 发送中断以检测传输完成（用于队列发送） @@ -1211,36 +1390,7 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) { * @brief CAN 传输中断处理函数（支持队列发送） */ void CAN0_TX_IRQHandler(void) { - // 检查是否有发送邮箱空闲 - if (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET || - can_flag_get(CAN0, CAN_FLAG_TME1) != RESET || - can_flag_get(CAN0, CAN_FLAG_TME2) != RESET) { - - can_tx_queue_item_t next_msg; - // 尝试从队列中取出下一条消息发送 - if (CAN_TX_QueuePop(&next_msg)) { - // 配置并发送队列中的消息（使用独立的队列消息结构体） - queue_transmit_message.tx_ft = CAN_FT_DATA; - queue_transmit_message.tx_ff = CAN_FF_EXTENDED; - queue_transmit_message.tx_efid = next_msg.id; - queue_transmit_message.tx_dlen = next_msg.length; - queue_transmit_message.fd_flag = CAN_FDF_FDFRAME; - queue_transmit_message.fd_brs = ENABLE; - queue_transmit_message.fd_esi = DISABLE; - - for (int i = 0; i < next_msg.length; i++) { - queue_transmit_message.tx_data[i] = next_msg.data[i]; - } - - can_message_transmit(CAN0, &queue_transmit_message); - } else { - // 队列为空，禁用发送中断 - can_interrupt_disable(CAN0, CAN_INTEN_TMEIE); - } - - // 清除发送中断标志 - can_flag_clear(CAN0, CAN_FLAG_TME0); - can_flag_clear(CAN0, CAN_FLAG_TME1); - can_flag_clear(CAN0, CAN_FLAG_TME2); + if (CAN_TX_HasEmptyMailbox()) { + CAN_TX_ServiceQueue(); } } diff --git a/Robot_V2/device/Can.h b/Robot_V2/device/Can.h index 769f9af..3c4af8a 100644 --- a/Robot_V2/device/Can.h +++ b/Robot_V2/device/Can.h @@ -57,6 +57,7 @@ u8 can_gpio_init(void); // CAN 引脚初始化 void can_config(void); // CAN 初始化 void canfd_config(void); // CANFD 初始化 void can_dma_init(void); // CAN DMA 初始化 +void CAN0_TX_IRQHandler(void); // CAN 发送中断处理函数 void CAN0_RX0_IRQHandler(void); // CAN 接收中断处理函数 void DMA0_Channel4_IRQHandler(void); // DMA 通道 4 中断处理函数 (CAN 接收) void DMA0_Channel5_IRQHandler(void); // DMA 通道 5 中断处理函数 (CAN 发送) diff --git a/backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.cpp b/backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.cpp new file mode 100644 index 0000000..1322d0f --- /dev/null +++ b/backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.cpp @@ -0,0 +1,1246 @@ +#include "Can.h" +#include "FOC.h" +#include "Filter.h" +#include "adc.h" +#include "ang_sensor.h" +#include "config.h" +#include "flash.h" +#include "gd32c10x.h" +#include "gd32c10x_can.h" +#include "gd32c10x_dma.h" +#include "gd32c10x_fmc.h" +#include "gd32c10x_gpio.h" +#include "initial.h" +#include "motor.h" +#include "systick.h" +#include +#include +#include +#include + +// CAN 调试宏定义 +#if CAN_DEBUG +#define CAN_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__) +#else +#define CAN_DEBUG_PRINT(fmt, ...) ((void)0) +#endif + +constexpr u16 CANX = CAN0; +extern FOC_Controller FOC; +extern sampler sampler; +extern angle_sensor KTH7823; +extern EncoderAngleKalman Angle_kalman; // 角度滤波 +extern EncoderVelocityKalman Vel_kalman; // 速度滤波 +extern FlashEEPROM flashEEP; +extern VersionManager Version; // 声明外部版本管理器实例 +extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式计时） + +// 添加 DMA 相关变量 +dma_parameter_struct dma_rx_parameter; +dma_parameter_struct dma_tx_parameter; +uint8_t can_rx_buffer[64]; // CAN 接收缓冲区（支持CAN FD最大64字节） +uint8_t can_tx_buffer[64]; // CAN 发送缓冲区（支持CAN FD最大64字节） + +// CAN发送队列结构 +#define CAN_TX_QUEUE_SIZE 8 +typedef struct { + u32 id; + u8 length; + u8 data[64]; + u8 valid; +} can_tx_queue_item_t; + +static can_tx_queue_item_t tx_queue[CAN_TX_QUEUE_SIZE]; +static volatile u8 tx_queue_head = 0; +static volatile u8 tx_queue_tail = 0; +static volatile u8 tx_queue_count = 0; + +// 发送队列使用的独立消息结构体 +static can_trasnmit_message_struct queue_transmit_message; + +CAN_transmiter CAN0_transmiter; // CAN 初始化 + +// ============================================================================ +// 节点 ID 相关函数实现 (多电机 CAN 总线隔离) +// 使用高 16 位 (bits 31-16) 存储节点 ID，低 16 位 (bits 15-0) 存储基础 ID +// 格式：[节点 ID(16 位)][基础 ID(16 位)] +// 例如：node_id=1, base_id=0x0200 → 0x00010200 +// ============================================================================ + +/*! + \brief 根据节点 ID 构建 CAN ID + \param[in] base_id 基础 CAN ID(低 16 位) + \param[out] none + \retval 构建后的完整 CAN ID + \note 将节点 ID 放在高 16 位，基础 ID 放在低 16 位 + 格式：[节点 ID(16 位)][基础 ID(16 位)] + 例如：node_id=1, base_id=0x0200 → 0x00010200 +*/ +u32 CAN_transmiter::build_can_id(u32 base_id) { + return ((u32)node_id << 16) | (base_id & 0xFFFF); +} + +/*! + \brief 判断接收到的消息是否是发给本节点的 + \param[in] received_id 接收到的 CAN ID + \param[out] none + \retval true: 消息是发给本节点的 (或广播消息) + false: 消息不是发给本节点的 + \note 支持广播地址 (0xFFFF)，广播消息所有节点都会响应 + 提取高 16 位进行节点 ID 匹配 +*/ +bool CAN_transmiter::is_message_for_me(u32 received_id) { + u16 rx_node_id = (u16)((received_id >> 16) & 0xFFFF); + return (rx_node_id == BROADCAST_NODE_ID) || (rx_node_id == node_id); +} + +/*! + \brief 获取当前节点 ID + \param[in] none + \param[out] none + \retval 当前节点 ID 值 (0-65534) +*/ +u16 CAN_transmiter::get_node_id() { return node_id; } + +/*! + \brief 设置节点 ID + \param[in] new_id 新的节点 ID 值 + \param[out] none + \retval none + \note 节点 ID 范围：0-65534，0xFFFF 保留为广播地址 +*/ +void CAN_transmiter::set_node_id(u16 new_id) { + if (new_id <= MAX_NODE_ID) { + node_id = new_id; + CAN_DEBUG_PRINT("Node ID set to: %d\r\n", node_id); + } +} + +/*! + \brief 从 Flash 加载节点 ID（使用魔数+双重备份+计数器机制） + \param[in] none + \param[out] none + \retval FLASH_EEPROM_OK: 加载成功 + 其他：加载失败，使用默认值 + \note 使用独立的节点 ID 存储区域，带魔数验证和 CRC 校验 + 防止位翻转导致错读 +*/ +u8 CAN_transmiter::load_node_id_from_flash() { + u16 node_id_temp = 0; + FlashEepromStatus status = flashEEP.readNodeId(&node_id_temp); + + // 检查读取是否成功 + if (status != FLASH_EEPROM_OK) { + node_id = DEFAULT_NODE_ID; + CAN_DEBUG_PRINT( + "Node ID not found in Flash (status=%d), using default: %d\r\n", + status, node_id); + return FLASH_EEPROM_DATA_NOT_FOUND; + } + + // 检查节点 ID 是否有效 (0-65534 都是有效值) + if (node_id_temp <= MAX_NODE_ID) { + node_id = node_id_temp; + CAN_DEBUG_PRINT("Node ID loaded from Flash: %d\r\n", node_id); + return FLASH_EEPROM_OK; + } else { + node_id = DEFAULT_NODE_ID; + CAN_DEBUG_PRINT("Invalid Node ID in Flash (%d), using default: %d\r\n", + node_id_temp, node_id); + return FLASH_EEPROM_DATA_NOT_FOUND; + } +} + +/*! + \brief 保存节点 ID 到 Flash（使用魔数+双重备份+计数器机制） + \param[in] new_id 要保存的节点 ID + \param[out] none + \retval FLASH_EEPROM_OK: 保存成功 + 其他：保存失败 + \note 写入主备份和备用备份，确保数据可靠性 +*/ +u8 CAN_transmiter::save_node_id_to_flash(u16 new_id) { + if (new_id > MAX_NODE_ID) { + CAN_DEBUG_PRINT("Invalid Node ID: %d\r\n", new_id); + return FLASH_EEPROM_INVALID_ADDR; + } + + FlashEepromStatus status = flashEEP.writeNodeId(new_id); + if (status == FLASH_EEPROM_OK) { + node_id = new_id; + CAN_DEBUG_PRINT("Node ID saved to Flash: %d\r\n", node_id); + return FLASH_EEPROM_OK; + } else { + CAN_DEBUG_PRINT("Failed to save Node ID to Flash (status=%d)\r\n", status); + return (u8)status; + } +} + +// CAN 接收缓冲区相关函数实现 +/*! + \brief 检查 CAN 接收缓冲区是否已满 + \param[in] none + \param[out] none + \retval bool 返回 true 表示缓冲区已满，false 表示未满 + \note 通过比较头指针的下一个位置和尾指针来判断缓冲区是否已满 + 使用循环缓冲区策略，当(rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE + 等于 rx_buffer_tail 时， 表示缓冲区已达到最大容量 +*/ +bool CAN_transmiter::is_rx_buffer_full() { + return ((rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE) == rx_buffer_tail; +} + +/*! + \brief 检查 CAN 接收缓冲区是否为空 + \param[in] none + \param[out] none + \retval bool 返回 true 表示缓冲区为空，false 表示缓冲区不为空 + \note 通过比较头指针和尾指针是否相等来判断缓冲区是否为空 +*/ +bool CAN_transmiter::is_rx_buffer_empty() { + return rx_buffer_head == rx_buffer_tail; +} + +/** + * @brief 从接收缓冲区中取出一条 CAN 消息 + * + * 该函数检查接收缓冲区是否为空，如果不为空则取出缓冲区队列头部的消息， + * 并更新队列尾部索引。如果缓冲区为空，则返回 false。 + * + * @param message 引用类型的输出参数，用于存储取出的 CAN 接收消息结构体 + * @return bool 返回操作是否成功，true 表示成功取出消息，false 表示缓冲区为空 + */ +bool CAN_transmiter::dequeue_rx_message(can_receive_message_struct &message) { + if (!is_rx_buffer_empty()) { + message = rx_message_buffer[rx_buffer_tail]; + rx_buffer_tail = (rx_buffer_tail + 1) % CAN_RX_BUFFER_SIZE; + return true; + } + return false; +} + +// 静默模式相关函数实现 +// 使用 systick 获取系统时间（ms） +extern u32 get_systick_ms(void); + +/*! + \brief 进入静默模式（停发 10 秒） +*/ +void CAN_transmiter::enter_silent_mode() { + is_silent = true; + silent_start_time = get_systick_ms(); // 记录当前时间（ms） + CAN_DEBUG_PRINT("Enter silent mode for %d ms, start_time=%d\r\n", CAN_SILENT_DURATION_MS, silent_start_time); +} + +/*! + \brief 检查是否在静默模式中 +*/ +bool CAN_transmiter::is_in_silent_mode() { return is_silent; } + +/*! + \brief 检查静默模式是否结束 +*/ +void CAN_transmiter::check_silent_mode() { + if (is_silent) { + u32 current_time = get_systick_ms(); // 当前时间（ms） + if (current_time - silent_start_time >= CAN_SILENT_DURATION_MS) { + is_silent = false; + CAN_DEBUG_PRINT("Exit silent mode, elapsed=%d ms\r\n", current_time - silent_start_time); + } + } +} + +/*! + \brief initialize CAN GPIO + \param[in] none + \param[out] none + \retval ErrStatus: SUCCESS or ERROR +*/ +u8 can_gpio_init() { + /* enable CAN clock */ + rcu_periph_clock_enable(RCU_CAN0); + rcu_periph_clock_enable(RCU_GPIOB); + rcu_periph_clock_enable(RCU_AF); + + /* configure CAN0 GPIO */ + gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_8); // rx + gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_9); // tx + gpio_pin_remap_config(GPIO_CAN0_PARTIAL_REMAP, ENABLE); + /* enable the EN GPIO clock */ + gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, + GPIO_PIN_13 | GPIO_PIN_14); + gpio_bit_reset(GPIOB, GPIO_PIN_13); + gpio_bit_reset(GPIOB, GPIO_PIN_14); + return 0; +} + +/*! + \brief 初始化 CAN DMA + \param[in] none + \param[out] none + \retval none +*/ +void can_dma_init(void) { + // 使能 DMA 时钟 + rcu_periph_clock_enable(RCU_DMA0); + + // 配置接收 DMA + dma_struct_para_init(&dma_rx_parameter); + dma_rx_parameter.periph_addr = + (uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0)); // CAN 接收数据寄存器地址 + dma_rx_parameter.memory_addr = (uint32_t)can_rx_buffer; // 接收缓冲区地址 + dma_rx_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; // 外设到内存 + dma_rx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度 8 位 + dma_rx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度 8 位 + dma_rx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级 + dma_rx_parameter.number = 8; // 传输字节数 + dma_rx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // 外设地址不递增 + dma_rx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增 + dma_init(DMA0, DMA_CH4, &dma_rx_parameter); // 使用 DMA 通道 4 + + // 配置发送 DMA + dma_struct_para_init(&dma_tx_parameter); + dma_tx_parameter.periph_addr = + (uint32_t)(&CAN_TMI0(CAN0)); // CAN 发送数据寄存器地址 + dma_tx_parameter.memory_addr = (uint32_t)can_tx_buffer; // 发送缓冲区地址 + dma_tx_parameter.direction = DMA_MEMORY_TO_PERIPHERAL; // 内存到外设 + dma_tx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度 8 位 + dma_tx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度 8 位 + dma_tx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级 + dma_tx_parameter.number = 8; // 传输字节数 + dma_tx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // 外设地址不递增 + dma_tx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增 + dma_init(DMA0, DMA_CH5, &dma_tx_parameter); // 使用 DMA 通道 5 + + // 使能 DMA 通道 + dma_channel_enable(DMA0, DMA_CH4); + dma_channel_enable(DMA0, DMA_CH5); + + // 使能 CAN 接收 DMA + // can_dma_enable(CAN0, CAN_DMA_RECEIVE); +} + +/*! + \brief 初始化 CAN0 2.0B 模式 + \param[in] none + \param[out] none + \retval none +*/ +void can_config(void) { + can_gpio_init(); + can_dma_init(); // 初始化 DMA + can_parameter_struct can_parameter; + can_filter_parameter_struct can_filter; + + can_struct_para_init(CAN_INIT_STRUCT, &can_parameter); + /* initialize CAN register */ + can_deinit(CAN0); + + /* initialize CAN parameters */ + can_parameter.time_triggered = DISABLE; + can_parameter.auto_bus_off_recovery = DISABLE; + can_parameter.auto_wake_up = DISABLE; + can_parameter.auto_retrans = DISABLE; + can_parameter.rec_fifo_overwrite = DISABLE; + can_parameter.trans_fifo_order = DISABLE; + can_parameter.working_mode = CAN_NORMAL_MODE; + /* configure CAN0 baud rate 1MBps, sample point at 80% */ + can_parameter.resync_jump_width = CAN_BT_SJW_1TQ; + can_parameter.time_segment_1 = CAN_BT_BS1_7TQ; + can_parameter.time_segment_2 = CAN_BT_BS2_2TQ; + can_parameter.prescaler = 6; + /* initialize CAN */ + if (can_init(CAN0, &can_parameter) != SUCCESS) { + while (1) + CAN_DEBUG_PRINT("can init error \r\n"); + } + + /* initialize filter */ + /* configure filter mode */ + can_filter.filter_mode = CAN_FILTERMODE_MASK; + can_filter.filter_bits = CAN_FILTERBITS_32BIT; + /* configure filter mask */ + can_filter.filter_mask_high = 0x0000; + can_filter.filter_mask_low = 0x0000; + /* select receiver fifo */ + can_filter.filter_fifo_number = CAN_FIFO0; + can_filter.filter_enable = ENABLE; + /* configure filter number */ + can_filter.filter_number = 0; + can_filter_init(&can_filter); + + /* configure CAN0 NVIC */ + nvic_irq_enable(CAN0_RX0_IRQn, 1, 0); + nvic_irq_enable(CAN0_TX_IRQn, 1, 1); // 添加发送中断 + /* enable can receive FIFO0 not empty interrupt */ + can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); + /* enable can transmit mailbox empty interrupt */ + can_interrupt_enable(CAN0, CAN_INTEN_TMEIE); + + // 使能 DMA 中断 + nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1); + dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF); +} + +/*! + \brief 初始化 CAN0 CANFD 模式 + \param[in] none + \param[out] none + \retval none +*/ +void canfd_config(void) { + can_gpio_init(); + can_dma_init(); // 初始化 DMA + can_parameter_struct can_parameter; + can_fdframe_struct can_fd_parameter; + can_fd_tdc_struct can_fd_tdc_parameter; + can_filter_parameter_struct can_filter; + + /* initialize CAN register */ + can_deinit(CAN0); + + /* initialize CAN */ + can_parameter.time_triggered = DISABLE; + can_parameter.auto_bus_off_recovery = DISABLE; + can_parameter.auto_wake_up = DISABLE; + can_parameter.auto_retrans = ENABLE; + can_parameter.rec_fifo_overwrite = ENABLE; + can_parameter.trans_fifo_order = ENABLE; + can_parameter.working_mode = CAN_NORMAL_MODE; + /* baudrate 1Mbps, sample piont at 80% */ + can_parameter.resync_jump_width = CAN_BT_SJW_1TQ; + can_parameter.time_segment_1 = CAN_BT_BS1_7TQ; + can_parameter.time_segment_2 = CAN_BT_BS2_2TQ; + can_parameter.prescaler = 6; + can_init(CAN0, &can_parameter); + + can_struct_para_init(CAN_FD_FRAME_STRUCT, &can_fd_parameter); + can_fd_parameter.fd_frame = ENABLE; + can_fd_parameter.excp_event_detect = ENABLE; + can_fd_parameter.delay_compensation = ENABLE; + can_fd_tdc_parameter.tdc_filter = 0x04; + can_fd_tdc_parameter.tdc_mode = CAN_TDCMOD_CALC_AND_OFFSET; + can_fd_tdc_parameter.tdc_offset = 0x04; + can_fd_parameter.p_delay_compensation = &can_fd_tdc_parameter; + can_fd_parameter.iso_bosch = CAN_FDMOD_ISO; + can_fd_parameter.esi_mode = CAN_ESIMOD_HARDWARE; + /* FD mode data baudrate 2Mbps, sample piont at 80% */ + can_fd_parameter.data_resync_jump_width = CAN_BT_SJW_1TQ; + can_fd_parameter.data_time_segment_1 = CAN_BT_BS1_3TQ; + can_fd_parameter.data_time_segment_2 = CAN_BT_BS2_1TQ; + /* CAN-FD data segement prescaler should be the same as non-data segement + * prescaler */ + can_fd_parameter.data_prescaler = 6; + can_fd_init(CAN0, &can_fd_parameter); + + /* initialize filter */ + can_filter.filter_mode = CAN_FILTERMODE_MASK; + can_filter.filter_bits = CAN_FILTERBITS_32BIT; + /* configure filter ID */ + can_filter.filter_list_high = 0x0000; + can_filter.filter_list_low = 0x0000; + /* configure filter mask */ + can_filter.filter_mask_high = 0x0000; + can_filter.filter_mask_low = 0x0000; + /* select receiver fifo */ + can_filter.filter_fifo_number = CAN_FIFO0; + can_filter.filter_enable = ENABLE; + /* configure filter number */ + can_filter.filter_number = 0; + can_filter_init(&can_filter); + + /* configure CAN0 NVIC */ + nvic_irq_enable(CAN0_RX0_IRQn, 1, 0); + /* enable can receive FIFO0 not empty interrupt */ + can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0); + // 使能 DMA 中断 + nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1); + dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF); +} + +/*! + \brief 初始化 CAN0 回环测试 + \param[in] none + \param[out] none + \retval none +*/ +void can_loopback_init(void) { + can_gpio_init(); + can_parameter_struct can_parameter; + can_filter_parameter_struct can_filter; + /* initialize CAN register */ + can_deinit(CANX); + + /* initialize CAN */ + can_parameter.time_triggered = DISABLE; + can_parameter.auto_bus_off_recovery = DISABLE; + can_parameter.auto_wake_up = DISABLE; + can_parameter.auto_retrans = DISABLE; + can_parameter.rec_fifo_overwrite = DISABLE; + can_parameter.trans_fifo_order = DISABLE; + can_parameter.working_mode = CAN_NORMAL_MODE; + /* baudrate 1Mbps, sample piont at 80% */ + can_parameter.resync_jump_width = CAN_BT_SJW_1TQ; + can_parameter.time_segment_1 = CAN_BT_BS1_7TQ; + can_parameter.time_segment_2 = CAN_BT_BS2_2TQ; + can_parameter.prescaler = 6; + if (can_init(CANX, &can_parameter) != SUCCESS) { + u16 i = 200; + while (--i > 0) { + delay_1ms(10); + CAN_DEBUG_PRINT("can init error \r\n"); + } + system_soft_reset(); + } + + /* initialize filter */ + /* configure filter mode */ + can_filter.filter_mode = CAN_FILTERMODE_MASK; + can_filter.filter_bits = CAN_FILTERBITS_32BIT; + /* configure filter ID */ + can_filter.filter_list_high = (uint16_t)(DEV_CAN_LOOPBACK_ID >> 13U); + can_filter.filter_list_low = + ((uint16_t)(DEV_CAN_LOOPBACK_ID)) << 3U | (1U << 2); + /* configure filter mask */ + can_filter.filter_mask_high = 0x0000; + can_filter.filter_mask_low = 0x0000; + /* select receiver fifo */ + can_filter.filter_fifo_number = CAN_FIFO0; + can_filter.filter_enable = ENABLE; + /* configure filter number */ + can_filter.filter_number = 0; + can_filter_init(&can_filter); +} + +/*! + \brief CAN0 FD 回环测试 + \param[in] none + \param[out] none + \retval ErrStatus +*/ +void canfd_loopback_test(void) { + can_trasnmit_message_struct transmit_message; + can_receive_message_struct receive_message; + uint32_t timeout = 0xFFFF; + uint8_t transmit_mailbox = 0; + + std::mt19937 gen; + std::uniform_int_distribution dis(0, 255); + + can_struct_para_init(CAN_FD_FRAME_STRUCT, &transmit_message); + transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID; + transmit_message.tx_efid = 0x00; + transmit_message.tx_ft = CAN_FT_DATA; + transmit_message.tx_ff = CAN_FF_EXTENDED; + transmit_message.tx_dlen = 7; + transmit_message.fd_flag = 1; + transmit_message.fd_brs = 1; + transmit_message.fd_esi = 0; + + int i = 0; + while (i++ < transmit_message.tx_dlen) { + transmit_message.tx_data[i] = dis(gen); + CAN_DEBUG_PRINT("%x\n", transmit_message.tx_data[i]); + } + + /* transmit message */ + transmit_mailbox = can_message_transmit(CAN0, &transmit_message); + /* waiting for transmit completed */ + while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) && + (0 != timeout)) { + timeout--; + } + timeout = 0xFFFF; + /* waiting for receive completed */ + while ((can_receive_message_length_get(CAN0, CAN_FIFO0) < 1) && + (0 != timeout)) { + timeout--; + } + can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message); + can_message_receive(CAN0, CAN_FIFO0, &receive_message); + + /* check the receive message */ + if ((transmit_message.tx_sfid == receive_message.rx_sfid) && + (transmit_message.tx_ff == receive_message.rx_ff) && + (transmit_message.tx_dlen == receive_message.rx_dlen)) { + /* loopback test is success */ + CAN_DEBUG_PRINT("CAN LO0PBACK SUCCESS\r\n"); + } else { + /* loopback test is failed */ + CAN_DEBUG_PRINT("CAN LO0PBACK FAIL\r\n"); + } +} + +/** + \brief CAN0 回环测试 + \param[in] none + \param[out] none + \retval none +*/ +void can_loopback_test(void) { + can_trasnmit_message_struct transmit_message; + can_receive_message_struct receive_message; + uint32_t timeout = 0xFF; + uint8_t transmit_mailbox = 0; + + std::mt19937 gen(rand()); // 使用随机设备生成的种子 + std::uniform_int_distribution dis(0, 0xff); + + can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message); + /* initialize transmit message */ + transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID; + transmit_message.tx_ft = CAN_FT_DATA; + transmit_message.tx_ff = CAN_FF_STANDARD; + transmit_message.tx_dlen = 7; + int i = 0; + while (i++ < transmit_message.tx_dlen) { + // transmit_message.tx_data[i] = dis(gen); + transmit_message.tx_data[i] = i; + } + /* transmit message */ + transmit_mailbox = can_message_transmit(CANX, &transmit_message); + /* waiting for transmit completed */ + while ((CAN_TRANSMIT_OK != can_transmit_states(CANX, transmit_mailbox)) && + (0 != timeout)) { + CAN_DEBUG_PRINT("sending:%x...\n", + can_transmit_states(CANX, transmit_mailbox)); + timeout--; + } + timeout = 0xFF; + /* waiting for receive completed */ + while ((can_receive_message_length_get(CANX, CAN_FIFO0) < 1) && + (0 != timeout)) { + timeout--; + } + can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message); + can_message_receive(CANX, CAN_FIFO0, &receive_message); + /* check the receive message */ + if ((DEV_CAN_LOOPBACK_ID == receive_message.rx_sfid) && + (CAN_FF_STANDARD == receive_message.rx_ff) && + (transmit_message.tx_dlen == receive_message.rx_dlen)) { + i = transmit_message.tx_dlen; + while (i-- > 0) { + CAN_DEBUG_PRINT("rx %d:%x:%x\n", i, transmit_message.tx_data[i], + receive_message.rx_data[i]); + } + if (std::memcmp(receive_message.rx_data, transmit_message.tx_data, + transmit_message.tx_dlen) == 0) { + /* loopback test is success */ + CAN_DEBUG_PRINT("loopback success\n"); + } else { + /* loopback test is failed */ + CAN_DEBUG_PRINT("loopback failed\n"); + } + } else { + /* loopback test is failed */ + CAN_DEBUG_PRINT("loopback failed\n"); + } +} + +/*! + \brief CAN_transmiter 构造函数 + \note 初始化 CAN 通信相关参数和硬件配置 +*/ +u8 CAN_transmiter::init() { + // 初始化 CAN 硬件 + // can_config(); + canfd_config(); + + // 初始化发送消息结构体 + can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message); + /* initialize receive message */ + can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message); + + // 初始化 DMA 缓冲区 + memset(can_rx_buffer, 0, sizeof(can_rx_buffer)); + memset(can_tx_buffer, 0, sizeof(can_tx_buffer)); + + // 初始化节点 ID 为默认值 + node_id = DEFAULT_NODE_ID; + + // 从 Flash 加载节点 ID (如果 Flash 中有有效值则覆盖默认值) + load_node_id_from_flash(); + return 0; +} + +/*! + \brief CAN 数据更新 +*/ +void CAN_transmiter::update(void) { + target_angle_union.f = FOC.target_angle; + target_velocity_union.f = FOC.target_velocity; + target_current_union.f = FOC.target_current; + + rt_angle_union.f = FOC.rt_angle; + rt_velocity_union.f = FOC.rt_velocity; + rt_current_union.f = FOC.rt_current; + + limit_voltage = FOC.limit_voltage * 1000; + mcu_temp = sampler.get_mcu_temp() * 100; + motor_temp = sampler.get_drv_temp() * 100; + Power_Voltage = sampler.get_vbus_voltage() * 1000; + MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET; + EEPROMStatus = flashEEP.EEPROMStatus; + Target_Control_Mode = FOC.control_mode; + controller_Mode = FOC.control_mode; + + // 使用简单平均滤波的电流值 + u_current_union.f = sampler.get_current('U') * 1000; + w_current_union.f = sampler.get_current('W') * 1000; + v_current_union.f = sampler.get_current('V') * 1000; +} + +/** + * @brief CANFD 发送函数 + * @retval 0: 发送失败（或处于静默模式） + * 1: 发送成功 + */ +u8 CAN_transmiter::CANFD_all_Send() { + // 检查是否在静默模式中，如果是则不发送 + if (is_in_silent_mode()) { + check_silent_mode(); // 检查是否已退出静默 + return 0; + } + + u32 success_flag = 0; + + // 打包 status 数据 - 只清零实际使用的 32 字节 + memset(transmit_data, 0, 32); + pack_u16_to_bytes(transmit_data, 0, mcu_temp); + pack_u16_to_bytes(transmit_data, 2, motor_temp); + pack_u16_to_bytes(transmit_data, 4, u16(u_current_union.f)); + pack_u16_to_bytes(transmit_data, 6, u16(v_current_union.f)); + pack_u16_to_bytes(transmit_data, 8, u16(w_current_union.f)); + transmit_data[10] = + (transmit_data[10] & ~0x07) | + ((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) | + ((ThermalStatus & 0x01) << 2) | ((EEPROMStatus & 0x07) << 3)); + pack_u16_to_bytes(transmit_data, 11, Power_Voltage); + pack_float_to_bytes(transmit_data, 13, rt_angle_union); + pack_float_to_bytes(transmit_data, 17, rt_velocity_union); + pack_float_to_bytes(transmit_data, 21, rt_current_union); + pack_float_to_bytes(transmit_data, 25, testfloat_union); + pack_u16_to_bytes(transmit_data, 29, test_u16); + // 使用节点 ID 构建发送 ID (高 16 位为节点 ID) + success_flag += + CANFD_AsyncSend(build_can_id(CANFD_STATUS_ID), 32, transmit_data); + + // 打包 config 数据 - 只清零实际使用的 24 字节 + memset(transmit_data, 0, 24); + transmit_data[0] = controller_Mode; + pack_u16_to_bytes(transmit_data, 1, limit_voltage); + pack_float_to_bytes(transmit_data, 3, target_current_union); + pack_float_to_bytes(transmit_data, 7, target_velocity_union); + pack_float_to_bytes(transmit_data, 11, target_angle_union); + // 使用节点 ID 构建发送 ID (高 16 位为节点 ID) + success_flag += + CANFD_AsyncSend(build_can_id(CANFD_CONFIG_ID), 24, transmit_data); + + return success_flag; +} + +/** + * @brief CAN 通信检查函数 + * @retval 0: 无新消息 + * 1: 有新消息 + */ +void CAN_transmiter::communication_check() { + // CAN0 发送数据标志 + if (SET == can0_send_flag) { + // 检查是否在静默模式中 + if (!is_in_silent_mode()) { + update(); + CANFD_all_Send(); + } else { + check_silent_mode(); // 检查静默是否结束 + } + can0_send_flag = RESET; + } + + // 处理所有接收到的 CAN 消息（从环形缓冲区中） + can_receive_message_struct current_message; + while (dequeue_rx_message(current_message)) { + // 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分) + u32 base_id = current_message.rx_efid & 0xFFFF; + + // 提取发送方的节点 ID + u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF); + + // 特殊处理 base_id=0x0000（BOOT 命令）：即使是其他节点的广播消息也要处理 + if (base_id == CANFD_BOOT_ID) { + // 判断是广播消息还是其他节点的命令 + if (sender_node_id == BROADCAST_NODE_ID || sender_node_id != node_id) { + // 广播消息或其他节点的命令 → 进入静默模式（停发 10 秒） + CAN_DEBUG_PRINT("Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n", sender_node_id); + enter_silent_mode(); + } else { + // 自己节点的命令 → 执行 BOOT 功能 + CAN_DEBUG_PRINT("Received base_id=0x0000 from self, jumping to BOOT\r\n"); + motor_sleep(); + flashEEP.save_system_config(false); + fmc_unlock(); + OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置 APP 为不激活 + fmc_lock(); + jumpToBOOT(); + } + continue; // BOOT 命令处理完跳过后续逻辑 + } + + // 设置节点 ID 命令 (基础 ID: 0x0400) + // 注意：此命令在 is_message_for_me 检查之前处理，所有节点都会响应 + // 这样即使节点ID已经改变，仍然可以用旧节点ID或广播ID来修改 + else if (base_id == CANFD_SET_NODE_ID) { + // 打印接收到的原始数据 + CAN_DEBUG_PRINT( + "SET_NODE_ID: rx_dlen=%d, data[0]=%02X data[1]=%02X\r\n", + current_message.rx_dlen, current_message.rx_data[0], + current_message.rx_data[1]); + // 解析新节点 ID - 支持 1 字节或 2 字节格式 + u16 new_node_id; + if (current_message.rx_dlen >= 2) { + new_node_id = unpack_bytes_to_u16(current_message.rx_data, 0); + } else { + new_node_id = + current_message.rx_data[0]; // 只发送 1 字节时直接使用 + } + u16 old_node_id = node_id; // 保存旧节点 ID 用于构建响应 ID + CAN_DEBUG_PRINT( + "SET_NODE_ID: recv new_id=%d (0x%04X), old_id=%d\r\n", + new_node_id, new_node_id, old_node_id); + + if (new_node_id <= MAX_NODE_ID && new_node_id != BROADCAST_NODE_ID) { + // 先发送响应（使用旧节点ID构建CAN ID） + // 响应数据格式：[status:1byte][new_node_id:2byte] = 3 字节 + transmit_data[0] = 0x01; // 成功 + transmit_data[1] = (u8)(new_node_id & 0xFF); // 新 ID 低字节 + transmit_data[2] = + (u8)((new_node_id >> 8) & 0xFF); // 新 ID 高字节 + + // 使用旧节点 ID 发送响应 + node_id = old_node_id; + u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, + transmit_data); + + // 然后保存新节点 ID 到 Flash + u8 save_status = save_node_id_to_flash(new_node_id); + + CAN_DEBUG_PRINT("SET_NODE_ID: save_status=%d, send_result=%d\r\n", + save_status, send_result); + + if (save_status == FLASH_EEPROM_OK) { + CAN_DEBUG_PRINT("SET_NODE_ID: SUCCESS, node_id updated from %d to %d\r\n", + old_node_id, new_node_id); + } else { + CAN_DEBUG_PRINT("SET_NODE_ID: Flash write FAILED, status=%d\r\n", + save_status); + // 发送错误响应 + transmit_data[0] = 0x00; // 失败 + node_id = old_node_id; + CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, + transmit_data); + } + } else { + // 发送错误响应 - 使用旧节点 ID 构建响应 ID + CAN_DEBUG_PRINT("SET_NODE_ID: invalid new_id=%d (max=%d)\r\n", + new_node_id, MAX_NODE_ID); + transmit_data[0] = 0x00; // 失败 + transmit_data[1] = (u8)(new_node_id & 0xFF); + transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF); + node_id = old_node_id; // 临时恢复旧 ID 用于构建响应 + CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, + transmit_data); + // 保持原 ID 不变 + } + continue; // SET_NODE_ID 命令处理完跳过后续逻辑 + } + + // 其他 base_id 的消息，检查是否是发给本节点的 + if (!is_message_for_me(current_message.rx_efid)) { + continue; // 不是发给本节点的消息，跳过 + } + + if (base_id == CANFD_CMD_ID) { + // 控制模式 + FOC.control_mode = current_message.rx_data[0]; + // 目标电压限制 + FOC.limit_voltage = + float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000; + // 目标电流 + target_current_union.f = + unpack_bytes_to_float(current_message.rx_data, 3).f; + FOC.current_loop.set_limit(target_current_union.f); + FOC.vel_inMITloop.set_limit(target_current_union.f); + // 目标速度 + target_velocity_union.f = + unpack_bytes_to_float(current_message.rx_data, 7).f; + FOC.ang_inVelAngloop.set_limit(target_velocity_union.f); + FOC.ang_inMITloop.set_limit(target_velocity_union.f); + // 目标角度 + target_angle_union.f = + unpack_bytes_to_float(current_message.rx_data, 11).f; + // 赋值 + FOC.target_angle = target_angle_union.f; + FOC.target_velocity = target_velocity_union.f; + FOC.target_current = target_current_union.f; + + // 解析 PID 参数 + float voltage = FOC.limit_voltage; + // PID 参数 + u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15); + FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage; + FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage; + + P_config = unpack_bytes_to_u16(current_message.rx_data, 17); + FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage; + FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config; + FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config; + + P_config = unpack_bytes_to_u16(current_message.rx_data, 19); + FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage; + FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage; + FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage; + } else if (base_id == CANFD_RESET_ID) { + u8 first_byte = current_message.rx_data[0]; + u8 eigth_byte = current_message.rx_data[7]; + // 电机重置 + if (first_byte & 0x01) { + motor_reset(); + testfloat_union.f = KTH7823.getcurrent_sensorangle(); + } + // EEPROM 重置 + if (first_byte >> 1 & 0x01) { + // flashEEP.format(); + } + // 系统参数保存 + if (first_byte >> 2 & 0x01) { + flashEEP.save_system_config(false); + } + // 角度传感器重置 + if (first_byte >> 3 & 0x01) { + KTH7823.angle_set(0); + Angle_kalman.init(0); // 角度滤波 + Vel_kalman.init(0); // 角度滤波 + } + // 休眠 + if (eigth_byte & 0x80 >> 7) { + app_prepare_deep_sleep(true); + // CAN_Sleep(); + } + } else if (base_id == CANFD_VERSION_ID) { + // 处理版本号请求，发送当前系统版本信息 + Version.sendVersionInfoViaCan(); + } else if (base_id == CANFD_OTA_ACK_ID) { + volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR; + u32 app_enable = *flash_ptr; + flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR; + u32 app_check = *flash_ptr; + transmit_data[0] = 0x0A; + transmit_data[1] = 0xAC; + delay_1ms(1); + if (app_enable == OTA_AVAL && app_check == OTA_AVAL) { + CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2, + transmit_data); + } + } + // 查询节点 ID 命令 (基础 ID: 0x0402) + else if (base_id == CANFD_GET_NODE_ID) { + pack_u16_to_bytes(transmit_data, 0, get_node_id()); + CANFD_AsyncSend(build_can_id(CANFD_GET_NODE_ID), 2, transmit_data); + } else { + CAN_DEBUG_PRINT("Unknown CAN ID: %X (node_id=%d)\n", + current_message.rx_efid, node_id); + } + } +} + +/** + * @brief CAN 睡眠 + * @param None + * @retval None + */ +u8 CAN_transmiter::CAN_Sleep() { + gpio_bit_set(GPIOB, GPIO_PIN_13); + is_sleep = true; + return 0; +} + +/** + * @brief CAN 唤醒 + * @param None + * @retval None + */ +u8 CAN_transmiter::CAN_Wakeup() { + CAN_DEBUG_PRINT("can wake\r\n"); + is_sleep = false; + canfd_config(); + + return 0; +} + +/** + * @brief CAN0 中断服务程序（优化版：直接读取数据，减少DMA竞态条件） + * @retval 无 + */ +void CAN0_RX0_IRQHandler(void) { + // 检查是否是 FIFO0 消息挂起中断 + if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFL0) != RESET) { + // 清除中断标志 + can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0); + + // 直接读取消息（包括数据），不依赖DMA + can_receive_message_struct temp_message; + can_message_receive(CAN0, CAN_FIFO0, &temp_message); + + // 将接收到的消息加入环形缓冲区 + CAN0_transmiter.enqueue_rx_message_from_isr(temp_message); + } + + // 检查其他可能的中断源 + if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFF0) != RESET) { + // FIFO0 满中断 + can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFF0); + CAN_DEBUG_PRINT("CAN FIFO0 full\n"); + } + + if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFO0) != RESET) { + // FIFO0 溢出中断 + can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFO0); + CAN_DEBUG_PRINT("CAN FIFO0 overflow\n"); + } +} + +/** + * @brief DMA 通道 4 中断服务程序 (CAN 接收 DMA) + * @retval 无 + */ +void DMA0_Channel4_IRQHandler(void) { + if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_FTF)) { + // 清除 DMA 传输完成中断标志 + dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G); + // 将 DMA 缓冲区的数据复制到接收消息结构体 + for (int i = 0; i < CAN0_transmiter.receive_message.rx_dlen && i < 8; + i++) { + CAN0_transmiter.receive_message.rx_data[i] = can_rx_buffer[i]; + } + } +} + +/** + * @brief DMA 通道 5 中断服务程序 (CAN/CANFD 发送 DMA) + * @retval 无 + */ +void DMA0_Channel5_IRQHandler(void) { + if (dma_interrupt_flag_get(DMA0, DMA_CH5, DMA_INT_FLAG_FTF)) { + // DMA 传输完成，可以在此处执行后续处理 + // 清除 DMA 传输完成中断标志 + dma_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_FTF); + // 可以在此添加后续处理逻辑，如发送下一个数据包等 + } +} + +/** + * @brief 异步 CAN 数据发送 (使用 DMA 和中断机制) + * @param id CAN 消息 ID + * @param length CAN 消息长度 + * @param data CAN 消息数据 + */ +u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) { + // 检查数据长度是否符合 CAN 规范 + if (length > 8) { + CAN_DEBUG_PRINT("CAN data length exceeds maximum of 8 bytes\r\n"); + return 0; + } + + // 检查数据指针是否有效 + if (data == nullptr) { + CAN_DEBUG_PRINT("Invalid data pointer\r\n"); + return 0; + } + + /* 配置发送消息参数 */ + transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 + transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式 + transmit_message.tx_sfid = id; // 设置帧 ID + transmit_message.tx_dlen = length; // 设置数据长度 + + // 复制数据到发送缓冲区 + for (int i = 0; i < length && i < 8; i++) { + transmit_message.tx_data[i] = *(data + i); + } + + // 自动选择空闲 mailbox 发送消息 + transmit_mailbox = can_message_transmit(CAN0, &transmit_message); + + // 检查是否成功获取 mailbox + if (transmit_mailbox == CAN_NOMAILBOX) { + // 没有可用的 mailbox，启用发送中断以便在邮箱可用时重试 + CAN_DEBUG_PRINT( + "No available mailbox for CAN transmission, enabling TX " + "interrupt\r\n"); + // 可以设置一个标志位，当邮箱可用时发送 + // 在实际应用中，可以维护一个待发送队列 + return 0; + } + + // 启用 CAN 发送中断以检测传输完成 + can_interrupt_enable(CAN0, CAN_INTEN_TMEIE); + return 1; +} + +/** + * @brief 向发送队列添加消息 + * @param id CAN消息ID + * @param length CAN消息长度 + * @param data CAN消息数据 + * @retval 1=成功, 0=队列满 + */ +static u8 CAN_TX_QueuePush(u32 id, u8 length, u8 *data) { + if (tx_queue_count >= CAN_TX_QUEUE_SIZE) { + CAN_DEBUG_PRINT("TX queue full, message dropped\r\n"); + return 0; // 队列满，丢弃消息 + } + + tx_queue[tx_queue_tail].id = id; + tx_queue[tx_queue_tail].length = length; + for (int i = 0; i < length && i < 64; i++) { + tx_queue[tx_queue_tail].data[i] = data[i]; + } + tx_queue[tx_queue_tail].valid = 1; + + tx_queue_tail = (tx_queue_tail + 1) % CAN_TX_QUEUE_SIZE; + tx_queue_count++; + + return 1; +} + +/** + * @brief 从发送队列取出消息 + * @param item 指向队列项的指针 + * @retval 1=成功, 0=队列空 + */ +static u8 CAN_TX_QueuePop(can_tx_queue_item_t *item) { + if (tx_queue_count == 0) { + return 0; // 队列空 + } + + *item = tx_queue[tx_queue_head]; + tx_queue[tx_queue_head].valid = 0; + tx_queue_head = (tx_queue_head + 1) % CAN_TX_QUEUE_SIZE; + tx_queue_count--; + + return 1; +} + +/** + * @brief 异步 CANFD 数据发送 (使用队列机制防止丢帧) + * @param id CAN 消息 ID + * @param length CAN 消息长度 + * @param data CAN 消息数据 + */ +u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) { + u8 valid_length = length; + // 检查 CANFD 模式是否启用 + if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) { + CAN_DEBUG_PRINT("CANFD mode not enabled\r\n"); + return 0; + } + + // 检查数据指针是否有效 + if (data == nullptr) { + CAN_DEBUG_PRINT("Invalid data pointer\r\n"); + return 0; + } + + // 检查数据长度是否符合 CANFD 规范 + if (length > 8) { + if (length > 8 and length < 13) { + valid_length = 12; + } else if (length > 12 and length < 17) { + valid_length = 16; + } else if (length > 16 and length < 21) { + valid_length = 20; + } else if (length > 20 and length < 25) { + valid_length = 24; + } else if (length > 24 and length < 33) { + valid_length = 32; + } else if (length > 32 and length < 49) { + valid_length = 48; + } else if (length > 48 and length < 65) { + valid_length = 64; + } else { + CAN_DEBUG_PRINT("CANFD data length error\r\n"); + return 0; + } + } + + /* 配置发送消息参数 */ + transmit_message.tx_ft = CAN_FT_DATA; // 数据帧 + transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式 + transmit_message.tx_efid = id; // 扩展帧 ID + transmit_message.tx_dlen = valid_length; // 设置数据长度 + + // 设置 CANFD 特定参数 + transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD 帧格式 + transmit_message.fd_brs = ENABLE; // 使能比特率切换 + transmit_message.fd_esi = DISABLE; // 错误状态指示 + // 复制数据到发送缓冲区 + for (int i = 0; i < valid_length; i++) { + if (i < length) { + transmit_message.tx_data[i] = *(data + i); + } else { + transmit_message.tx_data[i] = 0; + } + } + + // 自动选择空闲 mailbox 发送消息 + transmit_mailbox = can_message_transmit(CAN0, &transmit_message); + + // 检查是否成功获取 mailbox + if (transmit_mailbox == CAN_NOMAILBOX) { + // 没有可用的 mailbox，将消息加入发送队列 + CAN_DEBUG_PRINT("No available mailbox, adding to TX queue\r\n"); + return CAN_TX_QueuePush(id, valid_length, data); + } + + // 启用 CAN 发送中断以检测传输完成（用于队列发送） + can_interrupt_enable(CAN0, CAN_INTEN_TMEIE); + return 1; +} + +/** + * @brief CAN 传输中断处理函数（支持队列发送） + */ +void CAN0_TX_IRQHandler(void) { + // 检查是否有发送邮箱空闲 + if (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET || + can_flag_get(CAN0, CAN_FLAG_TME1) != RESET || + can_flag_get(CAN0, CAN_FLAG_TME2) != RESET) { + + can_tx_queue_item_t next_msg; + // 尝试从队列中取出下一条消息发送 + if (CAN_TX_QueuePop(&next_msg)) { + // 配置并发送队列中的消息（使用独立的队列消息结构体） + queue_transmit_message.tx_ft = CAN_FT_DATA; + queue_transmit_message.tx_ff = CAN_FF_EXTENDED; + queue_transmit_message.tx_efid = next_msg.id; + queue_transmit_message.tx_dlen = next_msg.length; + queue_transmit_message.fd_flag = CAN_FDF_FDFRAME; + queue_transmit_message.fd_brs = ENABLE; + queue_transmit_message.fd_esi = DISABLE; + + for (int i = 0; i < next_msg.length; i++) { + queue_transmit_message.tx_data[i] = next_msg.data[i]; + } + + can_message_transmit(CAN0, &queue_transmit_message); + } else { + // 队列为空，禁用发送中断 + can_interrupt_disable(CAN0, CAN_INTEN_TMEIE); + } + + // 清除发送中断标志 + can_flag_clear(CAN0, CAN_FLAG_TME0); + can_flag_clear(CAN0, CAN_FLAG_TME1); + can_flag_clear(CAN0, CAN_FLAG_TME2); + } +} diff --git a/backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.h b/backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.h new file mode 100644 index 0000000..769f9af --- /dev/null +++ b/backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.h @@ -0,0 +1,169 @@ +#ifndef _CAN_H +#define _CAN_H + +#include +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "gd32c10x_can.h" +#include "config.h" + +// 定义一个联合体类型用于 float 和 byte 数组之间的转换 +typedef union { + float f; + u8 b[4]; +} float_union_t; + +// 添加辅助函数用于将 u16 类型数据打包到 u8 数组中 +static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) { + buffer[offset] = (u8)(value & 0xFF); + buffer[offset + 1] = (u8)((value >> 8) & 0xFF); +} + +// 添加辅助函数用于将 float 类型数据打包到 u8 数组中 +static inline void pack_float_to_bytes(u8 *buffer, u16 offset, + float_union_t &float_union) { + buffer[offset] = float_union.b[0]; + buffer[offset + 1] = float_union.b[1]; + buffer[offset + 2] = float_union.b[2]; + buffer[offset + 3] = float_union.b[3]; +} + +// 添加辅助函数用于从 u8 数组中解包 short 类型数据 +static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) { + short value = (short)(buffer[offset] | (buffer[offset + 1] << 8)); + return value; +} + +// 添加辅助函数用于从 u8 数组中解包 u16 类型数据 +static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) { + u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8)); + return value; +} + +// 添加辅助函数用于从 u8 数组中解包 float 类型数据 +static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) { + float_union_t float_union; + float_union.b[0] = buffer[offset]; + float_union.b[1] = buffer[offset + 1]; + float_union.b[2] = buffer[offset + 2]; + float_union.b[3] = buffer[offset + 3]; + return float_union; +} + +u8 can_gpio_init(void); // CAN 引脚初始化 +void can_config(void); // CAN 初始化 +void canfd_config(void); // CANFD 初始化 +void can_dma_init(void); // CAN DMA 初始化 +void CAN0_RX0_IRQHandler(void); // CAN 接收中断处理函数 +void DMA0_Channel4_IRQHandler(void); // DMA 通道 4 中断处理函数 (CAN 接收) +void DMA0_Channel5_IRQHandler(void); // DMA 通道 5 中断处理函数 (CAN 发送) +void jumpToBOOT(); + +void can_loopback_init(void); +void can_loopback_test(void); + +/*! + \brief CAN 通信类 + \note 封装 CAN 发送和接收功能，支持多电机节点 ID 隔离 +*/ +class CAN_transmiter { + + private: + static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN 接收缓冲区大小 + static const u32 CAN_SILENT_DURATION_MS = 10000; // CAN 静默时长 10 秒 + + bool is_sleep = false; // 是否休眠 + bool is_silent = false; // 是否静默（收到 base_id=0x0000 后进入静默） + u32 silent_start_time = 0; // 静默开始时间（ms） + + u16 node_id; // 本节点 ID(0-254)，使用 CAN ID 高 8 位存储 (在 init() 中初始化为 DEFAULT_NODE_ID) + u8 Target_Control_Mode = 0; // 目标控制模式 + u8 controller_Mode = 0; // 当前模式 + short mcu_temp = 0; // MCU 温度 + short motor_temp = 0; // 电机温度 + u16 Power_Voltage = 0; // 电源电压 + + u16 limit_voltage = 0; // 限制电压 + + float_union_t rt_angle_union = {0}; // 实时角度 + float_union_t rt_velocity_union = {0}; // 实时速度 + float_union_t rt_current_union = {0}; // 实时电流 + + float_union_t target_angle_union = {0}; // 目标角度 + float_union_t target_velocity_union = {0}; // 目标速度 + float_union_t target_current_union = {0}; // 目标电流 + + float_union_t u_current_union = {0}; // U 相电流 + float_union_t w_current_union = {0}; // W 相电流 + float_union_t v_current_union = {0}; // V 相电流 + + bool MotorStatus = 0; // 电机状态码 + u8 EEPROMStatus = 0; // EEPROM 状态码 + + can_trasnmit_message_struct transmit_message; // 发送消息结构体 + u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无 mailbox 状态 + u8 transmit_data[64]; // CANFD 最大 64 字节 + + // CAN 接收消息队列 + can_receive_message_struct rx_message_buffer[CAN_RX_BUFFER_SIZE]; + volatile uint8_t rx_buffer_head = 0; + volatile uint8_t rx_buffer_tail = 0; + + void update(); // 更新数据 + u8 CAN_Receive(); // CAN 接收 + u8 CANFD_Receive(); // CANFD 接收 + bool is_rx_buffer_full(); + bool is_rx_buffer_empty(); + bool dequeue_rx_message(can_receive_message_struct& message); + void check_silent_mode(); // 检查静默模式是否结束 + + public: + // 节点 ID 相关辅助函数 (使用高 16 位存储节点 ID) + u32 build_can_id(u32 base_id); // 根据节点 ID 构建 CAN ID，格式：[节点 ID(16 位)][基础 ID(16 位)] + bool is_message_for_me(u32 received_id); // 判断消息是否发给本节点 + + public: + // 添加构造函数声明 + u8 init(void); + + can_receive_message_struct receive_message; // 接收消息结构体 + FlagStatus can0_send_flag; // CAN0 发送标志 + float_union_t testfloat_union = {0}; // 测试用 + u16 test_u16 = 0; + + bool MagStatus = 0; // 磁状态码 + bool ThermalStatus = 0; // 热能状态码 + void communication_check(); // 通信检查 + u8 CAN_Sleep(); // CAN 进入睡眠 + u8 CAN_Wakeup(); // CAN 唤醒 + u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data); // CAN 异步发送 (使用 DMA) + u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD 异步发送 (使用 DMA) + u8 CAN_all_Send(); // CAN 发送 + u8 CANFD_all_Send(); // CANFD 发送 + void enter_silent_mode(); // 进入静默模式（停发 10 秒） + bool is_in_silent_mode(); // 检查是否在静默模式中 + + // 节点 ID 管理接口 + u16 get_node_id(); // 获取当前节点 ID + void set_node_id(u16 new_id); // 设置节点 ID + u8 load_node_id_from_flash(); // 从 Flash 加载节点 ID + u8 save_node_id_to_flash(u16 new_id); // 保存节点 ID 到 Flash + + // 中断处理函数需要访问的公共接口 + void enqueue_rx_message_from_isr(const can_receive_message_struct& message) { + if (!is_rx_buffer_full()) { + rx_message_buffer[rx_buffer_head] = message; + rx_buffer_head = (rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE; + } + // 如果缓冲区满，丢弃新消息 + } +}; + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file