#include <MyProject.h>

MotStaM     McStaSet;

/**
    @brief     电机控制状态机
    @warning   电机的状态只能在电机状态控制中切换，禁止在其他地方切换电机状态
    @date      2022-07-14
*/
void Motor_Control_State(void)
{
    if (FaultSource != SYS_NO_FAULT)
    { sysState = MOTOR_FAULT; }
    else if ((isCtrlPowOn == false) && (sysState != SYS_READY))
    {
        sysState = MOTOR_STOP;
        
        if (sysState == MOTOR_RUN)
        { mcFocCtrl.State_Count = 3000; }
    }
    
    switch (sysState)
    {
        case SYS_READY:
            if (McStaSet.SetFlag.CalibFlag == 0)
            {
                McStaSet.SetFlag.CalibFlag = 1;
                MOE = 0;
                ClrBit(DRV_CR, FOCEN);
                ClrBit(DRV_CR, DRVEN);
                mcCurOffset.OffsetFlag = 0;
                mcCurOffset.OffsetCount = 0;
                #if ( Shunt_Resistor_Mode == Single_Resistor)
                SetBit(ADC_MASK, CH4EN | CH2EN);
                #else
                SetBit(ADC_MASK, CH4EN | CH2EN | CH1EN | CH0EN);
                #endif
            }
            
            if ((mcCurOffset.OffsetFlag == 1) && isCtrlPowOn)
            { sysState = SYS_INIT; }
            
            break;
            
        case SYS_INIT:
            #if ( Shunt_Resistor_Mode == Single_Resistor)
            ClrBit(ADC_MASK, CH4EN );
            #else
            ClrBit(ADC_MASK, CH4EN | CH1EN | CH0EN);
            #endif
            memset(&mcFocCtrl, 0, sizeof(FOCCTRL));
            memset(&faultCheck, 0, sizeof(Fault_Check_t));
            memset(&estData, 0, sizeof(Estimated_Data_t));
            memset(&loopCtrl, 0, sizeof(Loop_Control_t));
            //缺相变量清零
            McStaSet.SetMode = 0;
            Power_Currt = 0;
            #if (CHARGE_EN)
            mcFocCtrl.State_Count   = CHARGE_TIME;
            sysState = PRE_DRIVER_CHARGE;
            #else
            {
                #if (ALIGN_MOME != ALIGN_DSIABLE)
                mcFocCtrl.mcPosCheckAngle = Align_Angle1;
                sysState = MOTOR_ALIGN;
                mcFocCtrl.State_Count = AlignAll_Time;
                #else
                sysState = MOTOR_START;
                #endif
            }
            #endif
            break;
            
        case PRE_DRIVER_CHARGE:
            Motor_Charge();
            
            if (mcFocCtrl.State_Count == 0)
            {
                MOE = 0;
                #if (ALIGN_MOME != ALIGN_DSIABLE)
                mcFocCtrl.mcPosCheckAngle = Align_Angle1;
                sysState = MOTOR_ALIGN;
                mcFocCtrl.State_Count = AlignAll_Time;
                #else
                sysState = MOTOR_START;
                #endif
            }
            
            break;
            
        case MOTOR_ALIGN:
            Motor_Align();
            #if (ALIGN_MOME == ALIGN_TEST)
            
            while (1);
            
            #else
            
            if (mcFocCtrl.State_Count > (Align_Time1 + Align_Time2 + Align_Time3) )
            {
                mcFocCtrl.CurrentAlignStatus = 0;
                FOC__THETA = Align_Angle1;
                FOC_IQREF  = IQ_Align_CURRENT * ((float)(AlignAll_Time - mcFocCtrl.State_Count) / AlignRamp_Time);
            }
            else if (mcFocCtrl.State_Count > (Align_Time2 + Align_Time3))
            {
                mcFocCtrl.CurrentAlignStatus = 1;
                FOC__THETA = Align_Angle2;
                FOC_IQREF  = IQ_Align_CURRENT;
            }
            else if (mcFocCtrl.State_Count > (Align_Time3))
            {
                mcFocCtrl.CurrentAlignStatus = 2;
                mcFocCtrl.AngleProcess = Angle_AngleK * (Align_Time2 + Align_Time3 - mcFocCtrl.State_Count);
                mcFocCtrl.AngleStart = Align_Angle2 + mcFocCtrl.AngleProcess;
                FOC__THETA = mcFocCtrl.AngleStart;
            }
            else
            {
                mcFocCtrl.CurrentAlignStatus = 2;
                sysState = MOTOR_START;
            }
            
            #endif
            break;
            
        case MOTOR_START:
            Motor_Static_Open();
            sysState = MOTOR_RUN;
            break;
            
        case MOTOR_STOP:
            if ((estData.ActSpeedFlt < MOTOR_SPEED_STOP_RPM) && (mcFocCtrl.State_Count == 0))
            {
                sysState = SYS_READY;
                ClrBit(DRV_CR, FOCEN);
                ClrBit(DRV_CR, DRVEN);
                MOE = 0;
            }
            
            break;
            
        case MOTOR_FAULT:
            if (FaultSource == SYS_NO_FAULT)
            { sysState = SYS_READY; }
            else
            {
                DRV_CMR &= 0xFFC0;
                ClrBit(DRV_CR, FOCEN);
                MOE = 0;
            }
            
            break;
    }
}

/**
    @function     Get_LPF_Value
    @brief        数据滤波处理
    @date         2025-12-23
*/
void Get_LPF_Value(void)
{
    // 开启AD转换 等待转换结束
    SetBit(ADC_CR, ADCBSY);
    
    while (ReadBit(ADC_CR, ADCBSY));
    
    // 读取AD转换数据
    Power_Currt = (ADC7_DR);
    Power_Currt = Abs_F16(Power_Currt - mcCurOffset.Iw_busOffset);
    estData.ActBusCurr = LPF_Zero_Update(Power_Currt << 2, estData.ActBusCurr, 20);
    
    if (sysState != MOTOR_RUN)
    { estData.BusVoltage = LPF_Zero_Update(ADC2_DR, estData.BusVoltage, LPF_FC(20)); }
    else
    { estData.BusVoltage = FOC__UDCFLT; }
    
    // estData.BusVoltage = LPF_Zero_Update(ADC2_DR, estData.BusVoltage, LPF_FC(20));
    estData.AlongVoltage = LPF_Zero_Update(ADC3_DR, estData.AlongVoltage, LPF_FC(100));
    estData.NTCTemper = LPF_Zero_Update(ADC9_DR, estData.NTCTemper, LPF_FC(20));
    
    // 读取观测器数据
    if ((sysState != SYS_INIT) && (sysState != SYS_READY))
    {
        estData.ActSpeedFlt = LPF_Zero_Update(FOC__EOME, estData.ActSpeedFlt, LPF_FC(30.0));
        estData.UDFlt = LPF_Zero_Update(FOC__UD, estData.UDFlt, LPF_FC(20.0));
        estData.UQFlt = LPF_Zero_Update(FOC__UQ, estData.UQFlt, LPF_FC(20.0));
        estData.IDFlt = LPF_Zero_Update(FOC__ID, estData.IDFlt, LPF_FC(20.0));
        estData.IQFlt = LPF_Zero_Update(FOC__IQ, estData.IQFlt, LPF_FC(20.0));
    }
    else
    { estData.ActSpeedFlt = 0; }
    
    if ((sysState == MOTOR_RUN) || (sysState == MOTOR_STOP))
    {
        estData.Power = LPF_Zero_Update(FOC__POW, estData.Power, LPF_FC(5.0));
        estData.BackEMF = FOC__EMF;
        estData.BusCurr = Sqrt_alpbet(FOC__IA, FOC__IBET);
    }
    else
    { estData.Power = 0; }
    
    // 保护参数更新
    faultCheck.ActualSpeed = estData.ActSpeedFlt;
    faultCheck.BusVoltage = estData.BusVoltage;
    faultCheck.NTCTemper = estData.NTCTemper;
    faultCheck.Power = estData.Power;
    faultCheck.BackEMF = estData.BackEMF;
    faultCheck.BusCurr = estData.BusCurr;
}

/**
    @function     Loop_Control
    @brief        环路响应
    @date         2025-12-23
*/
void Loop_Control(void)
{
    switch (loopCtrl.State)
    {
        case OPEN_MODE:
        {
            if (estData.ActSpeedFlt > MOTOR_LOOP_RPM)
            {
                // 切入闭环并直接开始第一次运算
                loopCtrl.State = CLOSE_MODE;
                // 切换电流环KPKI
                FOC_QKP = QKP;
                FOC_QKI = QKI;
                FOC_DKP = DKP;
                FOC_DKI = DKI;
                // 禁用D轴PI
                // ClrBit(FOC_CR2,UDD);
                // 参数配置
                loopCtrl.ActualRef = _Q15(2000.0 / MOTOR_SPEED_BASE);
                loopCtrl.Inc = MOTOR_SPEED_INC;
                loopCtrl.Dec = MOTOR_SPEED_DEC;
                // PI计算初始化
                HW_Zero_PI_Init();
                // 弱磁初始化
                #if (Filed_Weaken_En)
                Filed_Weaken_Init();
                #endif
                PI0_UKH =  FOC_IQREF;
            }
        }
        break;
        
        case CLOSE_MODE:
        {
            if (++loopCtrl.CalcTime > SPEED_LOOP_TIME)
            {
                loopCtrl.CalcTime = 0;
                
                // 控制命令爬坡 用于实现调速信号之间平滑过渡
                if (loopCtrl.TargetRef > (loopCtrl.ActualRef + loopCtrl.Inc))
                { loopCtrl.ActualRef += loopCtrl.Inc; }
                else if (loopCtrl.TargetRef < (loopCtrl.ActualRef - loopCtrl.Dec))
                { loopCtrl.ActualRef -= loopCtrl.Dec;}
                else
                { loopCtrl.ActualRef = loopCtrl.TargetRef ;}
                
                // 环路计算
                estData.ISRef = HW_Zero_Calc(loopCtrl.ActualRef - estData.ActSpeedFlt);
                // 弱磁控制
                #if (Filed_Weaken_En)
                fieldWeaken.ISRef = estData.ISRef;
                File_Weaken_Control();
                estData.IDRef = fieldWeaken.IDRef;
                estData.IQRef = fieldWeaken.IQRef;
                #else
                estData.IDRef = 0;
                estData.IQRef = estData.ISRef;
                #endif
                // 电流给定
                FOC_IQREF = estData.IQRef;
                FOC_IDREF = estData.IDRef;
            }
        }
        break;
    }
}