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/tasks.cpp

@@ -11,6 +11,7 @@
 #include "ang_sensor.h"
 #include "health.h"
 #include "systick.h"
+#include "gd32c10x.h"
 
 extern CAN_transmiter CAN0_transmiter;
 extern sampler sampler;
@@ -45,6 +46,29 @@ static bool has_task(u32 currentTasks, SystemTask task) {
 	return (currentTasks & (1u << task)) != 0U;
 }
 
+// 按任务 DWT 负载计量：定位 CPU 占用分布（与全局 cpu_usage 同 1s 口径，
+// 由 SysTick 统计块调用 task_busy_cycles_reset() 同步清零）。
+// 120MHz 下 120000 周期 ≈ 1% CPU。仅统计任务本体，不含中断抢占时间。
+static volatile u32 task_busy_cycles[TASK_COUNT] = {0};
+
+u32 get_task_busy_cycles(u8 task) {
+	return (task < TASK_COUNT) ? task_busy_cycles[task] : 0U;
+}
+
+void task_busy_cycles_reset(void) {
+	for (u8 i = 0; i < TASK_COUNT; i++) {
+		task_busy_cycles[i] = 0;
+	}
+}
+
+// 执行任务并累计其忙碌周期
+#define RUN_TIMED(task_enum, call)                                             \
+	do {                                                                       \
+		u32 t0_ = DWT->CYCCNT;                                                 \
+		call;                                                                  \
+		task_busy_cycles[task_enum] += DWT->CYCCNT - t0_;                      \
+	} while (0)
+
 /**
  * @brief 控制模式变化时复位所有控制环，避免积分项跨模式继承
  * @return void
@@ -96,17 +120,19 @@ static void run_motor_control_task(void) {
  */
 static void run_fast_sample_tasks(u32 currentTasks) {
 	if (has_task(currentTasks, TASK_CURRENT_UPDATE)) {
-		sampler.update_current();
+		RUN_TIMED(TASK_CURRENT_UPDATE, sampler.update_current());
 	}
 	if (has_task(currentTasks, TASK_ANGLE_UPDATE)) {
-		KTH7823.update();
-		FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
+		RUN_TIMED(TASK_ANGLE_UPDATE, KTH7823.update());
+		RUN_TIMED(TASK_ANGLE_UPDATE,
+				  FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle()));
 	}
 	if (has_task(currentTasks, TASK_VELOCITY_UPDATE)) {
-		FOC.rt_velocity = Angle_kalman.getVelocity();
+		RUN_TIMED(TASK_VELOCITY_UPDATE,
+				  FOC.rt_velocity = Angle_kalman.getVelocity());
 	}
 	if (has_task(currentTasks, TASK_MOTOR_CONTROL)) {
-		run_motor_control_task();
+		RUN_TIMED(TASK_MOTOR_CONTROL, run_motor_control_task());
 	}
 }
 
@@ -117,19 +143,19 @@ static void run_fast_sample_tasks(u32 currentTasks) {
  */
 static void run_housekeeping_tasks(u32 currentTasks) {
 	if (has_task(currentTasks, TASK_MCU_TEMP_UPDATE)) {
-		sampler.update_mcu_temp();
+		RUN_TIMED(TASK_MCU_TEMP_UPDATE, sampler.update_mcu_temp());
 	}
 	if (has_task(currentTasks, TASK_DRV_TEMP_UPDATE)) {
-		sampler.update_drv_temp();
+		RUN_TIMED(TASK_DRV_TEMP_UPDATE, sampler.update_drv_temp());
 	}
 	if (has_task(currentTasks, TASK_THERMAL_CONTROL)) {
-		thermalController.update();
+		RUN_TIMED(TASK_THERMAL_CONTROL, thermalController.update());
 	}
 	if (has_task(currentTasks, TASK_MAGNETIC_CHECK)) {
-		angsens_MG_check();
+		RUN_TIMED(TASK_MAGNETIC_CHECK, angsens_MG_check());
 	}
 	if (has_task(currentTasks, TASK_VBUS_UPDATE)) {
-		power_manage();
+		RUN_TIMED(TASK_VBUS_UPDATE, power_manage());
 	}
 }
 
@@ -138,11 +164,13 @@ static void run_housekeeping_tasks(u32 currentTasks) {
  * @return void
  */
 static void run_can_task(void) {
+	u32 t0 = DWT->CYCCNT;
 	CAN0_transmiter.test_u16 = CAN0_transmiter.get_node_id();
 	cnt_test = 0;
 	CAN0_transmiter.can0_send_flag = SET;
 	CAN0_transmiter.communication_check();
 	fwdgt_counter_reload();
+	task_busy_cycles[TASK_CAN_TRANSMIT] += DWT->CYCCNT - t0;
 }
 
 /**