/********************************************************************************

 **** Copyright (C), 2020, Fortior Technology Co., Ltd.                      ****

 ********************************************************************************
    File Name     : AddFunction.c
    Author        : Bruce HW&RD
    Date          : 2020-10-17
    Description   : .C file function description
    Version       : 1.0
    Function List :

    Record        :
    1.Date        : 2020-10-17
     Author      : Bruce HW&RD
     Modification: Created file

********************************************************************************/

#include "FU68xx_5.h"
#include <Myproject.h>

extern int16 TD_output1;
extern uint16 SKI1, SKP1;
/* Private variables ---------------------------------------------------------*/
PWMINPUTCAL        xdata   mcPwmInput;
CurrentVarible     xdata   mcCurVarible;
FOCCTRL            xdata   mcFocCtrl;
ILIMIT             xdata   mcIimit;
ADCSample          xdata   AdcSampleValue;
MCLedDisplay       xdata   mcLedDisplay;
MCRAMP             xdata   mcSpeedRamp;
PFCMCRAMP          xdata   PFCCONTROL;
TIMERTypeDef       xdata   Time;
CONTROLCMDD        xdata   ConTrolCmd;
uint8              xdata   dog_Status = 0;
uint8              xdata   flag_1ms_main = 0;
int16     Huan_temp = 0, Guan_temp = 0, Paiqi_temp = 0;
int16 qiangtujiaodu = 0;
int16 s2_cvcf_i_set         = IF_I_Value;

#if (Debugg==1)
    extern int16  xdata GPFCON1, GPFCON2, GPFCON3, GPFCON4, GPFCON5, GPFCON6, GPFCON7, GPFCON8, GPFCON9, GPFCON10;
#endif
/*---------------------------------------------------------------------------*/
/*  Name     :   void FaultProcess(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 保护处理函数，关闭FOC输出，同时将状态变为mcFault
    /*---------------------------------------------------------------------------*/
void FaultProcess(void)
{
    ClrBit(DRV_CR, FOCEN);  //FOC Disable
    MOE     = 0;
}

int16 LPF_realize(int16 Xn1, int16 Xn0, int16 K)
{
    LPF0_K = K;
    LPF0_X = Xn1;
    LPF0_YH = Xn0;
    SMDU_RunBlock(0, 6);    //调用MDU运算单元0中的LPF模式（6）
    return LPF0_YH;
}

int16 LPF_realize1(int16 Xn1, int16 Xn0, int16 K)
{
    LPF1_K = K;
    LPF1_X = Xn1;
    LPF1_YH = Xn0;
    SMDU_RunBlock(1, 6);    //调用MDU运算单元0中的LPF模式（6）
    return LPF1_YH;
}

int16 LPF_realize2(int16 Xn1, int16 Xn0, int16 K)
{
    LPF2_K = K;
    LPF2_X = Xn1;
    LPF2_YH = Xn0;
    SMDU_RunBlock(2, 6);    //调用MDU运算单元0中的LPF模式（6）
    return LPF2_YH;
}


/*风机PI函数*/
int16 fengjiPI(int16 feedbackvalue, int16 referencevalue)
{
    PI2_EK =  referencevalue -  feedbackvalue;
    SMDU_RunBlock(2, 7);    //调用MDU运算单元0中的PI模式（7）
    return PI2_UKH;
}

/*---------------------------------------------------------------------------*/
/*  Name     :   int32 Abs_F32(int32 value)
    /* Input    :   value
    /* Output   :   int16
    /* Description: 对变量取16位的绝对值
    /*---------------------------------------------------------------------------*/
uint32 Abs_F32(int32 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/*****转矩补偿和无转矩补偿时过流值设置*****/
void DCOvercurrentValue(void)
{
    static uint8 DAC_Value, DAC_Value2;
    
    if ((mcSpeedRamp.ActualValue < _Q15(2000.0 / MOTOR_SPEED_BASE)) && IQCOMPENSATEENBLE == 1)
    {
        DAC_Value = TorDAC_OvercurrentValue;
    }
    else
    {
        DAC_Value = DAC_OvercurrentValue;
    }
    
    DAC_Value2 = DAC0_DR;
    
    if (DAC_Value < DAC_Value2)
    {
        DAC_Value2 -= 1;
    }
    else if (DAC_Value > DAC_Value2)
    {
        DAC_Value2 += 1;
    }
    
    if (DAC_Value2 < 255)
    {
        DAC0_DR = (uint8)DAC_Value2;
    }
    else
    {
        DAC0_DR = 255;
    }
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void Speed_response(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制、恒功率控制
    /*---------------------------------------------------------------------------*/
void Speed_response(void)
{
    static uint8 Start_CNT = 0;
    
    if ((mcState == mcRun) || (mcState == mcStop))
    {
        if (mcFocCtrl.RunStateCnt < MotorStartHoldTime)                                                 // MotorStartHoldTime目标转速为启动转速
        {
            //                    PI0_KP = SKP;
            //                      PI0_KI = SKI;
            //                  PI0_KP =  _Q12(0.01 * SKP1);
            //          PI0_KI             =  _Q15(0.001 * SKI1);
            mcSpeedRamp.IncValue    = SpeedRampStartInc;
            mcFocCtrl.RunStateCnt++;
            
            if (mcSpeedRamp.FlagONOFF == 1)
            {
                mcSpeedRamp.TargetValue = Motor_Start_Hold_Speed;                    //上油时间内维持上油转速
            }
            else
            {
                mcSpeedRamp.TargetValue = 0;
                mcSpeedRamp.DecValue    = Motor_Speed_Dec1;
            }
        }
        else                                                                                                                            //如果达到上油转速后，将起动速度环增量变为运行速度环增量
        {
            Current_Speed_PI();
            DCOvercurrentValue();
            mcSpeedRamp.IncValue    = Motor_Speed_Inc;
            
            if ((mcSpeedRamp.FlagONOFF == 0) || (mcSpeedRamp.TargetValue == 0))
            {
                mcSpeedRamp.DecValue    = Motor_Speed_Dec1;
            }
            else
            {
                mcSpeedRamp.DecValue    = Motor_Speed_Dec;
            }
        }
        
        switch (mcFocCtrl.CtrlMode)
        {
            case 0:
            {
                if (mcFocCtrl.SpeedFlt > Motor_Loop_Speed) //>=MOTOR_LOOP_RPM
                {
                    if (Start_CNT < 15)
                    {
                        Start_CNT ++;
                    }
                    else
                    {
                        mcFocCtrl.CtrlMode = 1;
                        Start_CNT = 0;
                        #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
                        {
                            mcSpeedRamp.ActualValue = mcFocCtrl.SpeedFlt + _Q15(150 / MOTOR_SPEED_BASE); //mcFocCtrl.SpeedFlt;  FOC__EOME
                        }
                        #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                        {
                            mcSpeedRamp.ActualValue = mcFocCtrl.Powerlpf;
                        }
                        #endif
                        mcFocCtrl.LoopTime = SPEED_LOOP_TIME;
                        mcFocCtrl.IND_DEC_LoopTime = SPEED_INC_DEC_TIME;
                        PI0_UKH = mcFocCtrl.mcIqref;
                        mcFocCtrl.IsRef    = mcFocCtrl.mcIqref;
                    }
                }
            }
            break;
            
            case 1:
            {
                IsLimit_Over_deal();
                mcFocCtrl.LoopTime++;
                mcFocCtrl.IND_DEC_LoopTime++;
                mcFocCtrl.Weak_LoopTime++;
                
                if (mcFocCtrl.IND_DEC_LoopTime > SPEED_INC_DEC_TIME)
                {
                    mc_ramp(&mcSpeedRamp);
                    mcFocCtrl.IND_DEC_LoopTime = 0;
                }
                
                if (mcFocCtrl.LoopTime > SPEED_LOOP_TIME)
                {
                    #if (OUTLoop_Mode == OUTLoop_Disable)
                    {
                        mcFocCtrl.mcIqref = FOC_IQREF;
                        
                        if (FOC_IQREF < mcFocCtrl.QoutValue)
                        {
                            mcFocCtrl.mcIqref += QOUTINC;
                            
                            if (mcFocCtrl.mcIqref > mcFocCtrl.QoutValue)
                            {
                                mcFocCtrl.mcIqref = mcFocCtrl.QoutValue;
                            }
                            
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                        else if (FOC_IQREF > mcFocCtrl.QoutValue)
                        {
                            mcFocCtrl.mcIqref -= QOUTINC;
                            
                            if (mcFocCtrl.mcIqref < mcFocCtrl.QoutValue)
                            {
                                mcFocCtrl.mcIqref = mcFocCtrl.QoutValue;
                            }
                            
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                    }
                    #elif (OUTLoop_Mode == OUTLoop_Enable)
                    {
                        #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
                        {
                            mcFocCtrl.IsRef = PI_realize(mcSpeedRamp.ActualValue - mcFocCtrl.SpeedFlt);
                        }
                        #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                        {
                            FOC_IQREF = HW_One_PI(mcSpeedRamp.ActualValue - mcFocCtrl.Powerlpf);
                        }
                        #endif
                    
                        STT_FOC_THECOMP_CLEAR();
                    }
                    #endif
                    mcFocCtrl.LoopTime = 0;
                }
            }
            break;
        }
    }
}


void main_ms(void)
{
    SetBit(ADC_CR, ADCBSY);           //使能ADC的DCBUS采样
    //        while(ReadBit(ADC_CR , ADCBSY));
    
    /****速度滤波、反电动势滤波*****/
    if ((mcState == mcStart) || (mcState == mcRun) || (mcState == mcStop))
    {
        //                      SqrtU_alpbet(FOC__UD,FOC__UQ,mcFocCtrl.VDQ);//
        mcFocCtrl.BEmf         =  LPF_realize1(FOC__EMF, mcFocCtrl.BEmf,     LPF_K(5.0));        //反电动势滤波
        mcFocCtrl.Powerlpf     =  LPF_realize1(FOC__POW, mcFocCtrl.Powerlpf, LPF_K(1.0));         //功率滤波
        mcFocCtrl.IDQFlt       =  LPF_realize1(mcCurVarible.Max_is, mcFocCtrl.IDQFlt,     LPF_K(1.0));     //反电动势滤波
        
        //                    mcFocCtrl.VDQFlt       =  LPF_realize1(mcFocCtrl.VDQ, mcFocCtrl.VDQFlt,     LPF_K(5.0));     //反电动势滤波
        if (mcFocCtrl.Powerlpf <= 0)
        {mcFocCtrl.Powerlpf = 0;}
    }
    else
    {
        //                    mcFocCtrl.VDQFlt=0;
        mcFocCtrl.IDQFlt = 0;
        mcFocCtrl.BEmf     = 0;
        mcFocCtrl.Powerlpf = 0;
    }
    
    /*****温度DCbus_AND_Ibus采样获取值并滤波******/
    AdcSampleValue.ADC_huan_temp   = LPF_realize1(Huan_ADC, AdcSampleValue.ADC_huan_temp, LPF_K(20.0));        //环静温度
    AdcSampleValue.ADC_guan_temp   = LPF_realize1(Guan_ADC, AdcSampleValue.ADC_guan_temp, LPF_K(20.0));        //盘管温度
    AdcSampleValue.ADC_paiqi_temp  = LPF_realize1(Paiqi_ADC, AdcSampleValue.ADC_paiqi_temp, LPF_K(20.0));      //排气温度
    AdcSampleValue.ADC_IPM_temp    = LPF_realize1(IPM_ADC, AdcSampleValue.ADC_IPM_temp, LPF_K(20.0));          //IPM温度
    mcFocCtrl.mcDcbusFlt           = LPF_realize1(DCBUS_ADC, mcFocCtrl.mcDcbusFlt, LPF_K(5.0));               //母线电压滤波
    mcFocCtrl.mcAcbusFlt           = LPF_realize1(mcFocCtrl.mcAcbus_max, mcFocCtrl.mcAcbusFlt, LPF_K(1.0));      //AC电压峰值滤波
    #if (IQCOMPENSATEENBLE)
    {
        TorquePeakK(mcFocCtrl.Powerlpf);  //分段处理
    }
    #endif
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void LED_Display(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: LED灯显示
    /*---------------------------------------------------------------------------*/
void LED_Display(void)
{
    if (mcFaultSource == FaultNoSource)
    {
        ResetLEDPin;            //低电平点亮，高电平灭
    }
    else
    {
        Led_OnOff(&mcLedDisplay, mcFaultSource);
    }
}

/*角度补偿清零*/
void STT_FOC_THECOMP_CLEAR(void)
{
    mcFocCtrl.foc_comp_temp = mcFocCtrl.STT_FOC_THECOMP;
    
    if (mcFocCtrl.foc_comp_temp < 0)
    {
        mcFocCtrl.foc_comp_temp++;
        mcFocCtrl.STT_FOC_THECOMP    =  mcFocCtrl.foc_comp_temp;
    }
    else if (mcFocCtrl.foc_comp_temp > 0)
    {
        mcFocCtrl.foc_comp_temp--;
        mcFocCtrl.STT_FOC_THECOMP    =  mcFocCtrl.foc_comp_temp;
    }
    else
    {
        mcFocCtrl.foc_comp_temp = 0;
        mcFocCtrl.STT_FOC_THECOMP       = 0;
    }
    
    FOC_THECOMP = mcFocCtrl.STT_FOC_THECOMP;
}

//LED灯的闪烁
void Led_OnOff(MCLedDisplay * hLedDisplay, uint8 htime)
{
    hLedDisplay->LedCount++;
    
    if (hLedDisplay->LedCount < hLedDisplay->Counttime)
    {
        if (hLedDisplay->Count < 200)
        {
            hLedDisplay->Count++;
        }
        else if ((hLedDisplay->Count >= 200) && (hLedDisplay->Count < 201))
        {
            hLedDisplay->Count = 0;
            LEDPinONOFF;
            hLedDisplay->LedTimCot++;
        }
        
        if (hLedDisplay->LedTimCot >= 2 * htime)
        {
            hLedDisplay->Count = 202;
            SetLEDPin;
        }
    }
    else if (hLedDisplay->LedCount >= hLedDisplay->Counttime)
    {
        hLedDisplay->LedCount  = 0;
        hLedDisplay->LedTimCot = 0;
        hLedDisplay->Count     = 0;
    }
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void mc_ramp(void)
    /* Input    :   hTarget,MC_RAMP *hSpeedramp
    /* Output   :   NO
    /* Description:
    /*---------------------------------------------------------------------------*/

void mc_ramp(MCRAMP * hSpeedramp)
{
    if (hSpeedramp->ActualValue < hSpeedramp->TargetValue)
    {
        if (hSpeedramp->ActualValue + hSpeedramp->IncValue < hSpeedramp->TargetValue)
        {
            hSpeedramp->ActualValue += hSpeedramp->IncValue;
        }
        else
        {
            hSpeedramp->ActualValue = hSpeedramp->TargetValue;
        }
    }
    else
    {
        if (hSpeedramp->ActualValue - hSpeedramp->DecValue > hSpeedramp->TargetValue)
        {
            hSpeedramp->ActualValue -= hSpeedramp->DecValue;
        }
        else
        {
            hSpeedramp->ActualValue = hSpeedramp->TargetValue;
        }
    }
}


/*---------------------------------------------------------------------------*/
/*  Name     :   void StarRampDealwith(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description:
    /*---------------------------------------------------------------------------*/

void StarRampDealwith(void)
{
    if ((mcState == mcRun) || (mcState == mcStart))
    {
        if ((mcFocCtrl.CtrlMode == 0) || (mcFocCtrl.State_Count > 10))
        {
            if (mcFocCtrl.iiqq <= IQ_Start_CURRENT2)
            {mcFocCtrl.iiqq = mcFocCtrl.iiqq + 2;}
            
            FOC_IQREF = mcFocCtrl.iiqq;
        }
        
        if (mcFocCtrl.State_Count == 1200) //2300
        {
            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) //2000
        {
            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) //1600
        {
            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) //1200
        {
            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
        }
    }
}

void MCTorqueControl(void)
{
}
int16  idref = 0;
void ID_IQ_Control(void)
{
    #if  (IFFDebugg==0)
    {
        if (mcFocCtrl.CtrlMode)
        {
            #if (IQCOMPENSATEENBLE==1)
            {
                FOC_IQREF = mcFocCtrl.IqRef + mcTorqueCompensation.TorqueCompensateIqCurrentValue;
            }
            #elif (IQCOMPENSATEENBLE==0)
            {
                FOC_IQREF = mcFocCtrl.IqRef;
            }
            #endif
            FOC_IDREF = mcFocCtrl.IdRef;
        }
    }
    #elif  (IFFDebugg==1)
    {
        //       iqfedback=FOC__IQ;
        //       iqffff=FOC_IQREF;
        /*假负载测试*/
        if ((mcState == mcRun))
        {
            if (Time.PowerOnCnt >= 1500)
            {
                qiangtujiaodu += 1920;
                idref = idref + 1;
    
                if (idref >= s2_cvcf_i_set)
                {idref = s2_cvcf_i_set;}
    
                FOC__THETA  =   qiangtujiaodu;
                FOC_IQREF   = idref;
                FOC_IDREF   = 0;
            }
            else
            {
                FOC_IQREF = 0;
                FOC_IDREF = 0;
            }
        }
    }
    #endif
}


/*PI参数分段处理*/
void Current_Speed_PI(void)
{
    static int16 Skp, Ski, DQkp, DQki;
    static int16 Skp_1, Ski_1, DQkp_1, DQki_1;
    
    if (mcFocCtrl.SpeedFlt < _Q15(2000 / MOTOR_SPEED_BASE))
    {
        Skp = SKPRun;
        Ski = SKIRun;
        DQkp = DQKP;
        DQki = DQKI;
    }
    else if (mcFocCtrl.SpeedFlt < _Q15(4200 / MOTOR_SPEED_BASE))
    {
        Skp = SKPRun1;
        Ski = SKIRun1;
        //          DQkp             =  _Q12(0.01 * SKP1);
        //      DQki             =  _Q15(0.001 * SKI1);
        DQkp = DQKP_zhong;
        DQki = DQKI_zhong;
    }
    else if (mcFocCtrl.SpeedFlt > _Q15(4700 / MOTOR_SPEED_BASE))
    {
        Skp = SKPRun2;
        Ski = SKIRun2;
        DQkp = DQKP_highspeed;
        DQki = DQKI_highspeed;
    }
    
    Skp_1 = PI0_KP;
    Ski_1 = PI0_KI;
    
    if (Skp_1 < Skp)
    {
        Skp_1 += 2;
    }
    else if (Skp_1 > Skp)
    {
        Skp_1 -= 2;
    }
    
    if (Ski_1 < Ski)
    {
        Ski_1 += 2;
    }
    else if (Ski_1 > Ski)
    {
        Ski_1 -= 2;
    }
    
    PI0_KP = Skp_1;
    PI0_KI = Ski_1;
    DQkp_1 = FOC_QKP;
    DQki_1 = FOC_QKI;
    
    if (DQkp_1 < DQkp)
    {
        DQkp_1 += 2;
    }
    else if (DQkp_1 > DQkp)
    {
        DQkp_1 -= 2;
    }
    
    if (DQki_1 < DQki)
    {
        DQki_1 += 2;
    }
    else if (DQki_1 > DQki)
    {
        DQki_1 -= 2;
    }
    
    FOC_DKP = DQkp_1;
    FOC_DKI = DQki_1;
    FOC_QKP = DQkp_1;
    FOC_QKI = DQki_1;
}

uint16 AimFrequencyMaxVAC = 500;
/*指令获取*/
void Control_Cmd(void)
{
    if (Time.PowerOnCnt >= 2000)
    {
        //压机指令获取
        if (ConTrolCmd.yajispeed > 300)
        {
            mcSpeedRamp.FlagONOFF   = 1;
        }
        else
        {
            mcSpeedRamp.TargetValue = 0;
            mcSpeedRamp.FlagONOFF   = 0;
            mcProtectTime.CurrentPretectTimes    = 0;
            mcProtectTime.SecondStartTimes       = 0;
            mcProtectTime.StallTimes             = 0;
            mcProtectTime.LossPHTimes            = 0;
            mcProtectTime.IbusOffsetProtectTimes = 0;
        }
        
        //4通阀指令获取   WAY4
        //           WAY4=ConTrolCmd.sitongfa;
        //电加热
        //           DJR=ConTrolCmd.dianjiare;
        //室外风机
        ConTrolCmd.fengjispeed;
        //电子膨胀阀开度获取
        ConTrolCmd.dianzifakaidu ;
    }
    else
    {
        mcSpeedRamp.TargetValue        = 0;
        mcSpeedRamp.FlagONOFF          = 0;
        ConTrolCmd.fengjispeed         = 0;
        //           WAY4                                               = 0;
    }
    
    //        AimFrequencyMaxVAC=VAC_Frequency_Max()*6;
    if (ConTrolCmd.yajispeed > Motor_Max_Speed) //限制最高转速
    {
        ConTrolCmd.yajispeed  = Motor_Max_Speed;
    }
    
    //压机闭环目标转速
    mcSpeedRamp.TargetValue =  S_Value(ConTrolCmd.yajispeed);
    
    if ((mcSpeedRamp.TargetValue <= Motor_Min_Speed) && (mcSpeedRamp.FlagONOFF == 1))
    {
        mcSpeedRamp.TargetValue = Motor_Min_Speed;
    }
}


#if (Debugg==1)
void Trace_Select_read(void)    //需要放载波函数里面
{
    GPFCON1                  = PFC__IAC;
    GPFCON2                  = PFC__UAC;
    GPFCON3                  = FOC__IA;//FOC__IA;
    GPFCON4                  = FOC__IB;//FOC__IB;
    GPFCON5                  = mcFocCtrl.mcDcbusFlt;
    GPFCON6                                    = mcFocCtrl.Powerlpf;
    GPFCON7                  = mcFocCtrl.IDQFlt;
    //    GPFCON9                  = mcFocCtrl.SpeedFlt;
    GPFCON10                               = mcFocCtrl.IPMtemp;
}
void Trace_Select_write(void)   //需要放主函数里面
{
    if (mcFaultDect.commu_time >= 6000)
    {
        ConTrolCmd.yajispeed     = GPFCON8;
        s2_cvcf_i_set                                                 = GPFCON9;
    }
}
#endif


void IsLimit_Over_tect(void)
{
    //      if(mcIimit.flag==0||mcIimit.flag==3)
    //      {
    //       if((mcFocCtrl.Powerlpf>P_OVER)||(mcFocCtrl.IDQFlt>I_OVER))
    //           {
    //                  mcIimit.mcIDQtime3=0;
    //                  if (mcIimit.mcIDQtime1 < 50)
    //                  {
    //                          mcIimit.mcIDQtime1++;
    //                  }
    //                  else
    //                  {
    //                          mcIimit.flag=1;
    //                  }
    //          }
    //      }
    //      if(mcIimit.flag==1)
    //      {
    //            if((mcFocCtrl.Powerlpf<P_LIMIT)||(mcFocCtrl.IDQFlt<I_LIMIT))
    //              {
    //                      mcIimit.mcIDQtime1=0;
    //                      if (mcIimit.mcIDQtime2 < 50)
    //                      {
    //                              mcIimit.mcIDQtime2++;
    //                      }
    //                      else
    //                      {
    //                              mcIimit.flag=2;
    //                      }
    //            }
    //              mcIimit.mcIDQtime4++;
    //              if(mcIimit.mcIDQtime4>=45000)
    //              {
    //                    mcIimit.flag=2;
    //                      mcIimit.mcIDQtime4=0;
    //        }
    //      }
    //      if(mcIimit.flag==2)
    //      {
    //              mcIimit.mcIDQtime2=0;
    //              if (mcIimit.mcIDQtime3 < 10000)
    //              {
    //                      mcIimit.mcIDQtime3++;
    //              }
    //              else
    //              {
    //                      mcIimit.flag=3;
    //              }
    //      }
    ////IIIIII
    if (mcIimit.flag == 0 || mcIimit.flag == 3)
    {
        if ((mcFocCtrl.IDQFlt > I_OVER))
        {
            mcIimit.mcIDQtime3 = 0;
            
            if (mcIimit.mcIDQtime1 < 50)
            {
                mcIimit.mcIDQtime1++;
            }
            else
            {
                mcIimit.flag = 1;
            }
        }
    }
    
    if (mcIimit.flag == 1)
    {
        if ((mcFocCtrl.IDQFlt < I_LIMIT))
        {
            mcIimit.mcIDQtime1 = 0;
            
            if (mcIimit.mcIDQtime2 < 50)
            {
                mcIimit.mcIDQtime2++;
            }
            else
            {
                mcIimit.flag = 2;
            }
        }
        
        mcIimit.mcIDQtime4++;
        
        if (mcIimit.mcIDQtime4 >= 45000)
        {
            mcIimit.flag = 2;
            mcIimit.mcIDQtime4 = 0;
        }
    }
    
    if (mcIimit.flag == 2)
    {
        mcIimit.mcIDQtime2 = 0;
        
        if (mcIimit.mcIDQtime3 < 10000)
        {
            mcIimit.mcIDQtime3++;
        }
        else
        {
            mcIimit.flag = 3;
        }
    }
    
    //PPPPPP
    if (mcIimit.Pflag == 0 || mcIimit.Pflag == 3)
    {
        if ((mcFocCtrl.Powerlpf > P_OVER))
        {
            mcIimit.mcPtime3 = 0;
            
            if (mcIimit.mcPtime1 < 50)
            {
                mcIimit.mcPtime1++;
            }
            else
            {
                mcIimit.Pflag = 1;
            }
        }
    }
    
    if (mcIimit.Pflag == 1)
    {
        if ((mcFocCtrl.Powerlpf < P_LIMIT))
        {
            mcIimit.mcPtime1 = 0;
            
            if (mcIimit.mcPtime2 < 50)
            {
                mcIimit.mcPtime2++;
            }
            else
            {
                mcIimit.Pflag = 2;
            }
        }
        
        mcIimit.mcPtime4++;
        
        if (mcIimit.mcPtime4 >= 45000)
        {
            mcIimit.Pflag = 2;
            mcIimit.mcPtime4 = 0;
        }
    }
    
    if (mcIimit.Pflag == 2)
    {
        mcIimit.mcPtime2 = 0;
        
        if (mcIimit.mcPtime3 < 10000)
        {
            mcIimit.mcPtime3++;
        }
        else
        {
            mcIimit.Pflag = 3;
        }
    }
}

void IsLimit_Over_deal(void)
{
    if ((mcIimit.flag == 1) || mcIimit.Pflag == 1)
    {
        mcSpeedRamp.TargetValue =  _Q15(1800 / MOTOR_SPEED_BASE);
    }
    else if ((mcIimit.flag == 2) || (mcIimit.Pflag == 2))
    {
        mcSpeedRamp.TargetValue = mcSpeedRamp.ActualValue;
    }
}


uint16 VAC_Frequency_Max(void)
{
    static int16  Limit_Fre_Max = 0, VAC_last = 32765, VAC_chazhi = 0;
    VAC_chazhi = VAC_last - mcFocCtrl.mcAcbusFlt;
    
    if ((VAC_chazhi > _Q15(7.0 / HW_BOARD_ACVOLT_MAX)) || (VAC_chazhi < (_Q15(-7.0 / HW_BOARD_ACVOLT_MAX))) || (mcFocCtrl.mcAcbustime > 10000))
    {
        if (mcFocCtrl.mcAcbusFlt <= VAC_Min)
        {
            Limit_Fre_Max = VAC_Min_F;
        }
        else if (mcFocCtrl.mcAcbusFlt <= VAC_Max)
        {
            Limit_Fre_Max = VAC_Min_F + (int16)(VAC_Max_K * (mcFocCtrl.mcAcbusFlt - VAC_Min));
        }
        else
        {
            Limit_Fre_Max = VAC_Max_F ;
        }
        
        VAC_last = mcFocCtrl.mcAcbusFlt;
        mcFocCtrl.mcAcbustime = 0;
    }
    
    return Limit_Fre_Max;
}