#include <Myproject.h>

CurrentOffset xdata mcCurOffset;

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_THECOMP     = Start_FOC_THECOMP;                      // SMO 估算补偿角
    FOC_THECOR      = 0x02;                                 // 误差角度补偿
    mcFocCtrl.STT_FOC_THECOMP = Start_FOC_THECOMP;
    // 电流环参数配置
    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
        // SVPWM 配置
        #if (SVPMW_Mode == SVPWM_7_Segment)
        ClrBit(FOC_CR2, F5SEG);
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        SetBit(FOC_CR2, F5SEG);
        #endif
    }
    // 双电阻采样，可设置死区补偿值，在下降沿结束前开始采样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;                 //下桥臂最小脉冲，保证采样
        // SVPWM 配置
        #if (SVPMW_Mode == SVPWM_7_Segment)
        ClrBit(FOC_CR2, F5SEG);
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        SetBit(FOC_CR2, F5SEG);
        #endif
        // 采样配置
        #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脉宽
        // SVPWM 配置
        #if (SVPMW_Mode == SVPWM_7_Segment)
        ClrBit(FOC_CR2, F5SEG);
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        SetBit(FOC_CR2, F5SEG);
        #endif
        // 采样配置
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        ClrBit(FOC_CR2, DSS);
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        SetBit(FOC_CR2, DSS);
        #endif
    }
    #endif
    // 写入电流偏置
    #if (CalibENDIS)
    {
        if (mcCurOffset.OffsetFlag)
        {
            #if (Shunt_Resistor_Mode == Single_Resistor)
            SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
            FOC_CSO = mcCurOffset.Iw_busOffset;
            #elif (Shunt_Resistor_Mode == Double_Resistor)
            SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
            FOC_CSO  = mcCurOffset.IuOffset;
            SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
            FOC_CSO  = mcCurOffset.IvOffset;
            #elif (Shunt_Resistor_Mode == Three_Resistor)
            {
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
                FOC_CSO = mcCurOffset.IuOffset;
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
                FOC_CSO = mcCurOffset.IvOffset;
                SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
                FOC_CSO = mcCurOffset.Iw_busOffset;
            }
            #endif
        }
    }
    #endif
    SetBit(DRV_CR, DRVEN);
    SetBit(DRV_CR, OCS);
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void Motor_Align(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 预定位函数，当无逆风判断时，采用预定位固定初始位置;当有逆风判断时，采用预定位刹车
    /*---------------------------------------------------------------------------*/
void Motor_Align(void)
{
    if (McStaSet.SetFlag.AlignSetFlag == 0)
    {
        McStaSet.SetFlag.AlignSetFlag = 1;
            // FOC初始化
            FOC_Init();
            // 配置预定位的电流、KP、KI
            FOC_IDREF = 0;
            FOC_IQREF = 0;
            FOC_DKP   = DQKP_Alignment;
            FOC_DKI   = DQKI_Alignment;
            FOC_QKP   = DQKP_Alignment;
            FOC_QKI   = DQKI_Alignment;
            FOC_EKP   = OBSW_KP_GAIN;
            FOC_EKI   = OBSW_KI_GAIN;
            // 配置预定位角度
            #if (EstimateAlgorithm == SMO)
            FOC__ETHETA   = FOC__THETA - 4836;
            #elif (EstimateAlgorithm == PLL)
            FOC__ETHETA   = FOC__THETA;
            #endif
            DRV_CMR |= 0x03F;
            MOE = 1;
    }
    
    if (mcFocCtrl.State_Count > (AlignmentHoldTime1 + AlignmentHoldTime2)) 
    {
        mcFocCtrl.CurrentAlignStatus = 0;
        FOC__THETA = Align_Angle1;
        FOC_IQREF = ID_Align_CURRENT_End * (Align_Time - mcFocCtrl.State_Count) / AlignmentRampTime;
    }
    else if (mcFocCtrl.State_Count > AlignmentHoldTime2)
    {
        mcFocCtrl.CurrentAlignStatus = 1;
        FOC__THETA = Align_Angle1;
        FOC_IQREF = ID_Align_CURRENT_End;
    }
    else if (mcFocCtrl.State_Count > 0)
    {
        mcFocCtrl.CurrentAlignStatus = 2;
        FOC__THETA = Align_Angle2;
        FOC_IQREF = ID_Align_CURRENT_End;
    }
    else
    { mcState = mcStart; }
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void Motor_Open(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 开环启动的参数配置
    /*---------------------------------------------------------------------------*/
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
    #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);
    #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);
    #endif
    // 不同启动方式下，切换到MCRUN状态
    #if (Open_Start_Mode == Open_Start)
    
    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)
    mcFocCtrl.State_Count = 2600;
    mcState = mcRun;
    #elif (Open_Start_Mode == Omega_Start)
    mcFocCtrl.State_Count = 1200;
    mcState = mcRun;
    #endif
	// Q轴启动电流
    FOC_IQREF = IQ_Start_CURRENT ;
}





/**
    @function     Get_Current_Offset
    @brief        采集电流偏置
    @date         2025-11-03
*/
void Get_Current_Offset(void)
{
    if (!mcCurOffset.OffsetFlag)
    {
        SetBit(ADC_CR, ADCBSY);             // 使能ADC
        
        while (ReadBit(ADC_CR, ADCBSY));
        
        mcCurOffset.IuOffsetSum     += ((ADC0_DR & 0x7ff8));
        mcCurOffset.IuOffset         = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum     -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum     += ((ADC1_DR & 0x7ff8));
        mcCurOffset.IvOffset         = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum     -= mcCurOffset.IvOffset;
        mcCurOffset.Iw_busOffset = mcCurOffset.IvOffset;
        mcCurOffset.OffsetCount++;
        
        if (mcCurOffset.OffsetCount > 1000)
        {
            if (((mcCurOffset.IuOffset > 19959) || (mcCurOffset.IuOffset < 10107)) ||
                ((mcCurOffset.IvOffset > 19959) || (mcCurOffset.IvOffset < 10107)))
            {
                mcFaultSource = FaultIbusOffset;//进入偏置电压错误保护
                FaultProcess();
            }
            else
            {
                mcCurOffset.OffsetFlag = 1;
            }
        }
    }
}