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

FaultStateType     xdata   mcFaultSource;
PFCFaultStateType  xdata   PFCFaultSource;
FaultVarible       idata   mcFaultDect;
PFCFaultVarible    idata   PFCFaultDect;
ProtectVarible     xdata   mcProtectTime;

/*****************************************************************************
    Function:         void   Fault_OverVoltage(mcFaultVarible *h_Fault)
    Description:  过压欠压保护函数：程序每5ms判断一次，母线电压大于过压保护值时，计数器加一，计数器值超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
                 同理，欠压保护。
                   电机过欠压保护状态下，母线电压恢复到欠压恢复值以上，过压恢复值以下时，计数器加一，超过200次后，恢复。根据档位信息来决定恢复到哪个状态。
    Parameter:        mcFaultVarible *h_Fault
    Return:           no
 *****************************************************************************/
void Fault_OverUnderVoltage(void)
{
    if ((mcFaultSource == FaultNoSource) && (mcState == mcRun)) //程序无其他保护下
    {
        //过压保护
        if (mcFocCtrl.mcDcbusFlt > OVER_PROTECT_VALUE)   //母线电压大于过压保护值时，计数，超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
        {
            mcFaultDect.OverVoltDetecCnt++;
            
            if (mcFaultDect.OverVoltDetecCnt > 20) //检测100ms
            {
                mcFaultDect.OverVoltDetecCnt = 0;
                mcFaultSource             = FaultOverVoltage;
                FaultProcess();
            }
        }
        else
        {
            if (mcFaultDect.OverVoltDetecCnt > 0)
            { mcFaultDect.OverVoltDetecCnt--; }
        }
        
        //欠压保护
        if (mcFocCtrl.mcDcbusFlt < UNDER_PROTECT_VALUE)
        {
            mcFaultDect.UnderVoltDetecCnt++;
            
            if (mcFaultDect.UnderVoltDetecCnt > 20) //检测100ms
            {
                mcFaultDect.UnderVoltDetecCnt = 0;
                mcFaultSource              = FaultUnderVoltage;
                FaultProcess();
            }
        }
        else
        {
            if (mcFaultDect.UnderVoltDetecCnt > 0)
            { mcFaultDect.UnderVoltDetecCnt--; }
        }
    }
    
    /*******过压欠压保护恢复*********/
    if ((mcState == mcFault) && ((mcFaultSource == FaultUnderVoltage) || (mcFaultSource == FaultOverVoltage)))
    {
        if ((mcFocCtrl.mcDcbusFlt < OVER_RECOVER_VALUE) && (mcFocCtrl.mcDcbusFlt > UNDER_RECOVER_VALUE))
        {
            mcFaultDect.VoltRecoverCnt++;
            
            if (mcFaultDect.VoltRecoverCnt > 2000) //连续检测1s，若正常则恢复
            {
                mcFaultSource           = FaultNoSource;
                mcFaultDect.VoltRecoverCnt = 0;
            }
        }
        else
        {
            mcFaultDect.VoltRecoverCnt = 0;
        }
    }
}

/*****************************************************************************
    Function:     void Fault_Overcurrent(CurrentVarible *h_Cur)
    Description:  电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则OverCurCnt加1。
                 连续累加3次，判断为软件过流保护。执行时间约30.4us。
    Parameter:    mcFaultVarible *h_Fault
    Return:       no
 *****************************************************************************/
void Fault_Overcurrent(void)
{
    static uint16 IIs;
    
    if (mcFaultSource == FaultNoSource)
    {
        if (mcState == mcStart || mcState == mcAlign || mcState == mcRun || mcState == mcStop)
        {
            SqrtI_alpbet(FOC__ID, FOC__IQ, IIs); //平方和后开根号
            mcCurVarible.Max_is = IIs;
            
            if (mcCurVarible.Max_is >= SW_OC_CurrentVal) // 过流保护
            {
                mcCurVarible.time++;
                
                if (mcCurVarible.time > SW_OC_DectTime)
                {
                    mcCurVarible.time  = 0;
                    mcFaultSource = FaultSoftOVCurrent;
                    FaultProcess();
                }
            }
            else
            {
                mcCurVarible.time = 0;
            }
        }
    }
}
/*****************************************************************************
    Function:         void   Fault_OverCurrentRecover(mcFaultVarible *h_Fault)
    Description:  软硬件过流保护恢复
    Parameter:        mcFaultVarible *h_Fault
    Return:           no
 *****************************************************************************/
void Fault_OverCurrentRecover(void)
{
    if ((mcState == mcFault) && ((mcFaultSource == FaultSoftOVCurrent)
            || (mcFaultSource == FaultHardOVCurrent)) && (mcProtectTime.CurrentPretectTimes < CurrentProtectRestartTimes))
    {
        mcFaultDect.CurrentRecoverCnt++;
        
        if (mcFaultDect.CurrentRecoverCnt >= OverCurrentRecoverTime) //200*5=1s
        {
            mcFaultDect.CurrentRecoverCnt = 0;
            mcProtectTime.CurrentPretectTimes++;
            mcFaultSource = FaultNoSource;
            PRE_DRIVER_RST = 0;
        }
    }
}

/*****************************************************************************
    Function:         void   Fault_Start(mcFaultVarible *h_Fault)
    Description:  启动保护函数，电机运行状态下，电机在前5s估算转速达到堵转保护值或者5s后反电动势值太低(此方法未验证)
                  或4s内还在CtrlMode状态，即速度低于MOTOR_LOOP_RPM，程序判断为启动失败，电机停机。
                  当程序判断为启动失败后，若重启次数少于或等于5次，程序立即进入校准状态，等待重启。
    Parameter:        mcFaultVarible *h_Fault
    Return:           no
 *****************************************************************************/
void Fault_Start(void)
{
    /*******启动保护恢复*********/
    if (mcState == mcRun)
    {
        //方法一，5s内速度大于最大速度，同时反电动势值低于一定值
        if (mcFaultDect.StartSpeedCnt <= 1000)
        {
            mcFaultDect.StartSpeedCnt++;
            
            if ((motorControl.ActualSpeed > MOTOR_MAX_SPEED) && (mcFocCtrl.BEmf < 1300))
            {
                mcFaultDect.StartSpeedCnt = 0;
                mcFaultSource          = FaultStart;
                FaultProcess();
                mcProtectTime.StartFlag = 1;
            }
        }
        
        //方法二
        if (mcFaultDect.StartEsCnt <= 1200) //前6s，等待1.5s后，开始判断ES，如果超过一定次数，则失败
        {
            mcFaultDect.StartEsCnt++;
            mcFaultDect.StartDelay++;
            
            if (mcFaultDect.StartDelay >= 300)               // 1.5s
            {
                mcFaultDect.StartDelay = 300;
                
                if ((mcFocCtrl.BEmf < 500)) //&&(mcFocCtrl.CtrlMode==0))
                {
                    mcFaultDect.StartESCount++;
                    
                    if (mcFaultDect.StartESCount >= 20)
                    {
                        mcFaultSource = FaultStart;
                        FaultProcess();
                        mcFaultDect.StartDelay     = 0;
                        mcFaultDect.StartESCount   = 0;
                        mcProtectTime.StartFlag = 2;
                    }
                }
                else
                {
                    if (mcFaultDect.StartESCount > 0)
                    {
                        mcFaultDect.StartESCount--;
                    }
                }
            }
        }
        else
        {
            mcFaultDect.StartESCount = 0;
        }
        
        //方法三，长时间在CtrlMode=0状态
        if (mcFocCtrl.CtrlMode == 0)        //
        {
            mcFaultDect.StartFocmode++;
            
            if (mcFaultDect.StartFocmode >= 400)
            {
                mcFaultDect.StartFocmode = 0;
                mcFaultSource         = FaultStart;
                FaultProcess();
                mcProtectTime.StartFlag = 3;
            }
        }
    }
    
    if ((mcFaultSource == FaultStart) && (mcState == mcFault) && (mcProtectTime.SecondStartTimes <= StartProtectRestartTimes))
    {
        mcProtectTime.SecondStartTimes++;
        mcFaultSource = FaultNoSource;
    }
}
/*****************************************************************************
    Function:         void   Fault_Stall(mcFaultVarible *h_Fault)
    Description:  堵转保护函数，有三种保护方式，
                  第一种，
                  第二种，电机运行状态下，延迟4s判断，估算速度绝对值超过堵转速度连续5次；
                  第三种，电机运行状态下，当U,V两相电流绝对值大于堵转电流保护值连续6次；
                  当以上三种的任何一种保护触发时，电机停机，程序判断为堵转保护；
                  当堵转保护状态下，U相采集值低于堵转恢复值时，若堵转次数小于或等于堵转重启次数8次，
                  程序延迟mcStallRecover重新启动，进行校准状态。
    Parameter:        mcFaultVarible *h_Fault
    Return:           no
*****************************************************************************/
//堵转保护
void Fault_Stall(void)
{
    if (mcState == mcRun)
    {
        if (mcFaultDect.StallDelayCnt <= 1000) //5s
        {
            mcFaultDect.StallDelayCnt++;
        }
        else
        {
            //method 1，判断反电动势太小或当反电动势太小，转速太大
            if ((mcFocCtrl.BEmf < 1000) || ((FOC__EOME > _Q15(3000.0 / MOTOR_SPEED_BASE)) && (mcFocCtrl.BEmf < 2000)))
            {
                mcFaultDect.StallDectEs++;
                
                if (mcFaultDect.StallDectEs >= 10)
                {
                    mcFaultDect.StallDectEs = 0;
                    mcFaultSource        = FaultStall;
                    FaultProcess();
                    mcProtectTime.StallFlag = 1;
                }
            }
            else
            {
                if ( mcFaultDect.StallDectEs > 0)
                {
                    mcFaultDect.StallDectEs--;
                }
            }
            
            //method 2，判断速度低于堵转最小值或者超过堵转最大值
            if ((motorControl.ActualSpeed < MOTOR_STALL_MIN_SPEED) || (motorControl.ActualSpeed > MOTOR_STALL_MAX_SPEED))
            {
                mcFaultDect.StallDectSpeed++;
                
                if (mcFaultDect.StallDectSpeed >= 8)
                {
                    mcFaultDect.StallDectSpeed = 0;
                    mcFaultSource           = FaultStall;
                    FaultProcess();
                    mcProtectTime.StallFlag = 2;
                }
            }
            else
            {
                if (mcFaultDect.StallDectSpeed > 0)
                {
                    mcFaultDect.StallDectSpeed--;
                }
            }
            
            //method 3
            //                      if((mcFocCtrl.BEmf<=25*(uint32)motorControl.ActualSpeed/10)&&mcFocCtrl.BEmf>=10*(uint32)motorControl.ActualSpeed/10)//1.15
            //                      {
            //                          mcFaultDect.StallDectEs2 = 0;
            //                      }
            //                      else
            //                      {
            //
            //                          mcFaultDect.StallDectEs2++;
            //                          if(mcFaultDect.StallDectEs2>=30)
            //                          {
            //                                  mcFaultDect.StallDectEs2 = 0;
            //                                  mcFaultSource = FaultStall;
            //                                  FaultProcess();
            //                          }
            //
            //                      }
        }
    }
    
    /*******堵转保护恢复*********/
    if ((mcFaultSource == FaultStall) && (mcState == mcFault) && (mcProtectTime.StallTimes < StallProtectRestartTimes))
    {
        mcFaultDect.StallReCount++;
        
        if (mcFaultDect.StallReCount >= StallRecoverTime)
        {
            mcProtectTime.StallTimes++;
            mcFaultDect.StallReCount = 0;
            mcFaultSource         = FaultNoSource;
        }
    }
    else
    {
        mcFaultDect.StallReCount = 0;
    }
}
/*****************************************************************************
    Function:         void   Fault_phaseloss(mcFaultVarible *h_Fault)
    Description:  缺相保护函数，当电机运行状态下，10ms取三相电流的最大值，
                  1.5s判断各相电流最大值，若存在两相电流值大于一定值，而第三相电流值却非常小，则判断为缺相保护，电机停机；
    Parameter:        mcFaultVarible *h_Fault
    Return:           no
*****************************************************************************/
void Fault_phaseloss(void)
{
    if (mcState == mcAlign)
    {
        if (mcFocCtrl.CurrentAlignStatus == 0)
        {
            mcFaultDect.CurrentASum += FOC__IA;
            mcFaultDect.CurrentBSum += FOC__IB;
            mcFaultDect.CurrentCSum += FOC__IC;
        }
        else if (mcFocCtrl.CurrentAlignStatus == 1)
        {
            mcFaultDect.CurrentAAlign =  mcFaultDect.CurrentASum / AlignmentHoldTime1 ;
            mcFaultDect.CurrentBAlign = -(mcFaultDect.CurrentBSum << 1) / AlignmentHoldTime1;
            mcFaultDect.CurrentCAlign = -(mcFaultDect.CurrentCSum << 1) / AlignmentHoldTime1;
            
            if ( mcFaultDect.CurrentAAlign > mcFaultDect.CurrentBAlign << 1
                || mcFaultDect.CurrentAAlign > mcFaultDect.CurrentCAlign << 1
                || mcFaultDect.CurrentBAlign > mcFaultDect.CurrentCAlign << 1
                || (mcFaultDect.CurrentAAlign < 20 && mcFaultDect.CurrentBAlign < 20 && mcFaultDect.CurrentCAlign < 20))
            {
                mcFaultSource = FaultLossPhase;
                FaultProcess();
            }
        }
    }
    
    if (mcState == mcRun)
    {
        mcCurVarible.Max_ia = FOC__IAMAX;
        mcCurVarible.Max_ib = FOC__IBMAX;
        mcCurVarible.Max_ic = FOC__ICMAX;
        mcFaultDect.Lphasecnt++;
        
        if (mcFaultDect.Lphasecnt > 40) //100*5=500ms
        {
            mcFaultDect.Lphasecnt = 0;
            
            if (((mcCurVarible.Max_ia > (mcCurVarible.Max_ib << 1)) || (mcCurVarible.Max_ia > (mcCurVarible.Max_ic << 1)))
                && (mcCurVarible.Max_ia > PhaseLossCurrentValue))
            {
                mcFaultDect.AOpencnt++;
            }
            else
            {
                if (mcFaultDect.AOpencnt > 0)
                {
                    mcFaultDect.AOpencnt --;
                }
            }
            
            if (((mcCurVarible.Max_ib > (mcCurVarible.Max_ia << 1)) || (mcCurVarible.Max_ib > (mcCurVarible.Max_ic << 1)))
                && (mcCurVarible.Max_ib > PhaseLossCurrentValue))
            {
                mcFaultDect.BOpencnt++;
            }
            else
            {
                if (mcFaultDect.BOpencnt > 0)
                {
                    mcFaultDect.BOpencnt --;
                }
            }
            
            if (((mcCurVarible.Max_ic > (mcCurVarible.Max_ia << 1)) || (mcCurVarible.Max_ic > (mcCurVarible.Max_ib << 1)))
                && (mcCurVarible.Max_ic > PhaseLossCurrentValue))
            {
                mcFaultDect.COpencnt++;
            }
            else
            {
                if (mcFaultDect.COpencnt > 0)
                {
                    mcFaultDect.COpencnt --;
                }
            }
            
            mcCurVarible.Max_ia = 0;
            mcCurVarible.Max_ib = 0;
            mcCurVarible.Max_ic = 0;
            SetBit(FOC_CR2, ICLR);  //FOC__IA/B/CMAX清零
            
            if ((mcFaultDect.AOpencnt > 3) || (mcFaultDect.BOpencnt > 3) || (mcFaultDect.COpencnt > 3))
            {
                mcFaultSource = FaultLossPhase;
                FaultProcess();
            }
        }
    }
    
    /*******缺相保护恢复*********/
    if ((mcFaultSource == FaultLossPhase) && (mcState == mcFault) && (mcProtectTime.LossPHTimes < PhaseProtectRestartTimes)) //可重启5次
    {
        mcFaultDect.mcLossPHRecCount++;
        
        if (mcFaultDect.mcLossPHRecCount >= PhaseLossRecoverTime)
        {
            mcProtectTime.LossPHTimes++;
            mcFaultSource     = FaultNoSource;
        }
    }
    else
    {
        mcFaultDect.mcLossPHRecCount = 0;
    }
}

void Fault_IPMOverTemp(void)
{
    if (mcFaultSource == FaultNoSource)
    {
        if (mcFocCtrl.IPMtemp > TempProtValue)
        {
            mcFaultDect.OverTempDetecCnt++;
            
            if (mcFaultDect.OverTempDetecCnt > 20)
            {
                mcFaultDect.OverTempDetecCnt = 0;
                mcFaultSource             = FaultIpmTemp;
                FaultProcess();
            }
        }
        else
        {
            mcFaultDect.OverTempDetecCnt = 0;
        }
    }
    
    /*******温度保护恢复*********/
    if ((mcState == mcFault) && (mcFaultSource == FaultIpmTemp))
    {
        if (mcFocCtrl.IPMtemp < TempRecoverValue)
        {
            mcFaultDect.TempRecoverCnt++;
            
            if (mcFaultDect.TempRecoverCnt > 200)
            {
                mcFaultSource           = FaultNoSource;
                mcFaultDect.TempRecoverCnt = 0;
            }
        }
        else
        {
            mcFaultDect.TempRecoverCnt = 0;
        }
    }
}

void Fault_TempSensor(void)
{
    if (mcFaultSource == FaultNoSource)
    {
        if ((Huan_temp == 1500) || (Huan_temp == -1000))
        {
            mcFaultDect.OverHuantempDetecCnt++;
            
            if (mcFaultDect.OverHuantempDetecCnt > 50)
            {
                mcFaultDect.OverHuantempDetecCnt = 0;
                mcFaultSource             = FaultHuanTempSensor;
                FaultProcess();
            }
        }
        else
        {
            mcFaultDect.OverHuantempDetecCnt = 0;
        }
        
        if ((Guan_temp == 1500) || (Guan_temp == -1000))
        {
            mcFaultDect.OverGuantempDetecCnt++;
            
            if (mcFaultDect.OverGuantempDetecCnt > 50)
            {
                mcFaultDect.OverGuantempDetecCnt = 0;
                mcFaultSource             = FaultGuanTempSensor;
                FaultProcess();
            }
        }
        else
        {
            mcFaultDect.OverGuantempDetecCnt = 0;
        }
    }
    
    /*******温度保护恢复*********/
    if ((mcState == mcFault) && ((mcFaultSource == FaultHuanTempSensor) || (mcFaultSource == FaultGuanTempSensor)))
    {
        if ((Huan_temp != 1500) && (Huan_temp != -1000))
        {
            mcFaultDect.HuantempRecoverCnt++;
            
            if (mcFaultDect.HuantempRecoverCnt > 200)
            {
                mcFaultSource           = FaultNoSource;
                mcFaultDect.HuantempRecoverCnt = 0;
            }
        }
        else
        {
            mcFaultDect.HuantempRecoverCnt = 0;
        }
        
        if ((Guan_temp != 1500) && (Guan_temp != -1000))
        {
            mcFaultDect.GuantempRecoverCnt++;
            
            if (mcFaultDect.GuantempRecoverCnt > 200)
            {
                mcFaultSource           = FaultNoSource;
                mcFaultDect.GuantempRecoverCnt = 0;
            }
        }
        else
        {
            mcFaultDect.GuantempRecoverCnt = 0;
        }
    }
}
/*通信保护*/
void Fault_cummution()
{
    if ((mcFaultDect.commu_time >= 30000) && (CummProtectEnable == 1))
    {
        ConTrolCmd.yajispeed = 0;
        ConTrolCmd.fengjispeed = 0;
    }
}

/*压机偏置电压错误恢复*/
void IOffsetRecoverEnable()
{
    if ((mcFaultSource == FaultIbusOffset) && (mcProtectTime.IbusOffsetProtectTimes < IbusOffsetRestartTimes))
    {
        mcFaultDect.IbusOffsetRecoverCnt++;
        
        if ((mcFaultDect.IbusOffsetRecoverCnt) >= IbusOffsetRecoverTime)//IbusOffsetRecoverTime*1ms
        {
            McStaSet.SetFlag.CalibFlag = 0;
            mcFaultDect.IbusOffsetRecoverCnt = 0;
            mcProtectTime.IbusOffsetProtectTimes++;
            mcFaultSource = FaultNoSource;
        }
    }
}
void Fault_UdcMax_Min(void)
{
    if ((mcFaultSource == FaultNoSource) && (mcState == mcRun))
    {
        if ((mcFocCtrl.mcDcbus_chazhi >= _Q15(120 / HW_BOARD_VOLT_MAX)) && mcFocCtrl.mcDcbus_min <= _Q15(380 / HW_BOARD_VOLT_MAX))
        {
            mcFaultDect.shurulossCnt++;
            
            if (mcFaultDect.shurulossCnt > 10)
            {
                mcFaultDect.shurulossCnt = 0;
                mcFaultSource             = FaultshuruLoss;
                FaultProcess();
            }
        }
        else
        {
            mcFaultDect.shurulossCnt--;
            
            if (mcFaultDect.shurulossCnt <= 0)
            {
                mcFaultDect.shurulossCnt = 0;
            }
        }
    }
    
    /*******保护恢复*********/
    if ((mcState == mcFault) && (mcFaultSource == FaultshuruLoss) && (mcProtectTime.shurulossTimes <= 3))
    {
        if (((mcFocCtrl.mcDcbus_chazhi) <= _Q15(20 / HW_BOARD_VOLT_MAX)) && mcFocCtrl.mcDcbus_min >= _Q15(500 / HW_BOARD_VOLT_MAX))
        {
            mcFaultDect.shurulossRecCount++;
            
            if (mcFaultDect.shurulossRecCount > 200)
            {
                mcProtectTime.shurulossTimes++;
                mcFaultSource           = FaultNoSource;
                mcFaultDect.shurulossRecCount = 0;
            }
        }
        else
        {
            mcFaultDect.shurulossRecCount = 0;
        }
    }
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void Fault_Detection(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 保护函数，因保护的时间响应不会很高，采用分段处理，每5个定时器中断执行一次对应的保护
                  常见保护有过欠压、过温、堵转、启动、缺相等保护，调试时，可根据需求，一个个的调试加入。
    /*---------------------------------------------------------------------------*/
void Fault_Detection(void)
{
    mcFaultDect.segment++;
    
    if (mcFaultDect.segment >= 7)
    {
        mcFaultDect.segment = 0;
    }
    
    if (mcFaultDect.segment == 0)
    {
        #if (CurrentRecoverEnable == 1)//过流保护恢复使能
        {
            Fault_OverCurrentRecover();
        }
        #endif
    }
    else if (mcFaultDect.segment == 1)
    {
        #if (VoltageProtectEnable == 1)//过压保护使能
        {
            Fault_OverUnderVoltage();
        }
        #endif
        #if (IbusOffsetRecoverEnable)  //偏执电压保护恢复
        {
            IOffsetRecoverEnable();
        }
        #endif
    }
    else if (mcFaultDect.segment == 2)
    {
        #if (StartProtectEnable == 1)//启动保护使能
        {
            Fault_Start();
        }
        #endif
    }
    else if (mcFaultDect.segment == 3)
    {
        #if (StallProtectEnable == 1)//堵转保护使能
        {
            Fault_Stall();
        }
        #endif
    }
    else if (mcFaultDect.segment == 4)
    {
        #if (PhaseLossProtectEnable == 1)//缺相保护使能
        {
            Fault_phaseloss();
        }
        #endif
    }
    else if (mcFaultDect.segment == 5)
    {
        #if (TempProtectEnable == 1)//IPM温度保护
        {
            Fault_IPMOverTemp();
        }
        #endif
        #if (TempSensorProtectEnable==1)
        {
            Fault_TempSensor();
        }
        #endif
    }
    
    #if (ShuruLossProtectEnable == 1)
    {
        Fault_UdcMax_Min();
    }
    #endif
}