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
@@ -308,7 +308,7 @@ void calibrate_zero_position(void) {
 * @return float 返回值
 */
 inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
-							   float current_c, float angle_el) {
+								   float current_c) {
 	if (current_a == ERROR_ADC_VAL || current_b == ERROR_ADC_VAL ||
 		current_c == ERROR_ADC_VAL) {
 		return Current_kalman.getCurrent();
@@ -320,19 +320,17 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
 	current_b -= current_avg;
 	current_c -= current_avg;

-	// 电气角度由调用方传入，避免重复计算
 	// Clarke变换 (使用三相电流)
 	float I_alpha, I_beta;
 	I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b -
 			  (1.0f / 3.0f) * current_c;
 	I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c;

-	// Park变换
-	float sin_el, cos_el;
-	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
+	// Park变换：sin/cos 由 update_el_sincos() 每周期算一次，
+	// 与 setSVPWM_PhaseVoltage 的逆 Park 共用，省一次三角函数
 	float I_q, I_d;
-	I_q = I_beta * cos_el - I_alpha * sin_el;
-	I_d = I_beta * sin_el + I_alpha * cos_el;
+	I_q = I_beta * el_cos - I_alpha * el_sin;
+	I_d = I_beta * el_sin + I_alpha * el_cos;

 	// 返回Iq值，乘以1000以转换为mA
 	return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
@@ -354,6 +352,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 	float min_target = 0.0f;
 	float stop_speed = 0.0f;
 	float brake_err = 0.0f;
+	float learn_err_max = 0.0f;
 	float learn_alpha = 0.0f;
 	float gain = 0.0f;
 	float max_ff = 0.0f;
@@ -366,6 +365,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 		min_target = MODE2_UQ_FF_MIN_TARGET;
 		stop_speed = MODE2_UQ_FF_STOP_SPEED;
 		brake_err = MODE2_UQ_FF_BRAKE_ERR;
+		learn_err_max = MODE2_UQ_FF_LEARN_ERR_MAX;
 		learn_alpha = MODE2_UQ_FF_LEARN_ALPHA;
 		gain = MODE2_UQ_FF_GAIN;
 		max_ff = MODE2_UQ_FF_MAX;
@@ -377,6 +377,7 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 		min_target = MODE3_UQ_FF_MIN_TARGET;
 		stop_speed = MODE3_UQ_FF_STOP_SPEED;
 		brake_err = MODE3_UQ_FF_BRAKE_ERR;
+		learn_err_max = MODE3_UQ_FF_LEARN_ERR_MAX;
 		learn_alpha = MODE3_UQ_FF_LEARN_ALPHA;
 		gain = MODE3_UQ_FF_GAIN;
 		max_ff = MODE3_UQ_FF_MAX;
@@ -386,10 +387,16 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 		return 0.0f;
 	}

+	// 超速/制动判定必须沿"运动方向"：velocity_error×dir_sign 是 PID 系误差，
+	// 再乘运动方向符号后，< -brake_err 才表示"沿运动方向明显超速"。
+	// 原实现只乘 dir_sign：反转(负速度)时判定极性恰好相反——反转超速时
+	// 前馈继续输出、反转欠速时前馈反而被切断，已学到的电压(可达±MAX)
+	// 随误差符号通断，形成正转没有、反转独有的 bang-bang 震荡。
+	const float motion_sign = (target_vel * dir_sign >= 0.0f) ? 1.0f : -1.0f;
 	const bool active = (fabsf(target_vel) > stop_speed) &&
 					(fabsf(actual_vel) > stop_speed) &&
 					((target_vel * actual_vel) > 0.0f) &&
-					((velocity_error * dir_sign) > -brake_err);
+					((velocity_error * dir_sign * motion_sign) > -brake_err);

 	float angle = normalizeAngle360(mech_angle);
 	float index_f = angle / 360.0f * (float)table_size;
@@ -402,7 +409,11 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 	if (ff > max_ff) ff = max_ff;
 	else if (ff < -max_ff) ff = -max_ff;

-	if (learn_enable && active && fabsf(target_vel) > min_target) {
+	// 学习仅在误差带内进行：启停/换向瞬态误差可达上百 °/s，直接学习会把
+	// ±MAX 钳位值写进途经的角度桶，下一轮运动经过该角度时突加满幅前馈
+	// 电压，本身就是一次转矩扰动。
+	if (learn_enable && active && fabsf(target_vel) > min_target &&
+		fabsf(velocity_error * dir_sign) < learn_err_max) {
 		float ff_target = velocity_error * dir_sign * gain;
 		if (ff_target > max_ff) ff_target = max_ff;
 		else if (ff_target < -max_ff) ff_target = -max_ff;
@@ -412,32 +423,44 @@ float FOC_Controller::update_angle_sync_uq_feedforward(
 	return (enable && active && fabsf(target_vel) > min_target) ? ff : 0.0f;
 }
 /**
- * @brief mode3 低速/静止附近降低速度内环 P，运行速度升高后平滑恢复。
+ * @brief mode3 低速缩放因子：静止附近降到 min_scale，速度升高后线性恢复到 1。
+ * @note P/I 使用各自的地板（I 地板更低）：低速段保持"比例主导"，
+ *       积分充电足够慢，压制静摩擦 hunting（停下瞬间小幅来回抖动）。
+ *       D 不衰减，保持阻尼。
 */
-float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
-	float vel_p = vel_angloop_p;
-	if (target_vel < -120.0f) {
-		vel_p = vel_angloop_p * 2.0f;
-	} else if (target_vel < -30.0f) {
-		vel_p = vel_angloop_p * 1.5f;
+float FOC_Controller::get_mode3_velocity_scale(float target_vel,
+											   float actual_vel,
+											   float min_scale) {
+	if (!MODE3_LOW_SPEED_P_SCALE_ENABLE) {
+		return 1.0f;
 	}

-	if (MODE3_LOW_SPEED_P_SCALE_ENABLE) {
-		float abs_target = fabsf(target_vel);
-		float abs_speed = fabsf(actual_vel);
-		float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed;
-		float p_scale = 1.0f;
-		if (speed_ref <= MODE3_LOW_SPEED_P_LOW) {
-			p_scale = MODE3_LOW_SPEED_P_MIN_SCALE;
-		} else if (speed_ref < MODE3_LOW_SPEED_P_FULL) {
-			p_scale = MODE3_LOW_SPEED_P_MIN_SCALE +
-					  (1.0f - MODE3_LOW_SPEED_P_MIN_SCALE) *
-					  (speed_ref - MODE3_LOW_SPEED_P_LOW) /
-					  (MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW);
-		}
-		vel_p *= p_scale;
+	float abs_target = fabsf(target_vel);
+	float abs_speed = fabsf(actual_vel);
+	float speed_ref = (abs_target > abs_speed) ? abs_target : abs_speed;
+	if (speed_ref <= MODE3_LOW_SPEED_P_LOW) {
+		return min_scale;
 	}
-	return vel_p;
+	if (speed_ref < MODE3_LOW_SPEED_P_FULL) {
+		return min_scale +
+			   (1.0f - min_scale) * (speed_ref - MODE3_LOW_SPEED_P_LOW) /
+				   (MODE3_LOW_SPEED_P_FULL - MODE3_LOW_SPEED_P_LOW);
+	}
+	return 1.0f;
+}
+
+/**
+ * @brief mode3 速度内环 P：低速缩放因子（方向无关）。
+ * @note 历史上曾有"反转 P 提升到 ×2"的增益调度（先是 −120/−30 硬台阶，
+ *       后改为连续线性过渡）：连续化后反转仍剧烈震荡，说明问题不在
+ *       台阶突跳，而在增益本身——×2 只对反转生效，把反转环路增益整体
+ *       翻倍而正转不变，正是"正转正常、反转震荡"的直接来源之一。
+ *       已移除。反转阻力差异由 REV 齿槽补偿表与 Uq 前馈学习承担；
+ *       若实测反转欠速/偏慢，优先增大前馈学习限幅，不要恢复非对称 P。
+ */
+float FOC_Controller::get_mode3_velocity_p(float target_vel, float actual_vel) {
+	return vel_angloop_p * get_mode3_velocity_scale(target_vel, actual_vel,
+													MODE3_LOW_SPEED_P_MIN_SCALE);
 }

 /**
@@ -489,14 +512,23 @@ u8 FOC_Controller::init() {
 	memcpy(cogging_table_rev, COGGING_TABLE_REV, sizeof(COGGING_TABLE_REV));
 	cogging_comp_enabled = true;

-	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
-	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
-	FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
+	// limit_voltage 从 Flash/CAN 恢复，可能为 0：分母下限保护，防止 P=inf
+	const float denom = (limit_voltage > MIN_LIMIT_VOLTAGE)
+							? limit_voltage
+							: voltage_power_supply;
+	FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / denom;
+	FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / denom;
+	FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / denom;
 	FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
 	FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
-	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
-	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
-	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
+	FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / denom;
+	FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / denom;
+	FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / denom;
+
+	// 运行期不每周期 set_P 的环路（mode1/mode5）在此同步一次有效增益，
+	// 避免沿用构造期的临时值
+	angle_loop.set_P(ang_loop_p);
+	current_loop.set_P(cur_loop_p);
 	return 0;
 }

@@ -506,6 +538,15 @@ u8 FOC_Controller::init() {
 */
 FOC_Controller::FOC_Controller() { ; }

+/**
+ * @brief 每控制周期调用一次：计算当前电角度的 sin/cos。
+ * @note cal_Iq_Id 的 Park 变换与 setSVPWM_PhaseVoltage 的逆 Park 变换
+ *       使用同一电角度，三角函数每周期只算这一次，两处共用。
+ */
+inline void FOC_Controller::update_el_sincos(float angle_el) {
+	arm_sin_cos_f32(angle_el, &el_sin, &el_cos);
+}
+
 /**
 * @brief 使用SVPWM算法设置电机输出
 *
@@ -514,13 +555,12 @@ FOC_Controller::FOC_Controller() { ; }
 *
 * 优化：atan2f → X/Y/Z 符号法判扇区（~15周期 vs atan2f ~300周期）
 *       边界上扇区编号可能与 atan2f 差1，但 duty 输出完全一致（已数值验证）
+ *       逆 Park 所需 sin/cos 由 update_el_sincos() 预计算，不再重复计算
 */
-inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud, float angle_el) {
-	// ① 逆Park变换（angle_el 由调用方传入）
-	float sin_el, cos_el;
-	arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
-	float Ualpha = Ud * cos_el - Uq * sin_el;
-	float Ubeta  = Ud * sin_el + Uq * cos_el;
+inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
+	// ① 逆Park变换（sin/cos 为本周期预计算值）
+	float Ualpha = Ud * el_cos - Uq * el_sin;
+	float Ubeta  = Ud * el_sin + Uq * el_cos;

 	// ② 扇区判定（替代 atan2f）
 	float X =  Ubeta;
@@ -727,11 +767,11 @@ void FOC_Controller::update_power(float voltage) {
 * @return void
 */
 void FOC_Controller::fast_voltage_check(void) {
-	// ADC1 单次转换读取 VBUS（ch6，11:1 分压）。
-	// 旧实现读 regular DMA 的 adc_value[2]，通道顺序调整后该下标已是 DRV温度，
-	// 属漏改：NTC 分压值(~1.06V)按 11 倍分压换算仅 ~11.5V，逼近跌落阈值。
+	// ADC1 连续模式零等待读最新 VBUS（ch6，11:1 分压）。
+	// 旧实现 adc1_read_raw 阻塞式"触发+等EOC"：10kHz 调用每次 ~1.5μs 死等。
+	// 通道顺序问题的历史注释已随 ADC1 连续模式改造失效（本函数只读 DR）。
 	float current_vbus =
-		adc1_read_raw(ADC_CHANNEL_6) * (3.3f / 4096.0f) * 11.0f;
+		adc1_read_vbus_latest() * (3.3f / 4096.0f) * 11.0f;

 	// 更新基准电压（峰值检测）
 	// 电压上升时，基准电压跟随；电压下跌时，基准电压保持
@@ -782,8 +822,9 @@ float FOC_Controller::velocity_open_loop() {
 		normalizeAngle360(shaft_angle + target_velocity * REDUCTION_RATIO / MOTOR_FREQUENCY);
 	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
 	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el_cog);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));

 	// 因为在开环控制中，Ud 始终为 0，只控制 Uq
 	// 幅度限幅必须保留符号：原 _constrain 下限为 +2V，负速度会被钳到 +2V，
@@ -801,7 +842,7 @@ float FOC_Controller::velocity_open_loop() {
 		}
 		Uq = (Uq < 0.0f) ? -Uq_mag : Uq_mag;
 	}
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 	return limit_voltage;
 }

@@ -812,17 +853,19 @@ float FOC_Controller::velocity_open_loop() {
 * @return float: 当前角度
 */
 float FOC_Controller::angle_control() {
-	// PID 控制计算
-	float Uq = angle_loop(target_angle - rt_angle);
+	// PID 控制计算（D 项对测量微分+低通，目标阶跃无微分踢）
+	angle_loop.set_P(ang_loop_p);
+	float Uq = angle_loop(target_angle - rt_angle, rt_angle);

 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
 	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el_cog);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 	return shaft_angle;
 }

@@ -842,24 +885,45 @@ float FOC_Controller::velocity_control() {
 	// mode2 使用独立速度环参数。
 	vel_loop.set_PID(vel_loop_p, vel_loop_Ki, vel_loop_Kd);
 	vel_loop.set_limit(limit_voltage);
-	float Uq_pid = vel_loop(velocity_error);
+
+	// 仅"实质性换向"时清积分：目标速度变号且新幅值够大（真实反向指令）才清。
+	// 最终贴合阶段误差过零引起的微小变号（|target|=150×0.01°≈1.5°/s）不清——
+	// 否则积分被反复清空，低速段缩小的 P 项（0.12V@0.01°）推不过静摩擦，
+	// 永远停在差 ~0.01° 的位置
+	static float last_mode2_target_vel = 0.0f;
+	if (last_mode2_target_vel * target_velocity < 0.0f &&
+		fabsf(target_velocity) > 5.0f) {
+		vel_loop.reset_integral();
+	}
+	last_mode2_target_vel = target_velocity;
+
+	float Uq_pid =
+		vel_loop(velocity_error, rt_velocity * DIR * REDUCTION_RATIO);

 	static float uq_ff_table[MODE2_UQ_FF_TABLE_SIZE] = {0.0f};
 	float Uq_ff = update_angle_sync_uq_feedforward(
 		2, uq_ff_table, MODE2_UQ_FF_TABLE_SIZE, shaft_angle,
 		target_velocity, rt_velocity, velocity_error);

+	// PID 输出已内部钳位；前馈叠加后超限时泄放积分，
+	// 让 PID 感知真实饱和（否则重载下积分继续累积→释载超调振荡）
 	float Uq = Uq_pid + Uq_ff;
-	if (Uq > limit_voltage) Uq = limit_voltage;
-	else if (Uq < -limit_voltage) Uq = -limit_voltage;
+	if (Uq > limit_voltage) {
+		vel_loop.bleed_integral(Uq - limit_voltage);
+		Uq = limit_voltage;
+	} else if (Uq < -limit_voltage) {
+		vel_loop.bleed_integral(Uq + limit_voltage);
+		Uq = -limit_voltage;
+	}
 	CAN0_transmiter.testfloat_union.f = Uq;

 	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
 	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el_cog);
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));

 	return rt_velocity;
 }
@@ -874,18 +938,20 @@ float FOC_Controller::current_control() {
 	CAN0_transmiter.testfloat_union.f = shaft_angle;
 	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
 	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el_cog);
 	// PID 控制计算
 	float Iq = rt_current =
 		cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-				  sampler.get_current('W'), angle_el_cog);
-	float Uq = current_loop(target_current - Iq);
+				  sampler.get_current('W'));
+	current_loop.set_P(cur_loop_p);
+	float Uq = current_loop(target_current - Iq, Iq);
 	if (Uq > limit_voltage) {
 		Uq = limit_voltage;
 	} else if (Uq < -limit_voltage) {
 		Uq = -limit_voltage;
 	}
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
 	return 1;
 }

@@ -901,21 +967,28 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 	float angle_error = target_angle - rt_angle;

 	ang_inVelAngloop.set_P(ang_angloop_p);
-	// 使用角度PID控制器计算目标速度
-	float target_velocity = ang_inVelAngloop(angle_error);
+	// 使用角度PID控制器计算目标速度（D 项对测量微分+低通）
+	float target_velocity = ang_inVelAngloop(angle_error, rt_angle);

 	// 使用速度PID控制器计算最终输出电压
-	vel_inVelAngloop.set_P(get_mode3_velocity_p(target_velocity, rt_velocity));
+	// 低速段 P/I 各自按地板衰减（D 保持满值）：I 地板更低（0.15），
+	// 停止瞬间积分充电速率仅为全速的 15%，进一步压制静摩擦 hunting
+	float i_scale = get_mode3_velocity_scale(target_velocity, rt_velocity,
+											 MODE3_LOW_SPEED_I_MIN_SCALE);
+	vel_inVelAngloop.set_PID(get_mode3_velocity_p(target_velocity, rt_velocity),
+							 vel_angloop_Ki * i_scale, vel_angloop_Kd);
 	float velocity_error = target_velocity - rt_velocity;

-	// 目标速度突变检测：target 大幅变化时清积分
+	// 仅"实质性换向"时清积分（变号且新幅值>5°/s）。制动段不清（防欠程）；
+	// 最终贴合阶段误差过零的微小变号也不清（防积分反复被清、停在差0.01°）
 	static float last_vang_target_vel = 0.0f;
-	if (fabsf(target_velocity - last_vang_target_vel) > 5.0f) {
+	if (last_vang_target_vel * target_velocity < 0.0f &&
+		fabsf(target_velocity) > 5.0f) {
 		vel_inVelAngloop.reset_integral();
 	}
 	last_vang_target_vel = target_velocity;

-	float Uq_pid = vel_inVelAngloop(velocity_error) * DIR;
+	float Uq_pid = vel_inVelAngloop(velocity_error, rt_velocity) * DIR;

 	// 获取电机轴角度：mode3 Uq 前馈补偿按物理 360° 查表。
 	shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -924,18 +997,25 @@ float FOC_Controller::angle_control_with_velocity_limit() {
 		3, mode3_uq_ff_table, MODE3_UQ_FF_TABLE_SIZE, shaft_angle,
 		target_velocity, rt_velocity, velocity_error);

+	// 前馈叠加后超限时泄放积分（×DIR 补偿：Uq = PID输出×DIR + 前馈）
 	float Uq = Uq_pid + Uq_ff;
-	if (Uq > limit_voltage) Uq = limit_voltage;
-	else if (Uq < -limit_voltage) Uq = -limit_voltage;
+	if (Uq > limit_voltage) {
+		vel_inVelAngloop.bleed_integral(DIR * (Uq - limit_voltage));
+		Uq = limit_voltage;
+	} else if (Uq < -limit_voltage) {
+		vel_inVelAngloop.bleed_integral(DIR * (Uq + limit_voltage));
+		Uq = -limit_voltage;
+	}
 	CAN0_transmiter.testfloat_union.f = shaft_angle;

 	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
 	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el_cog);
 	// 获取当前实际电流值
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));
 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);

 	return shaft_angle;
 }
@@ -952,36 +1032,38 @@ float FOC_Controller::MIT_Mode() {
 	float angle_error = target_angle - rt_angle;
 	ang_inMITloop.set_P(ang_MIT_p);
 	float target_velocity =
-		ang_inMITloop(angle_error);
+		ang_inMITloop(angle_error, rt_angle);

 	// 第二层：速度环（中环）
 	float velocity_error = (target_velocity - rt_velocity)* DIR;

-	// 目标速度变化检测：target 大幅变化时清积分
+	// 仅"实质性换向"时清积分（变号且新幅值>5°/s，理由同 mode3）
 	static float last_mit_target_vel = 0.0f;
-	if (fabsf(target_velocity - last_mit_target_vel) > 5.0f) {
+	if (last_mit_target_vel * target_velocity < 0.0f &&
+		fabsf(target_velocity) > 5.0f) {
 		vel_inMITloop.reset_integral();
 	}
 	last_mit_target_vel = target_velocity;

 	vel_inMITloop.set_P(vel_MIT_p);
-	float target_current = vel_inMITloop(velocity_error);
+	float target_current = vel_inMITloop(velocity_error, rt_velocity * DIR);

 	// 获取电机轴角度
 	shaft_angle = KTH7823.getcurrent_sensorangle();
 	// 电气角度（算一次，供 cal_Iq_Id / 齿槽补偿 / setSVPWM 共用）
 	float angle_el_cog = normalizeAngle360(shaft_angle * PP + KTH7823.calibration_angle);
+	update_el_sincos(angle_el_cog);

 	// 第三层：电流环（内环）
 	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
-						   sampler.get_current('W'), angle_el_cog);
+						   sampler.get_current('W'));

 	float current_error = target_current - rt_current;
 	current_inMITloop.set_P(cur_MIT_p);
-	float Uq = current_inMITloop(current_error);
+	float Uq = current_inMITloop(current_error, rt_current);

 	// 设置相电压
-	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f, angle_el_cog);
+	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);

 	return shaft_angle;
 }