#include <Myproject.h>

/**
    @function     Abs_F32
    @brief        取绝对值
    @param[in]    Xn0: [输入/出]
    @return       None
    @date         2025-11-08
*/
uint32_t Abs_F32(int32_t Xn0)
{
    if (Xn0 < 0)
    { return (-Xn0); }
    else
    { return (Xn0); }
}

/**
    @function     FOC_Init
    @brief        硬件FOC初始化
    @date         2025-11-11
*/
void FOC_Init(void)
{
    DRV_CMR = 0x0ABF;
    // 使能FOC
    ClrBit(DRV_CR, DRVEN);
    ClrBit(DRV_CR, FOCEN);
    SetBit(DRV_CR, FOCEN);
    SetBit(FOC_CR0, MERRS1 | MERRS0);
    FOC_EOMEKLPF    = 0xff;   //速度滤波系数,值越小滤波深度越深
    FOC_KFG         = 0;   //FG计算系数 = 6M(TIM4分频结果)/640（FBase）
    // 配置FOC寄存器
    FOC_CR1         = 0;
    FOC_CR2         = 0;
    FOC_IDREF       = 0;
    FOC_IQREF       = 0;
    FOC__THETA      = 0;
    FOC_RTHEACC     = 0;
    FOC__RTHESTEP   = 0;
    FOC_RTHECNT     = 0;
    FOC_THECOR      = 0x02;                                 // 误差角度补偿
    // 电流环参数配置
    FOC_DKP = DQKP;
    FOC_DKI = DQKI;
    FOC_QKP = DQKP;
    FOC_QKI = DQKI;
    FOC_ID_LPFK = 250;
    FOC_IQ_LPFK = 250;
    FOC_DMAX = DOUTMAX;
    FOC_DMIN = DOUTMIN;
    FOC_QMAX = QOUTMAX;
    FOC_QMIN = QOUTMIN;
    SetBit(FOC_CR0, ESCMS);
    // 估算器配置
    #if (EstimateAlgorithm == SMO)
    {
        ClrBit(FOC_CR2, ESEL);
        ClrBit(FOC_CR3, MFP_EN);
    }
    #elif (EstimateAlgorithm == AO)
    {
        SetBit(FOC_CR3, MFP_EN);
        ClrBit(FOC_CR2, ESEL);
    }
    #elif (EstimateAlgorithm == PLL)
    {
        ClrBit(FOC_CR3, MFP_EN);
        SetBit(FOC_CR2, ESEL);
        FOC_KSLIDE    = OBSE_PLLKP_GAIN1;
        FOC_EKLPFMIN  = OBSE_PLLKI_GAIN1;
    }
    #endif
    // 估算器参数初始化
    FOC_EK1         = OBS_K1T;
    FOC_EK2         = OBS_K2T;
    FOC_EK3         = OBS_K3T;
    FOC_EK4         = OBS_K4T;
    FOC_KSLIDE    = OBS_KSLIDE;
    FOC_EKLPFMIN  = OBS_EA_KS;
    FOC_FBASE       = OBS_FBASE;
    FOC_OMEKLPF     = SPEED_KLPF;
    FOC_TGLI        = PWM_TGLI_LOAD;
    SetBit(FOC_CR1, SVPWMEN);
    // 转向配置
    #if (IRMODE)
    SetBit(DRV_CR, DDIR);
    #else
    ClrBit(DRV_CR, DDIR);
    #endif
    // 过调制
    #if (OverModulation)
    SetBit(FOC_CR1, OVMDL);
    #endif
    // 单电阻采样；需要最小采样窗,FOC_TRGDLY为0，七段式SVPWM方式
    #if (Shunt_Resistor_Mode == Single_Resistor)
    {
        SetReg(FOC_CR1, CSM0 | CSM1, 0x00);
        FOC_TSMIN  = PWM_TS_LOAD;                           // 最小采样窗口
        FOC_TRGDLY = 0x3B;                                  // 采样时刻在中点，一般考虑开关噪声影响，会设置延迟；3B
        SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
        FOC_CSO = currOffset.IwBusOffset;
    }
    // 双电阻采样，可设置死区补偿值，在下降沿结束前开始采样Ia，配置81
    #elif (Shunt_Resistor_Mode == Double_Resistor)
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
        FOC_TSMIN = PWM_DT_LOAD;                   // 死区补偿值
        FOC_TRGDLY = 0x05;                         // ADC采样的时刻，采样时刻在计数器零点附近，83为下降沿结束前3个clock采样Ia，与单电阻不同
        // 01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        FOC_TBLO = PWM_DLOWL_TIME;                 //下桥臂最小脉冲，保证采样
        SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
        FOC_CSO  = currOffset.IuOffset;
        SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
        FOC_CSO  = currOffset.IvOffset;
        // 采样配置
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        ClrBit(FOC_CR2, DSS);
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        SetBit(FOC_CR2, DSS);
        #endif
    }
    // 三电阻采样
    #elif (Shunt_Resistor_Mode == Three_Resistor)
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0 | CSM1);
        FOC_TSMIN  = PWM_DT_LOAD; // 死区补偿值
        FOC_TRGDLY = 0x08;        // ADC采样的时刻，采样时刻在计数器零点附近，83为下降沿结束前3个clock采样Ia，与单电阻不同。
        // 01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        FOC_TBLO = PWM_OVERMODULE_TIME;  // 过调制电流采样处理的TB脉宽
        // 采样配置
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        ClrBit(FOC_CR2, DSS);
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        SetBit(FOC_CR2, DSS);
        #endif
        SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
        FOC_CSO = currOffset.IuOffset;
        SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
        FOC_CSO = currOffset.IvOffset;
        SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
        FOC_CSO = currOffset.IwBusOffset;
    }
    #endif
    // SVPWM 配置
    #if (SVPMW_Mode == SVPWM_7_Segment)
    ClrBit(FOC_CR2, F5SEG);
    #elif (SVPMW_Mode == SVPWM_5_Segment)
    SetBit(FOC_CR2, F5SEG);
    #endif
    SetBit(DRV_CR, DRVEN);
    SetBit(DRV_CR, OCS);
}

/**
    @function     Motor_Open
    @brief        启动
    @date         2025-11-08
*/
void Motor_Open(void)
{
    static uint8 OpenRampCycles;
    // FOC_Init();
    // 启动角度
    // FOC__THETA = _Q15((float)0.0 / 180.0);
    #if ((EstimateAlgorithm == SMO)||(EstimateAlgorithm == AO))
    FOC__ETHETA = FOC__THETA - 4836;    //SMO估算角度延迟
    #elif (EstimateAlgorithm == PLL)
    FOC__ETHETA = FOC__THETA;
    #endif
    FOC__EOME = 0;
    // 配置参数
    FOC_IDREF = 0;
    FOC_DKP = DQKPStart;
    FOC_DKI = DQKIStart;
    FOC_QKP = DQKPStart;
    FOC_QKI = DQKIStart;
    FOC_EKP = OBSW_KP_GAIN;
    FOC_EKI = OBSW_KI_GAIN;
    /// 启动方式选择
    #if (Open_Start_Mode == Omega_Start)
    {
        FOC_EFREQACC  = MOTOR_OMEGA_RAMP_ACC;
        FOC_EFREQMIN  = MOTOR_OMEGA_ACC_MIN;
        FOC_EFREQHOLD = MOTOR_OMEGA_ACC_END;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | ANGM);
        #if  (IFFDebugg)
        {
            // 估算器禁止输出
            ClrBit(FOC_CR1, EFAE);                      // 禁止估算器强制输出
            ClrBit(FOC_CR1, RFAE);                      // 使能强拉
            ClrBit(FOC_CR1, ANGM);                      // 禁止估算器输出
        }
        #endif
        // 切入启动
        mcFocCtrl.State_Count = 1200;
        mcState = mcRun;
    }
    #elif (Open_Start_Mode == Open_Start)
    {
        FOC_RTHEACC = MOTOR_OPEN_ACC;
        FOC__RTHESTEP = MOTOR_OPEN_ACC_MIN;
        FOC_RTHECNT = MOTOR_OPEN_ACC_CNT;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, RFAE);
    
        // 切入启动
        if (OpenRampCycles < (MOTOR_OPEN_ACC_CYCLE - 1))
        {
            if (!ReadBit(FOC_CR1, RFAE))
            {
                SetBit(FOC_CR1, RFAE);
                OpenRampCycles++;
            }
        }
        else
        {
            mcFocCtrl.State_Count = 2;
            mcState = mcRun;
        }
    
        FOC_EKP = OBSW_KP_GAIN_RUN4;
        FOC_EKI = OBSW_KI_GAIN_RUN4;
    }
    #elif (Open_Start_Mode == Open_Omega_Start)
    {
        FOC_RTHEACC   = MOTOR_OPEN_ACC;
        FOC__RTHESTEP = MOTOR_OPEN_ACC_MIN;
        FOC_RTHECNT   = MOTOR_OPEN_ACC_CNT;
        FOC_EFREQACC  = Motor_OMEGA_RAMP_ACC;
        FOC_EFREQMIN  = MOTOR_OMEGA_ACC_MIN;
        FOC_EFREQHOLD = MOTOR_OMEGA_ACC_END;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | RFAE | ANGM);
        // 切入启动
        mcFocCtrl.State_Count = 2600;
        mcState = mcRun;
    }
    #endif
    // Q轴启动电流
    FOC_IQREF = IQ_START_CURRENT ;
}



/**
    @function     StarRampDealwith
    @brief        估算器增益调整
    @date         2025-11-08
*/
void StarRampDealwith(void)
{
    if ((mcState == mcRun) || (mcState == mcStart))
    {
        if (mcFocCtrl.State_Count == 1200)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN;              // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN;              // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN2;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN2;
            }
            #endif
        }
        else if (mcFocCtrl.State_Count == 1000)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN1;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN1;             // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN3;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN3;
            }
            #endif
        }
        else if (mcFocCtrl.State_Count == 600)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN2;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN2;             // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN4;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN4;
            }
            #endif
        }
        else if (mcFocCtrl.State_Count == 400)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN3;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN3;             // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN5;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN5;
            }
            #endif
        }
        else if (mcFocCtrl.State_Count == 300)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN4;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN4;             // 估算器里的PI的KI
            #if (EstimateAlgorithm == PLL)
            {
                FOC_KSLIDE    = OBSE_PLLKP_GAIN5;
                FOC_EKLPFMIN  = OBSE_PLLKI_GAIN5;
            }
            #endif
        }
    }
}

#if (FiledWeakenCompEnable)

FieldWeakeningTypeDef xdata mcFieldWeaken;

void  FiledWeakenControl(int16 Ud, int16 Uq, uint16 Dcbusk, int16 Is)
{
    //获取Us和Udc
    SCAT1_COS =  Ud;
    SCAT1_SIN =  Uq;
    SMDU_RunBlock(1, 5);
    mcFieldWeaken.FieldWeakenActualUS_Q15 = SCAT1_RES1;
    mcFieldWeaken.FieldWeakenActualDcbus_Q15 = Dcbusk;
    // 计算弱磁角度
    mcFieldWeaken.FieldWeakenIsTheta = HW_One_Calc(mcFieldWeaken.FieldWeakenActualDcbus_Q15 -  mcFieldWeaken.FieldWeakenActualUS_Q15);
    //获取D轴电流和Q轴电流参考值。
    SCAT1_COS = Is;
    SCAT1_THE = mcFieldWeaken.FieldWeakenIsTheta;
    SCAT1_SIN = 0;
    SMDU_RunBlock(1, 4);
    mcFieldWeaken.mcIdref = SCAT1_RES2 ;//sin的结果
    mcFieldWeaken.mcIqref = SCAT1_RES1;//cos的结果
}
#endif