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/sys/health.cpp

@@ -10,6 +10,8 @@
 #include "FOC.h"
 #include "adc.h"
 #include "flash.h"
+#include "motor.h"
+#include <cmath>
 
 
 extern sampler sampler;
@@ -19,11 +21,10 @@ extern FlashEEPROM flashEEP;
 // 全局热管理控制器单例：由 main 任务周期调用 update()，用于维护过热保护状态。
 ThermalController thermalController; // 热量控制器初始化
 
-/** @brief max 宏定义 */
-#define max(a, b, c)                                                           \
-	((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))
 /** @brief PROTECT_TIME 常量定义 */
 constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
+// 三相电流连续全部失效帧数（×10ms）阈值，达到后按过热关机处理
+constexpr u16 CURRENT_ERROR_SHUTDOWN_COUNT = 100;
 
 // 电压管理状态机：仅 power_manage() 慢速任务访问，文件内封装。
 static VoltageState voltage_state = VoltageState::NORMAL;
@@ -34,6 +35,14 @@ static float last_vbus_slow = 0.0f;	// 上次电压值（慢速任务用）
 ** @retval 热量值
 */
 u32 ThermalController::update() {
+#if GATE_DRIVER_DRV8323
+	// nFAULT 快速保护（低有效，1ms 轮询）：GDF/UVLO/OTSD 等硬件故障
+	// → 立即封 PWM 并睡眠，锁存到 CAN 重新下发恢复命令为止。
+	// VDS 过流当前配置为"仅上报不动作"，不会拉低 nFAULT，不在此误触发。
+	if (drv_nfault_active()) {
+		FOC.shut_down();
+	}
+#endif
 	thermal_value = Q16_16_MUL(thermal_value, thermalRate);
 	// 添加温度积累
 	int drv_temp = (int)sampler.get_drv_temp();
@@ -60,28 +69,65 @@ u32 ThermalController::update() {
 		return --remainingTime10Ms;
 	} else {
 		// 当前没有过热停机
-		// 添加电流热积累
-		u32 currents[3] = {(u32)(sampler.get_current('U') * 1000),
-						   (u32)(sampler.get_current('V') * 1000),
-						   (u32)(sampler.get_current('W') * 1000)};
+		// 添加电流热积累：取三相有效相的最大绝对值（热量与电流方向无关，
+		// 负半周同样发热）。采样失效帧(ERROR_ADC_VAL)不参与积累：
+		// 失效值 255 按 mA 参与平方运算会超出 int/u32 范围，热积累不可控；
+		// 三相持续全部失效说明采样链异常，按过热关机 fail-safe。
+		static u16 current_error_frames = 0;
+		const float raw_current[3] = {sampler.get_current('U'),
+									  sampler.get_current('V'),
+									  sampler.get_current('W')};
+		u32 currents[3] = {0, 0, 0};
+		bool valid[3] = {false, false, false};
+		for (u8 i = 0; i < 3; i++) {
+			if (raw_current[i] == ERROR_ADC_VAL) {
+				continue;
+			}
+			// get_current 已保证有效值 |I| ≤ MAX_CURRENT(40A)，转 mA ≤ 40000
+			currents[i] = (u32)(fabsf(raw_current[i]) * 1000.0f);
+			valid[i] = true;
+		}
 
-		// 找最大电流（原始ADC值）
-		current_max = max(currents[0], currents[1], currents[2]);
+		if (!(valid[0] || valid[1] || valid[2])) {
+			if (++current_error_frames >= CURRENT_ERROR_SHUTDOWN_COUNT) {
+				current_error_frames = 0;
+				FOC.shut_down_thermal();
+				remainingTime10Ms = PROTECT_TIME;
+				CAN0_transmiter.ThermalStatus = 1;
+				return remainingTime10Ms;
+			}
+			return thermal_value;
+		}
+		current_error_frames = 0;
+
+		// 找最大电流（mA，绝对值）；个别相失效时用剩余有效相
+		u32 max_current = 0;
+		for (u8 i = 0; i < 3; i++) {
+			if (valid[i] && currents[i] > max_current) {
+				max_current = currents[i];
+			}
+		}
+		current_max = (u16)max_current;
+		// 平方运算先转 float：40A 档 40000²×10 超 int 范围，整数运算会溢出
+		const float imax = (float)current_max;
+		// 电流系数按"绝对值全周期"口径标定：取 fabs 后正负半周都积累，
+		// 交流负载下积累速度约为旧版(仅正半周)的 2 倍，故系数统一取旧值一半，
+		// 保持原实测触发时机不变；温度积累与恢复阈值不受此补偿影响。
 		if (current_max < 2000) {
-			thermal_value += current_max * current_max * gapTimeMs *
-							 0.00007f; // 计算当前电流热积累值
+			thermal_value += (u32)(imax * imax * gapTimeMs *
+								 0.000035f); // 计算当前电流热积累值
 		} else if (current_max > 3800) {
-			thermal_value += current_max * current_max * gapTimeMs *
-							 0.01f; // 添加当前电流热积累值
+			thermal_value += (u32)(imax * imax * gapTimeMs *
+								 0.005f); // 添加当前电流热积累值
 		} else if (current_max > 3400) {
-			thermal_value += current_max * current_max * gapTimeMs *
-							 0.005f; // 计算当前电流热积累值
+			thermal_value += (u32)(imax * imax * gapTimeMs *
+								 0.0025f); // 计算当前电流热积累值
 		} else if (current_max > 3000) {
-			thermal_value += current_max * current_max * gapTimeMs *
-							 0.0002f; // 计算当前电流热积累值
+			thermal_value += (u32)(imax * imax * gapTimeMs *
+								 0.0001f); // 计算当前电流热积累值
 		} else if (current_max > 2000) {
-			thermal_value += current_max * current_max * gapTimeMs *
-							 0.0001f; // 计算当前电流热积累值
+			thermal_value += (u32)(imax * imax * gapTimeMs *
+								 0.00005f); // 计算当前电流热积累值
 		}
 	}