/**
 * @copyright None
 * @file      MotorProtect.c
 * @author    Comment Vivre
 * @date      2024-08-26
 * @brief     None
 */
#include <FU68xx_5.h>
#include <Myproject.h>

FaultStateType data mcFaultSource; ///< 故障类型
uint8 xdata mcPOSTErrSource;       ///< 自检故障类型
FaultVarible xdata fault;          ///< 故障检测相关结构体变量
FaultRecoverTypedef xdata Restart; ///< 故障恢复重启控制相关结构体变量

/**
    @brief     过温检测
    @date      2022-07-14
*/
void Fault_Temperature(void)
{
    if (mcFaultSource == FaultNoSource)
    {
        if (mcFocCtrl.NTCValue <= OVER_Temperature) // 过温保护
        {
            if (fault.Temperature.DetecCnt < TemperatureProtectTime)
            {
                fault.Temperature.DetecCnt++;
            }
            else
            {
                fault.Temperature.DetecCnt = 0;
                mcFaultSource = FaultNtcOTErr;
            }
        }
        else
        {
            fault.Temperature.DetecCnt = 0;
        }
    }
}

/**
    @brief     过欠压检测
    @date      2022-07-14
*/
void Fault_Voltage(void)
{
    if (fault.Voltage.DectDealyCnt < 100)
    {
        fault.Voltage.DectDealyCnt++;
    }
    else
    {
        if (mcFaultSource == FaultNoSource)
        {
            // 过压检测
            if (mcFocCtrl.mcDcbusFlt > OVER_VOLTAGE_PROTECT)
            {
                fault.Voltage.OverVoltDetecCnt += 1;
                
                if (fault.Voltage.OverVoltDetecCnt >= 100)
                {
                    fault.Voltage.OverVoltDetecCnt = 0;
                    mcFaultSource = FaultOverVoltageDC;
                }
            }
            else if (fault.Voltage.OverVoltDetecCnt > 0)
            { fault.Voltage.OverVoltDetecCnt--; }
            
            if (mcFocCtrl.mcDcbusFlt < UNDER_VOLTAGE_PROTECT)
            {
                fault.Voltage.UnderVoltDetecCnt += 1;
                
                if (fault.Voltage.UnderVoltDetecCnt >= 20)
                { VoltageComp.Undervoltage_flag = 1; }
                
                if (fault.Voltage.UnderVoltDetecCnt >= 60)
                {
                    fault.Voltage.UnderVoltDetecCnt = 0;
                    mcFaultSource = FaultUnderVoltageDC;
                    fault.Voltage.VoltDetecBraketCount = 500;
                    fault.Voltage.VoltDetecBraketDuty = 0.05 * DRV_ARR;
                    fault.Voltage.FlagBrakeInit = 1;
                }
            }
            else if (fault.Voltage.UnderVoltDetecCnt)
            { fault.Voltage.UnderVoltDetecCnt--; }
        }
    }
}

/**
 * @brief     欠压处理
 * @brief     刹车处理
 * @date      2022-07-14
 */
void UnderProcess(void)
{
    if (mcFaultSource == FaultUnderVoltageDC)
    {
        // 过欠压停机刹车功能
        if (fault.Voltage.VoltDetecBraketCount > 0)
        {
            DRV_DR = fault.Voltage.VoltDetecBraketDuty; // 下桥臂10% duty
            
            if (fault.Voltage.FlagBrakeInit == 1)
            {
                fault.Voltage.FlagBrakeInit = 2;
                // DRV_CMR &= 0xFFC0;
                ClrBit(DRV_CR, OCS);            // OCS = 0, PWM来源DRV_COMR
                DRV_CMR = 0x0015;               // UVW相下桥输出
                SetBit(DRV_CR, DRVEN);          // DRV计数器使能，0-禁止，1-使能
                MOE = 1;
                DRV_DR = 0.05 * DRV_ARR;
            }
        }
        else
        {
            fault.Voltage.FlagBrakeInit = 0;
            MOE = 0;
            DRV_OUT = 0x00;
        }
    }
}
/**
    @brief     软件过流检测
    @date      2022-08-09
*/
void Fault_OverCurrent(void)
{
    if (mcFaultSource == FaultNoSource)
    {
        if (mcState == mcStart || mcState == mcAlign || mcState == mcRun || mcState == mcStop)
        {
            fault.Current.Is = mcFocCtrl.Is;
            
            if (fault.Current.Is >= SW_OC_CurrentVal)
            {
                if (fault.Current.SWOC_DectTimeCnt < SW_OC_DectTime)
                { fault.Current.SWOC_DectTimeCnt++; }
                else
                {
                    fault.Current.SWOC_DectTimeCnt = 0;
                    mcFaultSource = FaultSoftOVCurrent;
                }
            }
            else
            { fault.Current.SWOC_DectTimeCnt = 0; }
        }
    }
}

/**
    @brief     堵转检测
    @date      2022-07-14
*/
void Fault_Stall(void)
{
    if (mcState == mcRun)
    {
        fault.Stall.EsValue = mcFocCtrl.EMFsquare;
        
        if (fault.Stall.DectDealyCnt < 3000)
        { fault.Stall.DectDealyCnt++; }
        else
        {
            /* ****** 1 ****** */
            if ((fault.Stall.EsValue < EsThresholdValueL))
            {
                fault.Stall.EsDectCnt++;
                
                if (fault.Stall.EsDectCnt >= 75)
                {
                    fault.Stall.EsDectCnt = 0;
                    mcFaultSource = FaultStall;
                    fault.Stall.Type = 11;
                }
            }
            else if ((fault.Stall.EsValue < EsThresholdValueH) && (mcFocCtrl.SpeedFlt > EsThresholdSpeed))
            {
                fault.Stall.EsDectCnt++;
                
                if (fault.Stall.EsDectCnt >= 60)
                {
                    fault.Stall.EsDectCnt = 0;
                    mcFaultSource = FaultStall;
                    fault.Stall.Type = 12;
                }
            }
            else if (fault.Stall.EsDectCnt > 0)
                { fault.Stall.EsDectCnt--; }
            
            /* ****** 2 ****** */
            if (mcFocCtrl.SpeedFlt < STALL_SPEED_MIN || mcFocCtrl.SpeedFlt > STALL_SPEED_MAX)
            {
                fault.Stall.SpeedMinCnt++;
                
                if (fault.Stall.SpeedMinCnt >= 75)
                {
                    fault.Stall.SpeedMinCnt = 0;
                    mcFaultSource = FaultStall;
                    fault.Stall.Type = 21;
                }
            }
            else if (fault.Stall.SpeedMinCnt > 0)
                { fault.Stall.SpeedMinCnt--; }
        }
        
        /* ****** 3 ****** */
        if (mcFocCtrl.CtrlMode == 0)
        {
            fault.Stall.Mode0DectCnt++;
            
            if (fault.Stall.Mode0DectCnt >= 6000)
            {
                fault.Stall.Mode0DectCnt = 0;
                mcFaultSource = FaultStall;
                fault.Stall.Type = 31;
            }
        }
        else
        { fault.Stall.Mode0DectCnt = 0; }
    }
}

/**
    @brief     缺相检测
    @date      2022-07-14
*/
void Fault_PhaseLoss(void)
{
    if (mcState == mcRun)
    {
        if (fault.PhaseLoss.DectDealyCnt < LP_DectDealyTIME)
        { fault.PhaseLoss.DectDealyCnt++; }
        else
        {
            if (fault.PhaseLoss.DectCycleCnt < LP_DectCycleTIME)
            {
                fault.PhaseLoss.Max_ia = FOC__IAMAX;
                fault.PhaseLoss.Max_ib = FOC__IBMAX;
                fault.PhaseLoss.Max_ic = FOC__ICMAX;
                fault.PhaseLoss.DectCycleCnt++;
            }
            else
            {
                fault.PhaseLoss.DectCycleCnt = 0;
                
                if (((fault.PhaseLoss.Max_ia > (fault.PhaseLoss.Max_ib * 3)) || (fault.PhaseLoss.Max_ia > (fault.PhaseLoss.Max_ic * 3))) && (fault.PhaseLoss.Max_ia > LP_NoLoadCurrentValue))
                {
                    fault.PhaseLoss.ALossCnt++;
                }
                else
                {
                    if (fault.PhaseLoss.ALossCnt > 0)
                    {
                        fault.PhaseLoss.ALossCnt--;
                    }
                }
                
                if (((fault.PhaseLoss.Max_ib > (fault.PhaseLoss.Max_ia * 3)) || (fault.PhaseLoss.Max_ib > (fault.PhaseLoss.Max_ic * 3))) && (fault.PhaseLoss.Max_ib > LP_NoLoadCurrentValue))
                {
                    fault.PhaseLoss.BLossCnt++;
                }
                else
                {
                    if (fault.PhaseLoss.BLossCnt > 0)
                    {
                        fault.PhaseLoss.BLossCnt--;
                    }
                }
                
                if (((fault.PhaseLoss.Max_ic > (fault.PhaseLoss.Max_ia * 3)) || (fault.PhaseLoss.Max_ic > (fault.PhaseLoss.Max_ib * 3))) && (fault.PhaseLoss.Max_ic > LP_NoLoadCurrentValue))
                {
                    fault.PhaseLoss.CLossCnt++;
                }
                else
                {
                    if (fault.PhaseLoss.CLossCnt > 0)
                    {
                        fault.PhaseLoss.CLossCnt--;
                    }
                }
                
                fault.PhaseLoss.Max_ia = 0;
                fault.PhaseLoss.Max_ib = 0;
                fault.PhaseLoss.Max_ic = 0;
                SetBit(FOC_CR2, ICLR);
                
                if ((fault.PhaseLoss.ALossCnt > 10) || (fault.PhaseLoss.BLossCnt > 10) || (fault.PhaseLoss.CLossCnt > 10))
                {
                    mcFaultSource = FaultPhaseLost;
                }
            }
        }
    }
}

/**
 * @brief     功率保护函数
 * @date      2022-07-14
 */
void Fault_Power(void)
{
    if (mcFaultSource == FaultNoSource) // 程序无其他保护下
    {
        if ((mcFocCtrl.Power > OverPowerValue) && (mcState == mcRun)) // 功率大于保护值时计数，超过20次，判断为过载保护，关闭输出;反之，计数器慢慢减
        {
            fault.Power.OverPowerDetecCnt++;
            
            if (fault.Power.OverPowerDetecCnt > 150)
            {
                fault.Power.OverPowerDetecCnt = 0;
                mcFaultSource = FaultOverPowerErr;
            }
        }
        else
        {
            if (fault.Power.OverPowerDetecCnt > 0)
            {
                fault.Power.OverPowerDetecCnt--;
            }
        }
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Recovery
    Description    : 故障恢复，条件满足只清除故障码，状态跳转由状态机执行
    Date           : 2022-07-01
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
static void Fault_Recovery(void)
{
    if (mcState == mcFault)
    {
        #if (OV_RecoveryTimes) /* DC电压保护恢复 */
        {
        
            if (mcFaultSource == FaultUnderVoltageDC || mcFaultSource == FaultOverVoltageDC)
            {
                if ((mcFocCtrl.mcDcbusFlt > UNDER_VOLTAGE_RECOVER) && (mcFocCtrl.mcDcbusFlt < OVER_VOLTAGE_RECOVER))
                {
                    if (fault.Voltage.FlagBrakeInit == 0)
                    {
                        Restart.DC_DelayTcnt++;
                    }
                    else
                    {
                        Restart.DC_DelayTcnt = 0;
                    }
                    
                    if (Restart.DC_DelayTcnt > OV_RecoveryDelayTime)
                    {
                        Restart.DC_DelayTcnt = 0;
                        mcFaultSource = FaultNoSource;
                    }
                    
                    if (Restart.DC_DelayTcnt > 40)
                    { VoltageComp.Undervoltage_flag = 0; }
                }
                else
                { Restart.DC_DelayTcnt = 0; }
                
                if (mcFaultSource == FaultUnderVoltageDC)
                {
                    if (fault.Voltage.VoltDetecBraketCount > 0)
                    {
                        fault.Voltage.VoltDetecBraketCount--;
                    }
                    
                    if (fault.Voltage.VoltDetecBraketDuty < DRV_ARR + 4)
                    {
                        fault.Voltage.VoltDetecBraketDuty += 20;
                    }
                }
            }
        }
        #endif
        #if (OT_RecoveryTimes)
        
        /* 过温保护恢复 */
        if (mcFaultSource == FaultNtcOTErr)
        {
            if (mcFocCtrl.NTCValue >= UNDER_Temperature)
            {
                if (Restart.OT_Times <= OT_RecoveryTimes)
                {
                    if (Restart.OT_DelayTcnt < OT_RecoveryDelayTime)
                    {
                        Restart.OT_DelayTcnt++;
                    }
                    else
                    {
                        Restart.OT_Times++;
                        Restart.OT_DelayTcnt = 0;
                        mcFaultSource = FaultNoSource;
                    }
                }
            }
        }
        
        #endif
        #if (LP_RecoveryTimes)
        
        /* 缺相保护恢复 */
        if (mcFaultSource == FaultPhaseLost)
        {
            if (Restart.LP_Times < LP_RecoveryTimes)
            {
                if (Restart.LP_DelayTcnt < LP_RecoveryDelayTime)
                {
                    Restart.LP_DelayTcnt++;
                }
                else
                {
                    Restart.LP_Times++;
                    Restart.LP_DelayTcnt = 0;
                    mcFaultSource = FaultNoSource;
                }
            }
        }
        
        #endif
        #if (Stall_RecoveryTimes)
        
        /* 堵转保护恢复 */
        if (mcFaultSource == FaultStall)
        {
            if (Restart.Stall_Times < Stall_RecoveryTimes)
            {
                if (Restart.Stall_DealyTcnt < Stall_RecoveryDelayTime)
                {
                    Restart.Stall_DealyTcnt++;
                }
                else
                {
                    Restart.Stall_Times++;
                    Restart.Stall_DealyTcnt = 0;
                    mcFaultSource = FaultNoSource;
                }
            }
        }
        
        #endif
        #if (OC_RecoveryTimes)
        
        /* 软件过流恢复 */
        if (mcFaultSource == FaultSoftOVCurrent || mcFaultSource == FaultHardOVCurrent)
        {
            if (Restart.SWOC_Times < OC_RecoveryTimes)
            {
                if (Restart.SWOC_DelayTcnt < OC_RecoveryDelayTime)
                {
                    Restart.SWOC_DelayTcnt++;
                }
                else
                {
                    Restart.SWOC_Times++;
                    Restart.SWOC_DelayTcnt = 0;
                    mcFaultSource = FaultNoSource;
                }
            }
        }
        
        #endif
        #if (OP_RecoveryTimes) // 功率保护恢复使能
        {
            if (mcFaultSource == FaultOverPowerErr)
            {
                if (Restart.OverPower_Times < OP_RecoveryTimes)
                {
                    if (Restart.OverPower_DealyTcnt < OP_RecoveryDelayTime)
                    {
                        Restart.OverPower_DealyTcnt++;
                    }
                    else
                    {
                        Restart.OverPower_Times++;
                        Restart.OverPower_DealyTcnt = 0;
                        mcFaultSource = FaultNoSource;
                    }
                }
            }
        }
        #endif
    }
}
/**
    @brief      偏置电压检测
*/
void Fault_GetCurrentOffset(void)
{
    if (mcCurOffset.OffsetFlag == 1)
    {
        #if (VHALF_EN == Enable) // 有加VHALF偏置，理论值为16383
        {
            #if (Shunt_Resistor_Mode == Single_Resistor) // 单电阻模式
            {
                if ((mcCurOffset.Iw_busOffset < GetCurrentOffsetValueLow) || (mcCurOffset.Iw_busOffset > GetCurrentOffsetValueHigh))
                {
                    mcFaultSource = FaultGetOffset;
                }
            }
            #elif (Shunt_Resistor_Mode == Double_Resistor) // 双电阻模式
            {
                if ((mcCurOffset.IuOffset < GetCurrentOffsetValueLow) || (mcCurOffset.IuOffset > GetCurrentOffsetValueHigh) || (mcCurOffset.IvOffset < GetCurrentOffsetValueLow) || (mcCurOffset.IvOffset > GetCurrentOffsetValueHigh))
                {
                    mcFaultSource = FaultGetOffset;
                }
            }
            #elif (Shunt_Resistor_Mode == Three_Resistor)  // 三电阻模式
            {
                if ((mcCurOffset.IuOffset < GetCurrentOffsetValueLow) || (mcCurOffset.IuOffset > GetCurrentOffsetValueHigh) || (mcCurOffset.IvOffset < GetCurrentOffsetValueLow) || (mcCurOffset.IvOffset > GetCurrentOffsetValueHigh) || (mcCurOffset.Iw_busOffset < GetCurrentOffsetValueLow) || (mcCurOffset.Iw_busOffset > GetCurrentOffsetValueHigh))
                {
                    mcFaultSource = FaultGetOffset;
                }
            }
            #endif
        }
        #else                                          // 没加VHALF偏置,理论值在0
        {
            #if (Shunt_Resistor_Mode == Single_Resistor)   // 单电阻模式
            {
                if (mcCurOffset.Iw_busOffset > GetCurrentOffsetValue)
                {
                    mcFaultSource = FaultGetOffset;
                }
            }
            #elif (Shunt_Resistor_Mode == Double_Resistor) // 双电阻模式
            {
                if ((mcCurOffset.IuOffset > GetCurrentOffsetValue) || (mcCurOffset.IvOffset > GetCurrentOffsetValue))
                {
                    mcFaultSource = FaultGetOffset;
                }
            }
            #elif (Shunt_Resistor_Mode == Three_Resistor)  // 三电阻模式
            {
                if ((mcCurOffset.IuOffset > GetCurrentOffsetValue) || (mcCurOffset.IvOffset > GetCurrentOffsetValue) || (mcCurOffset.Iw_busOffset > GetCurrentOffsetValue))
                {
                    mcFaultSource = FaultGetOffset;
                }
            }
            #endif
        }
        #endif
    }
}
/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Detection
    Description    : 故障检测与保护,扫描周期默认为1ms
                     所有故障发送只进行 故障码 赋值
                     禁止在状态机以外地方进行状态跳转
    Date           : 2022-07-01
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Fault_Detection(void)
{
    #if (OC_SW_ProtectEn == 1)
    {
        Fault_OverCurrent();
    }
    #endif
    #if (OT_ProtectEn == 1)
    {
        Fault_Temperature();
    }
    #endif
    #if (OV_ProtectEn == 1)
    {
        Fault_Voltage();
    }
    #endif
    #if (Stall_ProtectEn == 1)
    {
        Fault_Stall();
    }
    #endif
    #if (LP_ProtectEn == 1)
    {
        Fault_PhaseLoss();
    }
    #endif
    #if (OP_ProtectEn == 1) // 功率保护使能
    {
        Fault_Power();
    }
    #endif
    Fault_Recovery();
}