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算法，显著提升性能
- 保留通用回退路径处理其他尺寸情况

Robot_V2/device/Timer.cpp

@@ -8,6 +8,7 @@
 #include "Can.h"
 #include "FOC.h"
 #include "Timer.h"
+#include "config.h"
 #include "gd32c10x.h"
 #include "gd32c10x_gpio.h"
 #include "gd32c10x_misc.h"
@@ -23,16 +24,13 @@ extern CAN_transmiter CAN0_transmiter;
 extern u32 cnt_test;
 
 // 任务调度查找表 - 每 100us 执行的任务 (1ms 周期内)
+// MCU/DRV 温度保持 1kHz：热模型兼作过流保护，温度积累按 1ms 步进，
+// 每毫秒都要拿到新温度采样。
 static const u32 task_schedule_100us[10] = {
     (1u << TASK_MCU_TEMP_UPDATE),      // 0: MCU 温度更新
     (1u << TASK_DRV_TEMP_UPDATE),      // 1: 驱动温度更新
     (1u << TASK_VBUS_UPDATE),          // 2: 电压更新
-    0,                                  // 3: 空闲
-    0,                                  // 4: 空闲
-    0,                                  // 5: 空闲
-    0,                                  // 6: 空闲
-    0,                                  // 7: 空闲
-    0,                                  // 8: 空闲
+    0, 0, 0, 0, 0, 0,                  // 3~8: 空闲
     (1u << TASK_CAN_TRANSMIT)          // 9: CAN 发送
 };
 
@@ -197,10 +195,18 @@ void timer1_gpio_config(void) {
 	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
 	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
 
+#if GATE_DRIVER_DRV8323
+	/* 新板：PB12(DRVSLP=ENABLE+INLx) 由 gate_driver 统一管理——
+	 * gate_driver_preinit() 已置低保持芯片睡眠，gate_driver_bringup()
+	 * 在 SPI 配置完成后才拉高。此处严禁提前置高（SPI 未配置前芯片
+	 * 带默认 6x PWM 模式醒来，INLx=1 时上管永远无法导通）。
+	 * PB0 板上接地未用，不配置。 */
+#else
 	gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,
 			  GPIO_PIN_0 | GPIO_PIN_12);
 	gpio_bit_set(GPIOB, GPIO_PIN_0);
 	gpio_bit_set(GPIOB, GPIO_PIN_12);
+#endif
 
 	/* 配置 TIMER1 完全重映射*/
 	gpio_pin_remap_config(GPIO_TIMER1_FULL_REMAP, DISABLE);
@@ -295,6 +301,15 @@ void TIMER1_IRQHandler(void) {
 void Set_Pwm(u16 channel, float duty_cycle) {
 	float pulse = duty_cycle * TIMER1_PERIOD;
 
+#if GATE_DRIVER_DRV8323
+	/* 新板通道重映射：逻辑相 A→CH2、B→CH1、C→CH0（硬件 INHA/B/C
+	 * 分别接 PWM1/2/3 = PA2/PA1/PA0），与 Set_3Pwm 保持一致 */
+	if (channel == 0) {
+		channel = 2;
+	} else if (channel == 2) {
+		channel = 0;
+	}
+#endif
 	if (channel <= 3) {
 		timer_channel_output_pulse_value_config(TIMER1, channel, pulse);
 	} else {
@@ -311,8 +326,17 @@ void Set_Pwm(u16 channel, float duty_cycle) {
 	\note       The channel parameter must be between 0 and 3 inclusive.
 */
 void Set_3Pwm(float duty_cycle0, float duty_cycle1, float duty_cycle2) {
-	// 直接写寄存器
+#if GATE_DRIVER_DRV8323
+	/* 新板硬件映射：CH0(PA0)→PWM3→INHC，CH1(PA1)→PWM2→INHB，
+	 * CH2(PA2)→PWM1→INHA。入参按 (A,B,C) 相序书写，此处把 A/C 换到
+	 * 物理通道，修正新板与老板相反的 INHx 接线。 */
+	TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD);
+	TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD);
+	TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD);
+#else
+	// 直接写寄存器（老板：CH0/1/2 = IN1/2/3 = A/B/C 相）
 	TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD);
 	TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD);
 	TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD);
+#endif
 }