Motor_v2/Robot_V2/device/Can.cpp

#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 <cstdio>
#include <cstdlib>
#include <cstring>
#include <random>

// 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<int> 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<int> 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);
	}
}