Motor Foc Control

三相静止坐标系 (ABC)
        ↓ Clarke 变换
两相静止坐标系 (α, β)
        ↓ Park 变换  
两相旋转坐标系 (d, q)

关键公式：
Clarke:  Iα = Ia
         Iβ = (Ia + 2Ib)/√3

Park:    Id = Iα·cos(θ) + Iβ·sin(θ)
         Iq = -Iα·sin(θ) + Iβ·cos(θ)

逆变换：
Iα = Id·cos(θ) - Iq·sin(θ)
Iβ = Id·sin(θ) + Iq·cos(θ)

三相重构：
Ia = Iα
Ib = -Iα/2 + Iβ·√3/2
Ic = -Iα/2 - Iβ·√3/2

        ┌──────────────────────────────────────────┐
        │                                          │
Iq_ref ──→ ┌────┐    ┌────┐    ┌────┐    ┌─────┐ │
           │ PI │───→│ SVPWM │──→│ 逆变器 │──→│ 电机 │ │
           └────┘    └────┘    └─────┘    │     │ │
           ↑ PI      ↑            ↑             │     │
Id_ref ───→┌────┐    │            │             ↓     ↓
           │ PI │────┘            │        ┌────────┐ │
           └────┘                 │        │ 三相输出 │ │
           ↑                      │        └────────┘ │
        0 ───→┌────┐             │             ↑       │
           ┌─→│ −ωLqIq │←───────┘             │       │
           │  └────┘                          │       │
      ┌────┐│                                 │       │
 θe ←│ PLL│←─────────────────────────────────┘       │
      └────┘                                          │
      (磁链观测器)                                      │
                                                     │
     ←←←←←← 转速环 ←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←←

// 8个基本空间矢量（6个非零 + 2个零矢量）
// U0(000): 000, U1(100): 100, U2(110): 110, ...
// U3(010): 010, U4(011): 011, U5(001): 001
// U6(101): 101, U7(111): 111

typedef struct {
    float alpha;
    float beta;
} SpaceVector;

// 合成任意方向电压矢量
SpaceVector sv_ref = {
    .alpha = Uref * cos(theta_e),
    .beta  = Uref * sin(theta_e)
};

// 判断参考电压矢量所在扇区
int sv_sector(SpaceVector *sv) {
    float a = sv->beta;
    float b = sv->alpha * 0.866 - sv->beta * 0.5;
    float c = -sv->alpha * 0.866 - sv->beta * 0.5;

    int N = 0;
    if (a > 0) N |= 1;
    if (b > 0) N |= 2;
    if (c > 0) N |= 4;

    // N → 扇区号 (1~6)
    const int sector_table[8] = {0, 2, 6, 1, 4, 3, 5, 0};
    return sector_table[N];
}

// 在扇区 k 中，计算相邻矢量作用时间
// T0 = (T - Ta - Tb) / 2
// Ta, Tb 根据扇区查表

typedef struct {
    float Ualpha;
    float Ubeta;
    float T;        // PWM周期
    float Udc;      // 母线电压
} SVPWM_Handle;

void svpwm_calc(SVPWM_Handle *h, float *Ta, float *Tb) {
    float X = h->Ubeta;
    float Y = h->Ualpha * 0.866 + h->Ubeta * 0.5;
    float Z = -h->Ualpha * 0.866 + h->Ubeta * 0.5;

    int sector = sv_sector((SpaceVector*)h);

    switch(sector) {
        case 1: *Ta = Z; *Tb = Y; break;
        case 2: *Ta = Y; *Tb = -X; break;
        case 3: *Ta = -Z; *Tb = X; break;
        case 4: *Ta = -X; *Tb = Z; break;
        case 5: *Ta = X; *Tb = -Y; break;
        case 6: *Ta = -Y; *Tb = -Z; break;
    }

    // 过调制处理
    float T_sum = *Ta + *Tb;
    if (T_sum > h->T) {
        *Ta = *Ta * h->T / T_sum;
        *Tb = *Tb * h->T / T_sum;
    }

    // 零矢量分配（7段式对称PWM）
    float T0 = (h->T - *Ta - *Tb) / 2;
    float T7 = T0;

    // 计算三相占空比
    // DPA, DPB, DPC 存入比较寄存器
}

// 各扇区占空比计算
// Tcm1, Tcm2, Tcm3 → 对应 ABC 三相比较值

void svpwm_duty(SVPWM_Handle *h, float *Tcm) {
    float Ta, Tb;
    svpwm_calc(h, &Ta, &Tb);
    float T0 = (h->T - Ta - Tb) / 2;

    int sector = sv_sector((SpaceVector*)h);

    switch(sector) {
        case 1:  // U4(100), U6(110), U0(000)
            Tcm[0] = (h->T + Ta + Tb) / 2;  // A: 先断后通
            Tcm[1] = (h->T - Ta + Tb) / 2;  // B
            Tcm[2] = (h->T - Ta - Tb) / 2;  // C
            break;
        case 2:  // U6(110), U2(010), U0(000)
            Tcm[0] = (h->T - Ta + Tb) / 2;
            Tcm[1] = (h->T + Ta + Tb) / 2;
            Tcm[2] = (h->T - Ta - Tb) / 2;
            break;
        case 3:  // U2(010), U3(011), U0(000)
            Tcm[0] = (h->T - Ta - Tb) / 2;
            Tcm[1] = (h->T + Ta + Tb) / 2;
            Tcm[2] = (h->T + Ta - Tb) / 2;
            break;
        case 4:  // U3(011), U1(001), U0(000)
            Tcm[0] = (h->T - Ta - Tb) / 2;
            Tcm[1] = (h->T - Ta + Tb) / 2;
            Tcm[2] = (h->T + Ta + Tb) / 2;
            break;
        case 5:  // U1(001), U5(101), U0(000)
            Tcm[0] = (h->T + Ta - Tb) / 2;
            Tcm[1] = (h->T - Ta - Tb) / 2;
            Tcm[2] = (h->T + Ta + Tb) / 2;
            break;
        case 6:  // U5(101), U4(100), U0(000)
            Tcm[0] = (h->T + Ta + Tb) / 2;
            Tcm[1] = (h->T - Ta - Tb) / 2;
            Tcm[2] = (h->T + Ta - Tb) / 2;
            break;
    }
}

电磁转矩方程：
T = 1.5 × np × [Ψm × Iq + (Ld - Lq) × Id × Iq]

定义电流幅值：
|I| = √(Id² + Iq²)

MTPA 轨迹：固定 |I|，寻找使 T 最大的 (Id, Iq) 组合

令 ∂T/∂Id = 0，约束条件 |I| = constant

拉格朗日函数：L = T - λ(√(Id²+Iq²) - I_ref)

∂L/∂Id = 1.5×np×[(Ld-Lq)×Iq] - λ×Id/|I| = 0
∂L/∂Iq = 1.5×np×[Ψm + (Ld-Lq)×Id] - λ×Iq/|I| = 0

联立求解（化简后）：

Id_MTPA = -(Ψm / (2×ΔL)) + √[(Ψm/(2ΔL))² + Iq²]

其中 ΔL = Lq - Ld > 0

当 ΔL 很小时（表贴式），Id_MTPA ≈ 0 → 退化为 Id=0 控制

// MTPA 查表法（实时性最好）
// 预计算 MTPA 曲线，运行时查表 + 线性插值

static const float mtpa_table_Iq[] = {
    0.0, 2.0, 4.0, 6.0, 8.0, 10.0, 12.0, 15.0, 18.0, 20.0
};
static const float mtpa_table_Id[] = {
    0.0, -0.5, -1.1, -1.8, -2.5, -3.2, -4.0, -5.3, -6.6, -7.8
};

float mtpa_interpolate(float Iq) {
    int n = sizeof(mtpa_table_Iq) / sizeof(mtpa_table_Iq[0]);
    // 边界
    if (Iq <= mtpa_table_Iq[0]) return mtpa_table_Id[0];
    if (Iq >= mtpa_table_Iq[n-1]) return mtpa_table_Id[n-1];
    // 查表
    for (int i = 0; i < n-1; i++) {
        if (Iq >= mtpa_table_Iq[i] && Iq < mtpa_table_Iq[i+1]) {
            float t = (Iq - mtpa_table_Iq[i]) /
                       (mtpa_table_Iq[i+1] - mtpa_table_Iq[i]);
            return mtpa_table_Id[i] + t * (mtpa_table_Id[i+1] - mtpa_table_Id[i]);
        }
    }
    return 0;
}

// MTPA 解析公式法（适合在线计算）
float mtpa_Id_formula(float Iq, float Psi_m, float Ld, float Lq) {
    float delta_L = Lq - Ld;
    if (delta_L < 1e-6) return 0;  // 表贴式，无 MTPA 效益

    float k = Psi_m / (2.0f * delta_L);
    float Id = -k + sqrtf(k * k + Iq * Iq);
    return -Id;  // Id 必须为负（去磁）
}

电压极限椭圆（d-q 平面）：
Vd² + Vq² ≤ Vdc² / 2   （SVPWM线性调制区最大输出）

反电动势约束：
V = ω_e × (Ψm - Ld×Id)  ≈ ω_e × Ψm_eff

弱磁的本质：在电压极限椭圆内重新分配 Id、Iq

typedef enum {
    MTPA_MODE,         // 最大转矩电流比
    FW_MODE,            // 弱磁模式
    SIXSTEP_MODE        // 六步方波
} FluxMode;

typedef struct {
    float Vdc;          // 母线电压
    float V_lim;        // 电压极限（Vdc/sqrt(2)）
    float V_th;         // 弱磁启动阈值（建议 0.9×V_lim）
    float Id_fw_min;    // 最大去磁电流（负值）
    float gamma;         // 弱磁PI积分系数
    float Kp_fw;        // 弱磁比例增益
    float Ki_fw;        // 弱磁积分增益
} FluxWeaken_Handle;

void flux_weaken_update(FluxWeaken_Handle *h,
                        float V_mag, float Id_ref,
                        float *Id_fw_out) {
    // V_mag = sqrt(Vd² + Vq²)
    // Id_ref = MTPA 给定的 Id

    static float integral = 0;
    float error = V_mag - h->V_lim * 0.95;  // 提前 5% 介入

    if (error > 0) {
        // 电压饱和，启动弱磁
        integral += h->gamma * error;
        integral = fminf(integral, 0);  // Id_fw 为负，积分限幅
        *Id_fw_out = h->Id_fw_min;       // 直接给最大去磁
    } else {
        // 电压裕量足够，逐渐减小弱磁
        integral *= 0.95;  // 缓慢衰减
        *Id_fw_out = Id_ref + integral;
    }

    // 与 MTPA 叠加
    *Id_fw_out = fmaxf(*Id_fw_out, h->Id_fw_min);
}

float select_Id_ref(float Id_MTPA, float Id_FW, float V_mag,
                    float V_lim, float speed) {
    float Id_ref;

    if (V_mag < V_lim * 0.85) {
        // 电压充裕，MTPA 控制
        Id_ref = Id_MTPA;
    } else if (V_mag < V_lim * 0.95) {
        // 电压接近饱和，过渡区
        float alpha = (V_mag - V_lim * 0.85) / (V_lim * 0.1);
        Id_ref = Id_MTPA + alpha * (Id_FW - Id_MTPA);
    } else {
        // 深度弱磁
        Id_ref = Id_FW;
    }

    return Id_ref;
}

typedef struct {
    float I_alpha;
    float I_beta;
    float Ia;
    float Ib;
    float Ic;
    float R;
    float L;
    float emf_alpha;
    float emf_beta;
    float angle;
    float speed;
    float prev_angle;
    float k_gain;       // 滑模增益
    float filter_bw;     // 低通截止频率 rad/s
} SMO_Handle;

// sign 函数（连续近似，避免抖振）
float sign_f(float x, float delta) {
    if (x > delta)  return 1.0f;
    if (x < -delta) return -1.0f;
    return x / delta;  // |x|<delta: 线性近似
}

#define K_SMO 0.5f    // 滑模增益（过大=抖振，过小=延迟）

void smo_update(SMO_Handle *smo, float V_alpha, float V_beta,
               float Ia, float Ib, float Ic, float dt) {

    float e_alpha = smo->I_alpha - Ia;
    float e_beta  = smo->I_beta  - Ib;

    // 滑模控制量
    float z_alpha = smo->k_gain * sign_f(e_alpha, 0.05f);
    float z_beta  = smo->k_gain * sign_f(e_beta,  0.05f);

    // 电流更新（欧拉法）
    smo->I_alpha += dt * (-smo->R * Ia + V_alpha - z_alpha) / smo->L;
    smo->I_beta  += dt * (-smo->R * Ib + V_beta  - z_beta ) / smo->L;

    // 反电动势提取（低通滤波）
    float alpha_lpf = 2.0f * M_PI * smo->filter_bw;
    smo->emf_alpha += dt * alpha_lpf * (z_alpha - smo->emf_alpha);
    smo->emf_beta  += dt * alpha_lpf * (z_beta  - smo->emf_beta);

    // 角度提取
    smo->angle = atan2f(smo->emf_alpha, -smo->emf_beta);

    // 速度计算（微分）
    float d_angle = smo->angle - smo->prev_angle;
    // 角度归一化（处理 -π 到 +π 跳变）
    if (d_angle > M_PI)  d_angle -= 2.0f * M_PI;
    if (d_angle < -M_PI) d_angle += 2.0f * M_PI;
    smo->speed = d_angle / dt;
    smo->prev_angle = smo->angle;
}

// 对 SMO 估算的反电动势做 PLL
// emf_d = -ω_e × Ψm × sin(Δθ) ≈ -ω_e × Ψm × Δθ（小角度时）
// emf_q =  ω_e × Ψm × cos(Δθ) ≈  ω_e × Ψm

typedef struct {
    float Kp;
    float Ki;
    float integral;
    float angle;
    float speed;
    float lock_threshold;  // 锁定阈值 rad/s
    int   locked;         // 锁定标志
} PLL_Handle;

void pll_update(PLL_Handle *pll, float emf_d, float emf_q, float dt) {
    // d轴反电动势应接近0（正确对齐时）
    // q轴反电动势 = ω_e × Ψm

    float error = emf_d;  // PLL 误差

    // PI 积分
    pll->integral += pll->Ki * error * dt;
    // 积分限幅
    pll->integral = fmaxf(fminf(pll->integral, 1000.0f), -1000.0f);

    pll->speed = pll->Kp * error + pll->integral;
    pll->angle += pll->speed * dt;

    // 角度归一化 0~2π
    while (pll->angle < 0)      pll->angle += 2.0f * M_PI;
    while (pll->angle > 2.0f*M_PI) pll->angle -= 2.0f * M_PI;

    // 锁定判断
    if (fabsf(error) < pll->lock_threshold && fabsf(pll->speed) > 10.0f) {
        pll->locked = 1;
    }
}

# 自动计算 PI 参数
python scripts/foc_pi_tuner.py --R 1.23 --L 5.6e-3 --J 1e-3 --Kt 0.15

# 交互模式（引导输入）
python scripts/foc_pi_tuner.py --mode interactive

# 指定目标带宽
python scripts/foc_pi_tuner.py --R 2.1 --L 8e-3 --target_bw 150

# 指定采样周期 + 生成Bode图
python scripts/foc_pi_tuner.py --R 1.23 --L 5.6e-3 --Ts_us 62.5 --plot

目标：带宽 ωbw = R/L × 0.5（典型因子 0.3~0.6）

Kp = L × ωbw
Ki = R / L

示例：
R = 1.23Ω, L = 5.6mH, ωbw = 1.23/0.0056 × 0.5 ≈ 110 rad/s
Kp = 0.0056 × 110 ≈ 0.616
Ki = 1.23/0.0056 ≈ 220 (需根据 Ts 离散化修正)

目标：带宽 ≈ 电流环带宽 / 10

Kp_speed ≈ J × ωbw_speed
Ki_speed ≈ Kp_speed / Tm

示例：
J = 0.001 kg·m²
ωbw_speed = 11 rad/s (电流环1/10)
Kp_speed = 0.001 × 11 = 0.011
Tm = J × R / Kt² = 0.001 × 1.23 / 0.15² ≈ 0.055 s
Ki_speed = 0.011 / 0.055 ≈ 0.2

给定采样周期 Ts：
Kp_d = Kp
Ki_d = Ki × Ts / 2

示例：Ts = 62.5μs (16kHz)
Ki_d = 220 × 31.25e-6 = 0.006875

实现（位置式）：
  integral += Ki_d × error
  output   = Kp_d × error + integral
  限幅：output ∈ [-Vmax, Vmax]

启动抖动？
├─ 初始角度问题 → 重新做初始角辨识（脉冲法/预定位）
├─ Hall错位 → 校正Hall安装位置
└─ 电流环带宽不足 → 提高电流环 Kp

高速失步？
├─ 弱磁不足 → 增强 Id_fw，验证 V_mag > 0.95×V_lim
├─ 母线电压不足 → 提高 Vdc 或加升压
└─ 反电动势估算错误 → 检查速度观测器

振动噪音？
├─ 开关频率共振 → 提高PWM频率或加RC吸收
├─ 机械共振 → 加陷波滤波器
└─ 电流纹波大 → 提高开关频率或加电感

# 完整参数计算（自动输出离散PI参数）
python scripts/foc_pi_tuner.py --R 1.23 --L 5.6e-3 --poles 8 --J 1e-3 --Kt 0.15 --Ts_us 62.5

# 绘制 Bode 图（验证稳定性）
python scripts/foc_pi_tuner.py --R 1.23 --L 5.6e-3 --plot

# 对比不同带宽参数
python scripts/foc_pi_tuner.py --R 2.1 --L 8e-3 --target_bw 80
python scripts/foc_pi_tuner.py --R 2.1 --L 8e-3 --target_bw 200