Motor_v2/Robot_V2/device/Can.cpp

#include "Can.h"
#include "DengFOC.h"
#include "Filter.h"
#include "adc.h"
#include "ang_sensor.h"
#include "gd32c10x.h"
#include "gd32c10x_can.h"
#include "gd32c10x_dma.h"
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <random>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

constexpr u16 CANX = CAN0;
constexpr u16 DEV_CAN0_ID = 0x0000;
constexpr u16 DEV_CAN0_MASK = 0x0000;
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;

CAN_transmiter CAN0_transmiter;		 // CAN初始化
extern FOC_Controlroller FOC;
extern sampler sampler;
extern angle_sensor KTH7823;
extern EncoderVelocityKalman Vel_kalman;

// 添加DMA相关变量
dma_parameter_struct dma_rx_parameter;
dma_parameter_struct dma_tx_parameter;
uint8_t can_rx_buffer[8]; // CAN接收缓冲区
uint8_t can_tx_buffer[8]; // CAN发送缓冲区

/*!
	\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_bit_reset(GPIOB, GPIO_PIN_13);
	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)
			printf("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);
	/* 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 CANFD模式
	\param[in]  none
	\param[out] none
	\retval     none
*/
void canfd_config(void) {
	can_gpio_init();
	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);
}

/*!
	\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) {
		while (1)
			printf("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 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);
		std::printf("%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 */
		std::printf("CAN LO0PBACK SUCCESS\r\n");
	} else {
		/* loopback test is failed */
		std::printf("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)) {
		printf("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) {
			std::printf("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 */
			std::printf("loopback success\n");
		} else {
			std::printf("loopback failed\n");
		}
	} else {
		/* loopback test is failed */
		std::printf("loopback failed\n");
	}
}

/*!
	\brief CAN_transmiter构造函数
	\note  初始化CAN通信相关参数和硬件配置
*/
CAN_transmiter::CAN_transmiter() {
	// 初始化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));
}

/*!
	\brief CAN 数据更新
*/
void CAN_transmiter::update(void) {
	limit_velocity_union.f = FOC.limit_velocity;
	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;
	rt_angle_union.f = FOC.rt_angle;
	// rt_velocity_union.f = KTH7823.getVelocity();
	rt_velocity_union.f = FOC.rt_velocity;
	MotorStatus = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;

	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;
	// testfloat_union.f = KTH7823.getVelocity();
}

/*!
	\brief CAN 数据发送
	\param id CAN ID
	\param length 数据长度
	\param data 数据
*/
u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) {
	// 检查数据长度是否符合CAN规范
	if (length > 8) {
		printf("CAN data length exceeds maximum of 8 bytes\r\n");
		return 0;
	}

	// 检查数据指针是否有效
	if (data == nullptr) {
		printf("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);
	}

	// 将数据复制到DMA缓冲区
	memcpy(can_tx_buffer, data, length);

	// 配置DMA发送参数
	dma_tx_parameter.number = length;
	dma_init(DMA0, DMA_CH5, &dma_tx_parameter);

	// 启动DMA传输
	dma_channel_enable(DMA0, DMA_CH5);

	// 自动选择空闲mailbox发送消息，不需要人为指定
	transmit_mailbox = can_message_transmit(CAN0, &transmit_message);

	// 检查是否成功获取mailbox
	if (CAN_NOMAILBOX == transmit_mailbox) {
		// 没有可用的mailbox，可能所有mailbox都在使用中
		printf("No available mailbox for CAN transmission\r\n");
		return 0;
	}
	return 1;
}

/*!
	\brief CANFD 数据发送
	\param id CAN消息ID
	\param length CAN消息长度
	\param data CAN消息数据
*/
u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) {
	// 检查CANFD模式是否启用
	if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
		printf("CANFD mode not enabled\r\n");
		return 0;
	}

	// 检查数据指针是否有效
	if (data == nullptr) {
		printf("Invalid data pointer\r\n");
		return 0;
	}

	// 检查数据长度是否符合CANFD规范
	if (length > 64) {
		printf("CANFD data length exceeds maximum of 64 bytes\r\n");
		return 0;
	}

	/* 配置发送消息参数 */
	transmit_message.tx_ft = CAN_FT_DATA;	  // 数据帧
	transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
	// transmit_message.tx_sfid = id;			  // 设置帧ID
	transmit_message.tx_efid = id;	   // 扩展帧ID
	transmit_message.tx_dlen = 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 < length && i < 64; i++) {
		transmit_message.tx_data[i] = *(data + i);
	}

	// 将数据复制到DMA缓冲区
	memcpy(can_tx_buffer, data, length);

	// 配置DMA发送参数
	dma_tx_parameter.number = length;
	dma_init(DMA0, DMA_CH5, &dma_tx_parameter);

	// 启动DMA传输
	dma_channel_enable(DMA0, DMA_CH5);

	// 自动选择空闲mailbox发送消息，不需要人为指定
	transmit_mailbox = can_message_transmit(CAN0, &transmit_message);

	// 检查是否成功获取mailbox
	if (CAN_NOMAILBOX == transmit_mailbox) {
		// 没有可用的mailbox，可能所有mailbox都在使用中
		printf("No available mailbox for CANFD transmission\r\n");
		return 0;
	}
	return 1;
}

/**
 * @brief  CAN发送函数
 * @retval 0: 发送失败
 *         1: 发送成功
 */
u8 CAN_transmiter::CAN_all_Send() {
	u32 success_flag = 0;

	constexpr u32 RT_Vel_Ang_FEED = 0x101;
	constexpr u32 SET_Vel_Ang_FEED = 0x110;
	constexpr u32 Current_FEED = 0x117;
	constexpr u32 Status_FEED = 0x118;
	constexpr u32 Limit_Vel_Vol_FEED = 0x119;

	// 打包RT_Vel_Ang_FEED
	pack_float_to_bytes(transmit_data, 0, rt_angle_union);
	pack_float_to_bytes(transmit_data, 4, rt_velocity_union);
	success_flag += CAN_Send(RT_Vel_Ang_FEED, 8, transmit_data);
	// 打包SET_Vel_Ang_FEED
	pack_float_to_bytes(transmit_data, 0, target_angle_union);
	pack_float_to_bytes(transmit_data, 4, target_velocity_union);
	success_flag += CAN_Send(SET_Vel_Ang_FEED, 8, transmit_data);
	// 打包Current_FEED
	pack_u16_to_bytes(transmit_data, 0, u16(u_current_union.f));
	pack_u16_to_bytes(transmit_data, 2, u16(w_current_union.f));
	pack_u16_to_bytes(transmit_data, 4, u16(v_current_union.f));
	success_flag += CAN_Send(Current_FEED, 6, transmit_data);

	// 打包Status_FEED
	transmit_data[0] = MotorStatus;
	transmit_data[1] = controller_Mode;
	pack_u16_to_bytes(transmit_data, 2, motor_temp);
	pack_u16_to_bytes(transmit_data, 4, mcu_temp);
	pack_u16_to_bytes(transmit_data, 6, Power_Voltage);
	success_flag += CAN_Send(Status_FEED, 8, transmit_data);
	// 打包Limit_Vel_Vol_FEED
	pack_float_to_bytes(transmit_data, 0, limit_velocity_union);
	pack_u16_to_bytes(transmit_data, 4, limit_voltage);
	success_flag += CAN_Send(Limit_Vel_Vol_FEED, 6, transmit_data);

	return success_flag;
}

/**
 * @brief  CANFD发送函数
 * @retval 0: 发送失败
 *         1: 发送成功
 */
u8 CAN_transmiter::CANFD_all_Send() {
	u32 success_flag = 0;

	constexpr u32 STATUS_ID = 0x18FF0100;
	constexpr u32 CONFIG_ID = 0x18FF0101;
	// 打包status数据
	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(w_current_union.f));
	pack_u16_to_bytes(transmit_data, 8, u16(v_current_union.f));
	transmit_data[10] = (transmit_data[10] & ~0x07) | 
	                    ((MotorStatus & 0x01) |
	                    ((MagStatus & 0x01) << 1) |
	                    ((ThermalStatus & 0x01) << 2));
	
	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, testfloat_union);
	success_flag += CANFD_Send(STATUS_ID, 32, transmit_data);

	// 打包config数据
	transmit_data[0] = controller_Mode;
	pack_u16_to_bytes(transmit_data, 1, limit_voltage);
	pack_float_to_bytes(transmit_data, 3, limit_velocity_union);
	pack_float_to_bytes(transmit_data, 7, target_angle_union);
	pack_float_to_bytes(transmit_data, 11, target_velocity_union);
	success_flag += CANFD_Send(CONFIG_ID, 16, transmit_data);

	return success_flag;
}

/**
 * @brief  CAN通信检查函数
 * @retval 0: 无新消息
 *         1: 有新消息
 */
void CAN_transmiter::communication_check() {
	// CAN0 发送数据标志
	if (SET == can0_send_flag) {
		// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
		CAN0_transmiter.update();
		// CAN0_transmiter.CAN_all_Send();
		CAN0_transmiter.CANFD_all_Send();
		can0_send_flag = RESET;
	}

	// CAN0 接收数据标志
	if (SET == can0_receive_flag) {
		// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
		// for (i = 0; i < receive_message.rx_dlen; i++) {
		// 	printf(" %02x", receive_message.rx_data[i]);
		// }
		if (receive_message.rx_efid == 0x18ff0200) {
			FOC.control_mode = receive_message.rx_data[0];
			if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 !=
				FOC.limit_voltage) {
				FOC.limit_voltage =
					float(unpack_bytes_to_u16(receive_message.rx_data, 1)) /
					1000;
			}
			if (unpack_bytes_to_float(receive_message.rx_data, 3).f !=
				FOC.limit_velocity) {
				FOC.limit_velocity =
					unpack_bytes_to_float(receive_message.rx_data, 3).f;
			}
			target_angle_union.f =
				unpack_bytes_to_float(receive_message.rx_data, 7).f;
			FOC.target_angle = target_angle_union.f;

			target_velocity_union.f =
				unpack_bytes_to_float(receive_message.rx_data, 11).f;
			FOC.target_velocity = target_velocity_union.f;

			can0_receive_flag = RESET;
		}
	}
}

/**
 * @brief  CAN0中断服务程序
 * @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读取接收到的数据
		dma_channel_disable(DMA0, DMA_CH4);
		dma_rx_parameter.number =
			can_receive_message_length_get(CAN0, CAN_FIFO0);
		dma_init(DMA0, DMA_CH4, &dma_rx_parameter);
		dma_channel_enable(DMA0, DMA_CH4);

		// 读取接收到的数据
		can_message_receive(CAN0, CAN_FIFO0, &CAN0_transmiter.receive_message);

		// 接收标志位置1
		CAN0_transmiter.can0_receive_flag = SET;
	}

	// 检查其他可能的中断源
	if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFF0) != RESET) {
		// FIFO0满中断
		can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFF0);
		printf("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);
		printf("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_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_G);
	}
}