diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index 4c8a5e0..cc79cd5 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -64,7 +64,7 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, current_c -= current_avg; // 计算电气角度 - float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f); + float angle_el = fmod(shaft_angle * PP + KTH7823.calibration_angle, 360.0f); // Clarke变换 (使用三相电流) float I_alpha, I_beta; @@ -249,7 +249,6 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) { duty_c = 0.5f; break; } - // 设置PWM输出 Set_3Pwm(duty_a, duty_b, duty_c); } @@ -409,17 +408,14 @@ void FOC_Controller::fast_voltage_check(void) { * @return float: q轴电压 */ float FOC_Controller::velocity_open_loop() { - // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 - shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO/ - MOTOR_FREQUENCY, - 360.0f); - // 获取电机轴角度 - // float shaft_angle = KTH7823.getcurrent_sensorangle(); - CAN0_transmiter.testfloat_union.f = shaft_angle; + // 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中 + shaft_angle = + fmod(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY, + 360.0f); rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W')); - // 因为在开环控制中，Ud始终为0，只控制Uq + // 因为在开环控制中，Ud 始终为 0，只控制 Uq const float Uq = _constrain(target_velocity * REDUCTION_RATIO * 0.005f, 2.0f, voltage_power_supply * 0.5f); setSVPWM_PhaseVoltage(Uq, 0.0f); @@ -434,7 +430,7 @@ float FOC_Controller::velocity_open_loop() { */ float FOC_Controller::angle_control() { // PID 控制计算 - float Uq = angle_loop(rt_angle - target_angle); + float Uq = angle_loop(target_angle - rt_angle); // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle(); diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index 7d2c33b..51a0e6f 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -14,7 +14,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data, class angle_sensor { public: - float calibration_angle = 195.0f; + float calibration_angle = ANGLESENS_CALIBRATION_ANGLE; u8 init(void); float read_sensorAngle(void); float update(void); diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index d15eabc..22283e0 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -89,7 +89,8 @@ constexpr float DIR = 1.0f; // 默认方向：-1.0f 反转，1.0f 正转 constexpr u16 REDUCTION_RATIO = 25; constexpr float CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移 constexpr float HOLD_PWM_DUTY = 0.1f; -constexpr u16 ANGLESENS_OFFSET = +15; +constexpr u16 ANGLESENS_OFFSET = 0; +constexpr float ANGLESENS_CALIBRATION_ANGLE = 83.0f; constexpr float KE_FACTOR = 0.0f; // 角度卡尔曼滤波器参数 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 deleted file mode 100644 index 1322d0f..0000000 --- a/backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.cpp +++ /dev/null @@ -1,1246 +0,0 @@ -#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 deleted file mode 100644 index 769f9af..0000000 --- a/backup_pre_can_comm_port_20260515_225336/Robot_V2/device/Can.h +++ /dev/null @@ -1,169 +0,0 @@ -#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