/**********************************
 * 文件名称: foc_algorithm.c
 * 功能描述: 磁场定向控制(FOC)算法实现
 * 主要功能:
 * 1. Clarke变换（三相到两相静止坐标系）
 * 2. Park变换（两相静止到两相旋转坐标系）
 * 3. 反Park变换（两相旋转到两相静止坐标系）
 * 4. SVPWM（空间矢量脉宽调制）计算
 * 5. 电流PID控制
 * 6. 电压限制
 * 7. EKF（扩展卡尔曼滤波器）状态估计
 * 8. 电感和电阻/磁链参数识别
 **********************************/
#include "main.h"
#include "foc_algorithm.h"

// 电流PID控制器参数
// 注：Vq和Vd是控制器输出，用于控制电机的q轴和d轴电压
// Iq和Id是反馈电流，分别控制电机的转矩和磁场
// 控制器参数通过自动调优获得
real32_T D_PI_I = 200.F;          // D轴积分系数
real32_T D_PI_KB = 1.0F;          // D轴反馈系数
real32_T D_PI_ISUM_MAX = 12.0f;   // D轴最大积分限幅
real32_T D_PI_ISUM_MIN = -12.0f;  // D轴最小积分限幅
real32_T D_PI_LOW_LIMIT = -24.0F; // D轴输出下限
real32_T D_PI_P = 1.0F;           // D轴比例系数
real32_T D_PI_UP_LIMIT = 24.0F;   // D轴输出上限
real32_T Q_PI_I = 200.F;          // Q轴积分系数
real32_T Q_PI_ISUM_MAX = 12.0f;   // Q轴最大积分限幅
real32_T Q_PI_ISUM_MIN = -12.0f;  // Q轴最小积分限幅
real32_T Q_PI_KB = 1.0F;          // Q轴反馈系数
real32_T Q_PI_LOW_LIMIT = -24.0F; // Q轴输出下限
real32_T Q_PI_P = 1.0F;           // Q轴比例系数
real32_T Q_PI_UP_LIMIT = 24.0F;   // Q轴输出上限


/***************************************
 * Clarke变换
 * 功能：将三相电流转换为alpha-beta坐标系电流
 * 描述：将120度三相坐标系转换为90度两相静止坐标系
 ***************************************/
void Clarke_Transf(CURRENT_ABC_DEF Current_abc_temp,CURRENT_ALPHA_BETA_DEF* Current_alpha_beta_temp)
{
  Current_alpha_beta_temp->Ialpha = (Current_abc_temp.Ia - (Current_abc_temp.Ib + Current_abc_temp.Ic) * 0.5F) * 2.0F / 3.0F;
  Current_alpha_beta_temp->Ibeta = (Current_abc_temp.Ib - Current_abc_temp.Ic) * 0.866025388F * 2.0F / 3.0F;
}

/***************************************
 * SVPWM计算
 * 功能：将alpha-beta电压转换为PWM占空比
 * 描述：根据alpha-beta电压向量计算SVPWM占空比
 ***************************************/
void SVPWM_Calc(VOLTAGE_ALPHA_BETA_DEF v_alpha_beta_temp,real32_T Udc_temp,real32_T Tpwm_temp)
{
  int32_T sector;
  real32_T Tcmp1,Tcmp2,Tcmp3,Tx,Ty,f_temp,Ta,Tb,Tc;
  sector = 0;
  Tcmp1 = 0.0F;
  Tcmp2 = 0.0F;
  Tcmp3 = 0.0F;
  
  // 确定扇区，根据Vbeta和Valpha的值计算
  if (v_alpha_beta_temp.Vbeta > 0.0F) {
    sector = 1;
  }
  
  if ((1.73205078F * v_alpha_beta_temp.Valpha - v_alpha_beta_temp.Vbeta) / 2.0F > 0.0F) {
    sector += 2;
  }
  
  if ((-1.73205078F * v_alpha_beta_temp.Valpha - v_alpha_beta_temp.Vbeta) / 2.0F > 0.0F) {
    sector += 4;
  }
  
  // 根据扇区计算Tx和Ty
  switch (sector) {
  case 1:
    Tx = (-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    Ty = (1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    break;
    
  case 2:
    Tx = (1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    Ty = -(1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp);
    break;
    
  case 3:
    Tx = -((-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    Ty = 1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp;
    break;
    
  case 4:
    Tx = -(1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp);
    Ty = (-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp);
    break;
    
  case 5:
    Tx = 1.73205078F * v_alpha_beta_temp.Vbeta * Tpwm_temp / Udc_temp;
    Ty = -((1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    break;
    
  default:
    Tx = -((1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    Ty = -((-1.5F * v_alpha_beta_temp.Valpha + 0.866025388F * v_alpha_beta_temp.Vbeta) * (Tpwm_temp / Udc_temp));
    break;
  }
  
  // 归一化处理
  f_temp = Tx + Ty;
  if (f_temp > Tpwm_temp) {
    Tx /= f_temp;
    Ty /= (Tx + Ty);
  }
  
  // 计算各相占空比
  Ta = (Tpwm_temp - (Tx + Ty)) / 4.0F;
  Tb = Tx / 2.0F + Ta;
  Tc = Ty / 2.0F + Tb;
  
  // 根据扇区分配占空比
  switch (sector) {
  case 1:
    Tcmp1 = Tb;
    Tcmp2 = Ta;
    Tcmp3 = Tc;
    break;
    
  case 2:
    Tcmp1 = Ta;
    Tcmp2 = Tc;
    Tcmp3 = Tb;
    break;
    
  case 3:
    Tcmp1 = Ta;
    Tcmp2 = Tb;
    Tcmp3 = Tc;
    break;
    
  case 4:
    Tcmp1 = Tc;
    Tcmp2 = Tb;
    Tcmp3 = Ta;
    break;
    
  case 5:
    Tcmp1 = Tc;
    Tcmp2 = Ta;
    Tcmp3 = Tb;
    break;
    
  case 6:
    Tcmp1 = Tb;
    Tcmp2 = Tc;
    Tcmp3 = Ta;
    break;
  }
  
  // 更新FOC输出
  FOC_Output.Tcmp1 = Tcmp1;
  FOC_Output.Tcmp2 = Tcmp2;
  FOC_Output.Tcmp3 = Tcmp3;
}

/***************************************
 * 角度转余弦正弦值
 * 功能：将角度转换为余弦和正弦值
 * 描述：使用ARM DSP库计算角度的余弦和正弦值
 ***************************************/
void Angle_To_Cos_Sin(real32_T angle_temp,TRANSF_COS_SIN_DEF* cos_sin_temp)
{
  cos_sin_temp->Cos = arm_cos_f32(angle_temp);
  cos_sin_temp->Sin = arm_sin_f32(angle_temp);
}

/***************************************
 * Park变换
 * 功能：将alpha-beta坐标系电流转换为DQ坐标系电流
 * 描述：使用余弦和正弦值将两相静止坐标系转换为两相旋转坐标系
 ***************************************/
void Park_Transf(CURRENT_ALPHA_BETA_DEF current_alpha_beta_temp,TRANSF_COS_SIN_DEF cos_sin_temp,CURRENT_DQ_DEF* current_dq_temp)
{
  current_dq_temp->Id = current_alpha_beta_temp.Ialpha * cos_sin_temp.Cos + current_alpha_beta_temp.Ibeta * cos_sin_temp.Sin;
  current_dq_temp->Iq = -current_alpha_beta_temp.Ialpha * cos_sin_temp.Sin + current_alpha_beta_temp.Ibeta * cos_sin_temp.Cos;
}

/***************************************
 * 反Park变换
 * 功能：将DQ坐标系电压转换为alpha-beta坐标系电压
 * 描述：使用余弦和正弦值将两相旋转坐标系转换为两相静止坐标系
 ***************************************/
void Rev_Park_Transf(VOLTAGE_DQ_DEF v_dq_temp,TRANSF_COS_SIN_DEF cos_sin_temp,VOLTAGE_ALPHA_BETA_DEF* v_alpha_beta_temp)
{
  v_alpha_beta_temp->Valpha = cos_sin_temp.Cos * v_dq_temp.Vd - cos_sin_temp.Sin * v_dq_temp.Vq;
  v_alpha_beta_temp->Vbeta  = cos_sin_temp.Sin * v_dq_temp.Vd + cos_sin_temp.Cos * v_dq_temp.Vq;
}

/***************************************
 * 电流PID控制器
 * 功能：计算电流PID控制器输出
 * 描述：根据参考值和反馈值计算PID控制器输出
 ***************************************/
void Current_PID_Calc(real32_T ref_temp, real32_T fdb_temp, 
                       real32_T* out_temp, CURRENT_PID_DEF* pid)
{

    real32_T err  = ref_temp - fdb_temp;
    
    real32_T p_out = pid->P_Gain * err;
    
    real32_T i_out = pid->I_Gain * pid->I_Sum;
    
    real32_T pre_out = p_out + i_out;
    
    real32_T output;
    if (pre_out > pid->Max_Output) {
        output = pid->Max_Output;
    } else if (pre_out < pid->Min_Output) {
        output = pid->Min_Output;
    } else {
        output = pre_out;
    }
    
    if ((pre_out <= pid->Max_Output && pre_out >= pid->Min_Output) ||
        (err* (output - pre_out) <= 0)) {
        pid->I_Sum += err * FOC_PERIOD;
    }
    
//    if (pid->I_Sum > pid->Max_Integral) pid->I_Sum = pid->Max_Integral;
//    if (pid->I_Sum < pid->Min_Integral) pid->I_Sum = pid->Min_Integral;
     // vd = R*id + Ld*(did/dt) - ωe*Lq*iq      // 电压方程
    // vq = R*iq + Lq*(diq/dt) + ωe*Ld*id + ωe*λm
    *out_temp = output;
}

/***************************************
 * 电压限制
 * 功能：限制电压向量幅值
 * 描述：确保电压向量不超过SVPWM的最大幅值
 ***************************************/
void voltage_limit(float *vd, float *vq, float vdc)
{
    float vmax = vdc * 0.57735f;  // 计算SVPWM的最大电压幅值
    float v_sq = (*vd)*(*vd) + (*vq)*(*vq);
    
    if (v_sq > vmax*vmax) {
        float scale = vmax / sqrtf(v_sq);
        *vd *= scale;
        *vq *= scale;
    }
}