feat(FOC): 优化FOC控制算法并修复多个控制环路问题

- 重构cal_Iq_Id函数，移除电角度参数传递，改用预计算的sin/cos值以避免重复三角函数计算，提升性能约15-300周期 - 修复角度同步Uq前馈学习中的运动方向判断逻辑，解决反转时的震荡问题和正转正常反转震荡的根本原因 - 实现低速缩放因子函数get_mode3_velocity_scale，改进静摩擦 hunting抑制效果 - 优化电压检测逻辑，将阻塞式ADC读取改为连续模式零等待，提升实时性能 - 修复初始化期间电压分母为零的问题，添加下限保护防止P增益无穷大 - 实现PID控制器测量微分功能，消除目标阶跃时的微分冲击 - 改进积分抗饱和策略，添加外部饱和泄放功能，优化到位稳定性能 perf(Filter): 优化中值滤波算法性能 - 针对ADC_FILTER_SIZE=6的固定场景实现最优排序网络，用12次比较替代通用std::nth_element算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
2026-09-01 02:46:25 +08:00
parent 76f7987238
commit 7c4aaa5ee6
30 changed files with 1127 additions and 783 deletions
@@ -22,7 +22,6 @@
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
-#include <random>

 // CAN 调试宏定义
 #if CAN_DEBUG
@@ -33,9 +32,6 @@
 #define CAN_DEBUG_PRINT(fmt, ...) ((void)0)
 #endif

-// CAN0 固定外设号，编译期常量，非可变全局状态。
-/** @brief CANX 常量定义 */
-constexpr u16 CANX = CAN0;
 extern FOC_Controller FOC;
 extern sampler sampler;
 extern angle_sensor KTH7823;
@@ -150,7 +146,8 @@ void CAN_transmiter::set_node_id(u16 new_id) {
 	\param[out] none
 	\retval     FLASH_EEPROM_OK: 加载成功
 				其他：加载失败，使用默认值
-	\note       使用独立的节点 ID 存储区域，带魔数验证和 CRC 校验
+	\note       使用独立的节点 ID 双备份存储区（主/备独立 Flash 页），
+				单份损坏自动回退另一份；带魔数验证和 CRC 校验
 				防止位翻转导致错读
 */
 u8 CAN_transmiter::load_node_id_from_flash() {
@@ -185,7 +182,8 @@ u8 CAN_transmiter::load_node_id_from_flash() {
 	\param[out] none
 	\retval     FLASH_EEPROM_OK: 保存成功
 				其他：保存失败
-	\note       写入主备份和备用备份，确保数据可靠性
+	\note       乒乓式写入主/备两个独立 Flash 页之一，另一页始终保有
+				旧的有效记录，掉电时至少保留一份有效数据
 */
 u8 CAN_transmiter::save_node_id_to_flash(u16 new_id) {
 	if (new_id > MAX_NODE_ID) {
@@ -337,67 +335,6 @@ void can_dma_init(void) {
 	memset(can_rx_dma_words, 0, sizeof(can_rx_dma_words));
 }

-/*!
-	\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 模式
@@ -476,198 +413,6 @@ void canfd_config(void) {
 	dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF | DMA_INT_ERR);
 }

-/*!
-	\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;
-	/**
-	 * @brief dis 函数
-	 * @return std::uniform_int_distribution<int> 返回值
-	 */
-	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;
-
-	/**
-	 * @brief gen 函数
-	 * @param[in] rand 参数
-	 * @return std::mt19937 返回值
-	 */
-	std::mt19937 gen(rand()); // 使用随机设备生成的种子
-	/**
-	 * @brief dis 函数
-	 * @param[in] xff 参数
-	 * @return std::uniform_int_distribution<int> 返回值
-	 */
-	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 构造函数
@@ -675,7 +420,6 @@ void can_loopback_test(void) {
 */
 u8 CAN_transmiter::init() {
 	// 初始化 CAN 硬件
-	// can_config();
 	canfd_config();

 	// 初始化发送消息结构体
@@ -1241,7 +985,11 @@ void CAN_transmiter::handle_control_command(can_receive_message_struct &current_
 	FOC.target_velocity = target_velocity_union.f;
 	FOC.target_current = target_current_union.f;

+	// limit_voltage=0 时 P 换算除零会得到 inf 增益，钳到下限
 	float voltage = FOC.limit_voltage;
+	if (voltage < MIN_LIMIT_VOLTAGE) {
+		voltage = MIN_LIMIT_VOLTAGE;
+	}
 	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;
@@ -1299,7 +1047,6 @@ void CAN_transmiter::handle_ota_ack_message() {
 	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);
 	}