Motor_v2/Robot_V2/Algorithm/FOC.cpp

/**
 * @file Algorithm/FOC.cpp
 * @brief 算法模块源文件
 * @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
 */

#include "FOC.h"
#include "Can.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "config.h"
#include "flash.h"
#include "motor.h"
#include "pid.h"
#include "systick.h"
#include <arm_math.h>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <gd32c10x.h>

// 全局 FOC 控制器单例：系统只有一个电机控制对象。
// 初始化顺序由 main::hardware_init() 明确调用 FOC.init() 完成；
// 运行期主要由主循环控制任务访问，CAN 命令只更新目标/参数。
FOC_Controller FOC; // FOC控制初始化
extern sampler sampler;
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern CurrentKalman Current_kalman;
extern FlashEEPROM flashEEP;

// 齿槽转矩补偿：查表 + 线性插值
// angle_el: 电角度（度）
// 返回: 补偿电压值 (V)，叠加到 Uq 上
/**
 * @brief 获取 get_cogging_compensation 对应的数据或状态
 * @param[in] angle_el 参数
 * @return float 返回值
 */
inline float FOC_Controller::get_cogging_compensation(float angle_el) {
	if (!cogging_comp_enabled) return 0.0f;

	// angle_el 已由调用方归一化到 [0, 360)，无需重复
	float angle = angle_el;

	// 线性插值查表
	float index_f = angle / 360.0f * (float)COGGING_TABLE_SIZE;
	u16 idx0 = (u16)index_f;
	u16 idx1 = (idx0 + 1) % COGGING_TABLE_SIZE;
	float frac = index_f - (float)idx0;

	// 根据转向选择补偿表（带滞回，避免零速附近跳变）
	static float *last_table = cogging_table;
	if (rt_velocity < -0.5f) last_table = cogging_table_rev;
	else if (rt_velocity > 0.5f) last_table = cogging_table;
	float *table = last_table;
	float comp = table[idx0] * (1.0f - frac) + table[idx1] * frac;

	return -comp * cogging_comp_scale; // mA→V: 补偿电流(mA) × 换算系数(V/mA)
}

// === 齿槽转矩校准（DEBUG，平时不启用）===
// 启用方法：config.h 中 FOC_DEBUG 改为 1，通过 CAN 命令触发（RESET_ID bit4）
// 校准完成后把 C Array 硬编码到 init() 中，再改回 FOC_DEBUG 0
#if FOC_DEBUG
// 齿槽转矩校准参数
constexpr u32 COG_CALIB_SETTLE_TICKS = 16000;       // 800ms / 50us
constexpr u32 COG_CALIB_STOP_TICKS = 10000;         // 500ms / 50us
constexpr u32 COG_CALIB_TARGET_SAMPLES = 3 * COGGING_TABLE_SIZE * 8;
constexpr float COG_CALIB_SLOW_VEL = 1.0f;          // 开环速度 rad/s
constexpr u32 COG_CALIB_SAMPLE_DIV = 20;            // 1ms / 50us
constexpr u8 COG_CALIB_TOTAL_RUNS = 6;              // 3正+3反

// 齿槽转矩校准静态数据
float FOC_Controller::cog_calib_run_data[6][COGGING_TABLE_SIZE] = {};
float FOC_Controller::cog_calib_accum[COGGING_TABLE_SIZE] = {};
u32 FOC_Controller::cog_calib_count[COGGING_TABLE_SIZE] = {};

/**
 * @brief 齿槽转矩校准 - 非阻塞状态机
 * 正转3次 + 反转3次，分开存储补偿表。
 */
void FOC_Controller::calibrate_cogging_tick(void) {
	// 入口只做状态派发，避免单函数堆叠所有校准分支。
	switch (cog_calib_state) {
	case CogCalibState::IDLE:
		start_cogging_calibration();
		return;
	case CogCalibState::SETTLING:
		cogging_settling_tick();
		return;
	case CogCalibState::SAMPLING:
		cogging_sampling_tick();
		return;
	case CogCalibState::STOPPING:
		cogging_stopping_tick();
		return;
	case CogCalibState::DONE:
		cogging_done_tick();
		return;
	default:
		// 防御性处理：枚举值异常时回到空闲态，避免状态机卡死。
		cog_calib_state = CogCalibState::IDLE;
		return;
	}
}

void FOC_Controller::start_cogging_calibration(void) {
	// IDLE 状态下没有请求则保持空闲，不扰动正常控制路径。
	if (!cogging_calib_request) return;
	cogging_calib_request = false;

	cog_calib_run = 0;
	// 保存原控制状态，校准完成后恢复现场。
	cog_calib_saved_mode = control_mode;
	cog_calib_saved_vel = target_velocity;
	cogging_comp_enabled = false;
	memset(cog_calib_run_data, 0, sizeof(cog_calib_run_data));

	printf("\r\n[COG] === Cogging Calibration: 3 fwd + 3 rev ===\r\n");

	control_mode = 6;
	target_velocity = COG_CALIB_SLOW_VEL;
	cog_calib_settle_tick = 0;
	cog_calib_state = CogCalibState::SETTLING;
}

void FOC_Controller::cogging_settling_tick(void) {
	// 给低速开环运行留出稳定时间，再开始采样。
	cog_calib_settle_tick++;
	if (cog_calib_settle_tick < COG_CALIB_SETTLE_TICKS) return;

	cog_calib_state = CogCalibState::SAMPLING;
	cog_calib_sample_count = 0;
	memset(cog_calib_accum, 0, sizeof(cog_calib_accum));
	memset(cog_calib_count, 0, sizeof(cog_calib_count));
}

void FOC_Controller::cogging_sampling_tick(void) {
	static u32 sample_div = 0;
	// 每 1ms 采一次，降低校准数据相关处理对实时控制的影响。
	sample_div++;
	if (sample_div < COG_CALIB_SAMPLE_DIV) return;
	sample_div = 0;

	float real_angle = KTH7823.getcurrent_sensorangle();
	float angle_el = normalizeAngle360(real_angle * PP + KTH7823.calibration_angle);
	float iq = rt_current;

	if (fabsf(iq) < 5000.0f) {
		u16 idx = (u16)(angle_el / 360.0f * (float)COGGING_TABLE_SIZE);
		if (idx >= COGGING_TABLE_SIZE) idx = COGGING_TABLE_SIZE - 1;
		cog_calib_accum[idx] += iq;
		cog_calib_count[idx]++;
	}
	cog_calib_sample_count++;

	if (cog_calib_sample_count >= COG_CALIB_TARGET_SAMPLES) {
		target_velocity = 0;
		cog_calib_state = CogCalibState::STOPPING;
		cog_calib_stop_tick = 0;
	}
}

void FOC_Controller::cogging_stopping_tick(void) {
	// 每轮采样结束后先停稳，再计算本轮去直流数据。
	cog_calib_stop_tick++;
	if (cog_calib_stop_tick < COG_CALIB_STOP_TICKS) return;

	float dc = process_cogging_run_data();
	printf("[COG] Run %d/%d done. DC=%.2f mA\r\n",
		   cog_calib_run + 1, COG_CALIB_TOTAL_RUNS, dc);

	cog_calib_run++;
	if (cog_calib_run < COG_CALIB_TOTAL_RUNS) {
		start_next_cogging_run();
	} else {
		cog_calib_state = CogCalibState::DONE;
	}
}

float FOC_Controller::process_cogging_run_data(void) {
	// 先求每个角度桶的平均电流，再统一去除直流偏置。
	float dc = 0;
	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
		cog_calib_run_data[cog_calib_run][i] =
			(cog_calib_count[i] > 0) ? cog_calib_accum[i] / (float)cog_calib_count[i] : 0.0f;
		dc += cog_calib_run_data[cog_calib_run][i];
	}
	dc /= (float)COGGING_TABLE_SIZE;
	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
		cog_calib_run_data[cog_calib_run][i] -= dc;
	}
	return dc;
}

void FOC_Controller::start_next_cogging_run(void) {
	control_mode = 6;
	target_velocity = (cog_calib_run < 3) ? COG_CALIB_SLOW_VEL : -COG_CALIB_SLOW_VEL;
	cog_calib_state = CogCalibState::SETTLING;
	cog_calib_settle_tick = 0;
}

void FOC_Controller::cogging_done_tick(void) {
	// 生成最终表后恢复用户进入校准前的控制模式和目标速度。
	build_cogging_tables();
	cogging_comp_enabled = true;
	control_mode = cog_calib_saved_mode;
	target_velocity = cog_calib_saved_vel;

	print_cogging_table("\r\n[COG] === FWD Average ===\r\n", cogging_table);
	print_cogging_table("\r\n[COG] === REV Average ===\r\n", cogging_table_rev);
	print_cogging_c_array("\r\n[COG] --- FWD C Array ---\r\n",
						 "COGGING_TABLE_FWD", cogging_table);
	print_cogging_c_array("\r\n[COG] --- REV C Array ---\r\n",
						 "COGGING_TABLE_REV", cogging_table_rev);

	printf("\r\n[COG] === Done. ===\r\n");
	cog_calib_state = CogCalibState::IDLE;
}

void FOC_Controller::build_cogging_tables(void) {
	// 前 3 轮为正转，后 3 轮为反转，分别求平均表。
	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
		cogging_table[i] = (cog_calib_run_data[0][i] +
							 cog_calib_run_data[1][i] +
							 cog_calib_run_data[2][i]) / 3.0f;
		cogging_table_rev[i] = (cog_calib_run_data[3][i] +
								 cog_calib_run_data[4][i] +
								 cog_calib_run_data[5][i]) / 3.0f;
	}
}

void FOC_Controller::print_cogging_table(const char *title, float *table) {
	printf("%s", title);
	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
		float angle_deg = (float)i / (float)COGGING_TABLE_SIZE * 360.0f;
		printf("[COG] %.2f, %.2f\r\n", angle_deg, table[i]);
		fwdgt_counter_reload();
	}
}

void FOC_Controller::print_cogging_c_array(const char *title,
										   const char *arrayName,
										   float *table) {
	printf("%s", title);
	printf("static const float %s[COGGING_TABLE_SIZE] = {\r\n", arrayName);
	for (int i = 0; i < COGGING_TABLE_SIZE; i++) {
		if (i % 8 == 0) printf("    ");
		printf("%.2ff", table[i]);
		if (i < COGGING_TABLE_SIZE - 1) printf(", ");
		if (i % 8 == 7 || i == COGGING_TABLE_SIZE - 1) printf("\r\n");
		fwdgt_counter_reload();
	}
	printf("};\r\n");
}
#endif // FOC_DEBUG

// 初始变量及函数定义
#define _constrain(amt, low, high)                                             \
	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
/** @brief constrain 宏定义 */
#define constrain(value, min, max, target_min, target_max)                     \
	((value - min) / (max - min) * (target_max - target_min))

// 校准步骤（代码模板）
/**
 * @brief calibrate_zero_position 函数
 * @return void
 */
void calibrate_zero_position(void) {
	// 运行中禁止校准：要求控制模式为停止(0)且基本静止，
	// 否则转子无法对齐到电气 0°，标定结果错误，甚至因电角度突变导致失控。
	if (FOC.control_mode != 0 || fabsf(FOC.rt_velocity) > CALIB_MAX_SPEED) {
		return;
	}

	// 使能电机驱动器，准备对齐
	motor_resume();

	// 施加固定电压矢量（A 相高电平），让转子对齐到电气 0° 位置
	Set_3Pwm(ALIGN_PWM_DUTY, 0.0f, 0.0f);
	delay_1ms(ALIGN_SETTLE_MS);

	// 对齐稳定后重新采样传感器角度，再计算零位补偿角，使当前位置对应电气 0°
	KTH7823.update();
	float current_sensor_angle = KTH7823.getcurrent_sensorangle();
	float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f);
	KTH7823.set_calibration(-electrical_angle);

	// 停止施加电压
	Set_3Pwm(0.0f, 0.0f, 0.0f);

	// 保存校准角到 Flash，掉电不丢
	flashEEP.save_calibration_angle(KTH7823.calibration_angle);
}

// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
/**
 * @brief cal_Iq_Id 函数
 * @param[in] current_a 参数
 * @param[in] current_b 参数
 * @param[in] current_c 参数
 * @param[in] angle_el 参数
 * @return float 返回值
 */
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
								   float current_c) {
	if (current_a == ERROR_ADC_VAL || current_b == ERROR_ADC_VAL ||
		current_c == ERROR_ADC_VAL) {
		return Current_kalman.getCurrent();
	}

	// 去除三相公共模偏置，使 Ia + Ib + Ic 更接近 0
	const float current_avg = (current_a + current_b + current_c) / 3.0f;
	current_a -= current_avg;
	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变换：sin/cos 由 update_el_sincos() 每周期算一次，
	// 与 setSVPWM_PhaseVoltage 的逆 Park 共用，省一次三角函数
	float I_q, I_d;
	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);
	// return I_q * 1000 - KE_FACTOR * rt_velocity;
}


/**
 * @brief 角度同步 Uq 前馈更新/输出。
 * @note 只在稳定同向巡航时输出和学习；停止、低速、反向、明显刹车/超速时立即退出。
 */
float FOC_Controller::update_angle_sync_uq_feedforward(
	u8 mode, float table[], u16 table_size, float mech_angle,
	float target_vel, float actual_vel, float velocity_error) {
	if (table_size == 0U) return 0.0f;

	bool enable = false;
	bool learn_enable = false;
	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;
	float dir_sign = 1.0f;

	switch (mode) {
	case 2:
		enable = MODE2_UQ_FF_ENABLE;
		learn_enable = MODE2_UQ_FF_LEARN_ENABLE;
		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;
		dir_sign = 1.0f;
		break;
	case 3:
		enable = MODE3_UQ_FF_ENABLE;
		learn_enable = MODE3_UQ_FF_LEARN_ENABLE;
		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;
		dir_sign = DIR;
		break;
	default:
		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 * motion_sign) > -brake_err);

	float angle = normalizeAngle360(mech_angle);
	float index_f = angle / 360.0f * (float)table_size;
	u16 idx0 = (u16)index_f;
	if (idx0 >= table_size) idx0 = table_size - 1U;
	u16 idx1 = (idx0 + 1U) % table_size;
	float frac = index_f - (float)idx0;

	float ff = table[idx0] * (1.0f - frac) + table[idx1] * frac;
	if (ff > max_ff) ff = max_ff;
	else if (ff < -max_ff) ff = -max_ff;

	// 学习仅在误差带内进行：启停/换向瞬态误差可达上百 °/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;
		table[idx0] += learn_alpha * (ff_target - table[idx0]);
	}

	return (enable && active && fabsf(target_vel) > min_target) ? ff : 0.0f;
}
/**
 * @brief mode3 低速缩放因子：静止附近降到 min_scale，速度升高后线性恢复到 1。
 * @note P/I 使用各自的地板（I 地板更低）：低速段保持"比例主导"，
 *       积分充电足够慢，压制静摩擦 hunting（停下瞬间小幅来回抖动）。
 *       D 不衰减，保持阻尼。
 */
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;
	}

	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;
	}
	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);
}

/**
 * @brief 电机控制类初始化函数
 *
 * @return u8 0表示初始化成功，1表示初始化失败
 */
u8 FOC_Controller::init() {
	motor_init(); // 电机驱动初始化

	// 齿槽转矩补偿表（硬编码，3正+3反平均，用真实角度算Iq）
	static const float COGGING_TABLE_FWD[COGGING_TABLE_SIZE] = {
		29.28f, 28.84f, 28.26f, 28.83f, 29.79f, 30.26f, 29.52f, 28.77f,
		28.23f, 27.68f, 26.65f, 25.19f, 23.92f, 22.59f, 19.61f, 16.77f,
		13.32f, 10.40f, 7.64f, 3.51f, -0.83f, -4.96f, -8.19f, -11.11f,
		-13.40f, -14.39f, -15.69f, -15.99f, -16.39f, -15.50f, -14.76f, -14.21f,
		-13.76f, -13.88f, -13.22f, -11.18f, -10.65f, -10.54f, -10.62f, -10.23f,
		-9.52f, -8.19f, -6.31f, -4.14f, -1.94f, -0.25f, 2.06f, 4.08f,
		6.05f, 8.30f, 9.74f, 11.38f, 13.32f, 15.65f, 17.55f, 20.22f,
		21.72f, 22.28f, 23.30f, 24.78f, 23.53f, 23.35f, 23.00f, 21.42f,
		20.35f, 20.57f, 20.07f, 18.18f, 17.04f, 16.03f, 14.81f, 13.71f,
		12.47f, 10.93f, 9.38f, 6.53f, 4.63f, 3.37f, 1.29f, -1.28f,
		-5.08f, -11.11f, -16.35f, -21.00f, -24.61f, -28.30f, -31.11f, -33.48f,
		-35.25f, -36.54f, -38.01f, -38.69f, -39.27f, -39.52f, -38.18f, -36.55f,
		-34.96f, -33.14f, -31.92f, -31.45f, -31.32f, -30.25f, -29.60f, -28.72f,
		-27.51f, -25.30f, -22.66f, -20.87f, -18.19f, -16.10f, -13.25f, -10.81f,
		-8.00f, -4.67f, -2.28f, 0.87f, 3.83f, 7.25f, 10.56f, 13.70f,
		17.70f, 19.90f, 22.14f, 23.62f, 24.53f, 26.03f, 26.96f, 27.95f,
	};
	static const float COGGING_TABLE_REV[COGGING_TABLE_SIZE] = {
		22.45f, 24.95f, 26.05f, 26.01f, 26.11f, 25.87f, 26.20f, 26.23f,
		26.19f, 25.87f, 25.97f, 26.09f, 24.77f, 24.22f, 23.71f, 22.10f,
		19.84f, 17.60f, 15.36f, 12.99f, 9.54f, 5.35f, 1.71f, -1.94f,
		-4.15f, -6.16f, -7.93f, -9.10f, -10.01f, -10.25f, -10.85f, -11.74f,
		-11.98f, -10.78f, -9.65f, -8.89f, -8.14f, -6.85f, -5.94f, -5.37f,
		-4.28f, -3.38f, -3.36f, -3.40f, -2.81f, -2.36f, -1.04f, -0.18f,
		1.08f, 2.15f, 4.19f, 5.51f, 7.33f, 8.64f, 10.46f, 11.89f,
		13.42f, 14.83f, 17.11f, 19.12f, 20.76f, 21.88f, 22.35f, 22.86f,
		22.73f, 22.13f, 21.10f, 20.53f, 18.65f, 17.09f, 15.47f, 14.76f,
		14.02f, 12.94f, 11.98f, 10.29f, 8.11f, 5.83f, 4.23f, 2.42f,
		1.03f, -1.80f, -4.01f, -7.93f, -11.79f, -15.90f, -20.22f, -23.04f,
		-25.66f, -28.80f, -30.45f, -32.11f, -33.17f, -33.97f, -34.68f, -34.26f,
		-34.18f, -33.78f, -32.47f, -31.11f, -29.40f, -27.89f, -27.05f, -26.72f,
		-25.59f, -24.86f, -23.61f, -22.97f, -22.89f, -21.81f, -20.78f, -19.23f,
		-17.74f, -15.09f, -13.18f, -10.51f, -8.18f, -5.89f, -3.35f, -0.02f,
		2.02f, 3.87f, 6.37f, 9.11f, 12.33f, 14.63f, 17.12f, 19.12f,
	};
	memcpy(cogging_table, COGGING_TABLE_FWD, sizeof(COGGING_TABLE_FWD));
	memcpy(cogging_table_rev, COGGING_TABLE_REV, sizeof(COGGING_TABLE_REV));
	cogging_comp_enabled = true;

	// 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 / 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;
}

/**
 * @brief 电机控制类构造函数
 *
 */
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算法设置电机输出
 *
 * @param Uq: q轴电压
 * @param Ud: d轴电压
 *
 * 优化：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) {
	// ① 逆Park变换（sin/cos 为本周期预计算值）
	float Ualpha = Ud * el_cos - Uq * el_sin;
	float Ubeta  = Ud * el_sin + Uq * el_cos;

	// ② 扇区判定（替代 atan2f）
	float X =  Ubeta;
	float Y =  SQRT3DV2 * Ualpha - 0.5f * Ubeta;
	float Z = -SQRT3DV2 * Ualpha - 0.5f * Ubeta;

	u8 sector_code = (X > 0.0f ? 1u : 0u)
	              + (Y > 0.0f ? 2u : 0u)
	              + (Z > 0.0f ? 4u : 0u);

	// 编码 → 扇区 查找表（由 atan2f 扇区定义反推，全角度数值验证通过）
	static const u8 sector_lut[8] = {0, 2, 6, 1, 4, 3, 5, 0};
	u8 sector = sector_lut[sector_code];

	if (sector == 0u) {
		Set_3Pwm(0.5f, 0.5f, 0.5f);
		return;
	}

	// ③ 模长与调制比（arm_sqrt_f32 替代 sqrtf，~14周期 vs ~100周期）
	float magnitude;
	arm_sqrt_f32(Ualpha * Ualpha + Ubeta * Ubeta, &magnitude);
	float inv_Udc = 1.0f / voltage_power_supply;
	float modulation_index = magnitude * inv_Udc;

	// 电压限制
	if (limit_voltage > voltage_power_supply) {
		limit_voltage = voltage_power_supply;
	}
	float limit_percent = limit_voltage / v_safe;

	// ④ T1/T2 计算（替代 atan2f + 第二次 arm_sin_cos_f32）
	//    用预计算系数表直接从 Ualpha/Ubeta 算出
	static const float svpwm_coeff[6][4] = {
		{  SQRT3DV2, -0.5f,    0.0f,    1.0f},  // sector 1
		{  SQRT3DV2,  0.5f, -SQRT3DV2,  0.5f},  // sector 2
		{  0.0f,      1.0f, -SQRT3DV2, -0.5f},  // sector 3
		{ -SQRT3DV2,  0.5f,    0.0f,   -1.0f},  // sector 4
		{ -SQRT3DV2, -0.5f,  SQRT3DV2, -0.5f},  // sector 5
		{  0.0f,     -1.0f,  SQRT3DV2,  0.5f},  // sector 6
	};

	float T1 = (svpwm_coeff[sector-1][0] * Ualpha + svpwm_coeff[sector-1][1] * Ubeta) * inv_Udc;
	float T2 = (svpwm_coeff[sector-1][2] * Ualpha + svpwm_coeff[sector-1][3] * Ubeta) * inv_Udc;

	// 过调制限制：等比缩放 T1/T2
	if (modulation_index > limit_percent) {
		float scale = limit_percent / modulation_index;
		T1 *= scale;
		T2 *= scale;
	}
	float T0 = 1.0f - T1 - T2;

	// ⑤ 占空比计算（不变）
	switch (sector) {
	case 1:
		duty_a = 0.5f * T0 + T1 + T2;
		duty_b = 0.5f * T0 + T2;
		duty_c = 0.5f * T0;
		break;
	case 2:
		duty_a = 0.5f * T0 + T1;
		duty_b = 0.5f * T0 + T1 + T2;
		duty_c = 0.5f * T0;
		break;
	case 3:
		duty_a = 0.5f * T0;
		duty_b = 0.5f * T0 + T1 + T2;
		duty_c = 0.5f * T0 + T2;
		break;
	case 4:
		duty_a = 0.5f * T0;
		duty_b = 0.5f * T0 + T1;
		duty_c = 0.5f * T0 + T1 + T2;
		break;
	case 5:
		duty_a = 0.5f * T0 + T2;
		duty_b = 0.5f * T0;
		duty_c = 0.5f * T0 + T1 + T2;
		break;
	case 6:
		duty_a = 0.5f * T0 + T1 + T2;
		duty_b = 0.5f * T0;
		duty_c = 0.5f * T0 + T1;
		break;
	default:
		duty_a = 0.5f;
		duty_b = 0.5f;
		duty_c = 0.5f;
		break;
	}
	Set_3Pwm(duty_a, duty_b, duty_c);
}

/**
 * @brief 电机关机（通用关机，不带原因标记）
 *
 */
void FOC_Controller::shut_down() {
	drv_stop();
	motor_sleep();
	is_shut_down = true;
}

/**
 * @brief 过热关机
 *
 */
void FOC_Controller::shut_down_thermal() {
	drv_stop();
	motor_sleep();
	is_shut_down = true;
	thermal_shutdown = true;
}

/**
 * @brief 过热恢复
 *
 */
void FOC_Controller::resume_from_thermal() {
	thermal_shutdown = false;
	if (!voltage_error && !over_current) {
		is_shut_down = false;
		motor_resume();
	}
}
/**
 * @brief 占空比置0
 *
 */
void FOC_Controller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
/**
 * @brief 电机锁止
 */
float FOC_Controller::drv_hold() {
	float hold_pwm_duty = HOLD_PWM_DUTY;
	if (HOLD_PWM_DUTY > 0.95f) {
		hold_pwm_duty = 0.95f;
	} else if (HOLD_PWM_DUTY < 0.0f) {
		hold_pwm_duty = 0.0f;
	}
	Set_3Pwm(hold_pwm_duty, 0, 0);
	return hold_pwm_duty;
}


/**
 * @brief PWM模式（调试）
 */
float FOC_Controller::PWM_out() {
	float hold_pwm_duty = target_velocity/100;
	Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
	return hold_pwm_duty;
}


/**
 * @brief 电机恢复
 *
 */
void FOC_Controller::resume() {
	over_current = false;
	voltage_error = false;
	is_shut_down = false;
	motor_resume();
}
/**
 * @brief 更新电源电压
 *
 */
void FOC_Controller::update_power(float voltage) {
	if (voltage < 36.0f and voltage > 8.0f) {
		voltage_error = false;
		voltage_power_supply = voltage;
		v_half = voltage * 0.5f;
		v_safe = voltage * 1.1f;
	} else {
		voltage_error = true;
	}
}

/**
 * @brief 快速电压检测（在电机控制循环中每100us调用一次）
 *
 * 直接从DMA缓冲区读取最新ADC值，检测电压跌幅
 * 响应时间 < 200us
 *
 * 算法：
 * 1. 使用峰值检测算法跟踪当前额定电压（vbus_reference）
 *    - 电压上升时，基准电压跟随
 *    - 电压下跌时，基准电压保持（不跟随下跌）
 * 2. 计算电压跌幅 = 基准电压 - 当前电压
 * 3. 关机条件（满足任一即可）：
 *    - 电压跌幅超过 VBUS_DROP_THRESHOLD
 *    - 电压低于绝对下限 VBUS_ABSOLUTE_MIN
 *
 * 参数定义在 config.h 中:
 * - VBUS_DROP_THRESHOLD: 电压跌幅阈值 (默认5V)
 * - VBUS_ABSOLUTE_MIN: 绝对最低电压 (默认8V)
 * - VBUS_DEBOUNCE_COUNT: 防抖计数 (默认5次)
 */
/**
 * @brief fast_voltage_check 函数
 * @return void
 */
void FOC_Controller::fast_voltage_check(void) {
	// ADC1 连续模式零等待读最新 VBUS（ch6，11:1 分压）。
	// 旧实现 adc1_read_raw 阻塞式"触发+等EOC"：10kHz 调用每次 ~1.5μs 死等。
	// 通道顺序问题的历史注释已随 ADC1 连续模式改造失效（本函数只读 DR）。
	float current_vbus =
		adc1_read_vbus_latest() * (3.3f / 4096.0f) * 11.0f;

	// 更新基准电压（峰值检测）
	// 电压上升时，基准电压跟随；电压下跌时，基准电压保持
	if (current_vbus > vbus_reference) {
		vbus_reference = current_vbus;
	}
	// 电压下跌时不更新基准电压，保持之前的峰值

	// 更新上次电压值
	last_vbus_fast = current_vbus;

	// 计算电压跌幅（相对于基准电压）
	float voltage_drop = vbus_reference - current_vbus;

	// 关机条件（满足任一即可）：
	// 1. 电压跌幅超过阈值
	// 2. 电压低于绝对下限
	bool drop_exceeded = (voltage_drop > VBUS_DROP_THRESHOLD);
	bool below_absolute = (current_vbus < VBUS_ABSOLUTE_MIN);

	if ((drop_exceeded || below_absolute) && (!is_shut_down)) {
		// 电压跌落计数（防抖）
		vbus_drop_count++;

		if (vbus_drop_count >= VBUS_DEBOUNCE_COUNT) {
			// 确认需要关机，立即停机
			drv_stop();
			motor_sleep();
			is_shut_down = true;
			emergency_shutdown = true; // 标记紧急关机，低频任务会执行保存
			vbus_drop_count = 0;	 // 重置计数器
		}
	} else {
		// 电压正常，重置计数器
		vbus_drop_count = 0;
	}
}

/**
 * @brief 速度开环控制
 *
 * @param target_velocity: 目标速度
 * @return float: q轴电压
 */
float FOC_Controller::velocity_open_loop() {
	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在 shaft_angle 中
	shaft_angle =
		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'));

	// 因为在开环控制中，Ud 始终为 0，只控制 Uq
	// 幅度限幅必须保留符号：原 _constrain 下限为 +2V，负速度会被钳到 +2V，
	// 磁场角度反向而 Uq 仍为正，导致反转时电机原地抖动。
	float Uq = target_velocity * REDUCTION_RATIO * 0.005f;
	const float UQ_MIN_MAG = 2.0f;
	const float UQ_MAX_MAG = voltage_power_supply * 0.5f;
	// 零速不输出，避免 2V 最小幅值导致静止时缓慢爬行
	if (Uq != 0.0f) {
		float Uq_mag = fabsf(Uq);
		if (Uq_mag < UQ_MIN_MAG) {
			Uq_mag = UQ_MIN_MAG;
		} else if (Uq_mag > UQ_MAX_MAG) {
			Uq_mag = UQ_MAX_MAG;
		}
		Uq = (Uq < 0.0f) ? -Uq_mag : Uq_mag;
	}
	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
	return limit_voltage;
}

/**
 * @brief 角度闭环控制
 *
 * @param target_angle: 目标角度
 * @return float: 当前角度
 */
float FOC_Controller::angle_control() {
	// 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'));
	// 设置相电压
	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);
	return shaft_angle;
}

/**
 * @brief 速度闭环控制
 *
 * @param target_velocity: 目标速度
 * @return float: 当前速度
 */
float FOC_Controller::velocity_control() {
	// 获取电机轴角度：mode2 Uq 前馈补偿需要用物理 360° 角度查表。
	shaft_angle = KTH7823.getcurrent_sensorangle();

	// mode2 速度环：真实速度波动保留在反馈里，PID 继续看真实 rt_velocity。
	float velocity_error = (target_velocity - rt_velocity) * DIR * REDUCTION_RATIO;

	// mode2 使用独立速度环参数。
	vel_loop.set_PID(vel_loop_p, vel_loop_Ki, vel_loop_Kd);
	vel_loop.set_limit(limit_voltage);

	// 仅"实质性换向"时清积分：目标速度变号且新幅值够大（真实反向指令）才清。
	// 最终贴合阶段误差过零引起的微小变号（|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) {
		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);
	rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
						   sampler.get_current('W'));

	return rt_velocity;
}

/**
 * @brief current_control 函数
 * @return float 返回值
 */
float FOC_Controller::current_control() {
	// 获取电机轴角度
	shaft_angle = KTH7823.getcurrent_sensorangle();
	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'));
	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);
	return 1;
}

/**
 * @brief 带速度限制的角度闭环控制
 *
 * @param target_angle: 目标角度
 * @param max_velocity: 最大速度限制 (单位: rad/s)
 * @return float: 当前角度
 */
float FOC_Controller::angle_control_with_velocity_limit() {
	// 计算角度误差
	float angle_error = target_angle - rt_angle;

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

	// 使用速度PID控制器计算最终输出电压
	// 低速段 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;

	// 仅"实质性换向"时清积分（变号且新幅值>5°/s）。制动段不清（防欠程）；
	// 最终贴合阶段误差过零的微小变号也不清（防积分反复被清、停在差0.01°）
	static float last_vang_target_vel = 0.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, rt_velocity) * DIR;

	// 获取电机轴角度：mode3 Uq 前馈补偿按物理 360° 查表。
	shaft_angle = KTH7823.getcurrent_sensorangle();
	static float mode3_uq_ff_table[MODE3_UQ_FF_TABLE_SIZE] = {0.0f};
	float Uq_ff = update_angle_sync_uq_feedforward(
		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) {
		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'));
	// 设置相电压
	setSVPWM_PhaseVoltage(Uq + get_cogging_compensation(angle_el_cog), 0.0f);

	return shaft_angle;
}

/**
 * @brief MIT控制
 *
 * @param target_angle: 目标角度
 * @param max_velocity: 最大速度限制 (单位: rad/s)
 * @return float: 当前角度
 */
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, rt_angle);

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

	// 仅"实质性换向"时清积分（变号且新幅值>5°/s，理由同 mode3）
	static float last_mit_target_vel = 0.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, 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'));

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

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

	return shaft_angle;
}