#include <Myproject.h>

CurrentOffset xdata mcCurOffset;

/*---------------------------------------------------------------------------*/
/*  Name     :   void FOC_Init(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: mcInit状态下，对FOC的相关寄存器进行配置,先清理寄存器，后配置，最后使能
    /*---------------------------------------------------------------------------*/
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;//ID_Align_CURRENT;
        FOC_IQREF = 0;//IQ_Align_CURRENT;
        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;
        //        /*配置预定位角度*/
        //        FOC__THETA  = 0;//Align_Theta;
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO)
        FOC__ETHETA   = FOC__THETA - 4836;
        #elif (EstimateAlgorithm == PLL)
        FOC__ETHETA   = FOC__THETA;
        #endif //end    EstimateAlgorithm
        /*使能输出*/
        DRV_CMR |= 0x0abF;      // U、V、W相上下互补输出 FTC6805
        MOE = 1;
    }
    
    if (mcFocCtrl.State_Count > (AlignmentHoldTime1 + AlignmentHoldTime2)) /* Ramp 1s */
    {
        mcFocCtrl.CurrentAlignStatus = 0;
        FOC__THETA      = Align_Angle1;
        FOC_IDREF       = ID_Align_CURRENT_End * (Align_Time - mcFocCtrl.State_Count) / AlignmentRampTime;
    }
    else if (mcFocCtrl.State_Count > AlignmentHoldTime2)             /* Hold 900ms*/
    {
        mcFocCtrl.CurrentAlignStatus = 1;
        FOC__THETA      = Align_Angle1;
        FOC_IDREF       = ID_Align_CURRENT_End;
    }
    else if (mcFocCtrl.State_Count > 0)             /* Hold */
    {
        mcFocCtrl.CurrentAlignStatus = 2;
        FOC__THETA      = Align_Angle2;
        FOC_IDREF       = 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;
    /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
    FOC_IDREF = ID_Start_CURRENT;                         // D轴启动电流
    mcFocCtrl.mcIqref = IQ_Start_CURRENT;                 // Q轴启动电流
    mcFocCtrl.iiqq = IQ_Start_CURRENT;
    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)              // Omega 启动
    FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
    FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
    FOC_EFREQHOLD = Motor_Omega_Ramp_End;
    SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | ANGM);
    #if  (IFFDebugg==1)
    {
        /*估算器禁止输出*/
        ClrBit(FOC_CR1, EFAE);                      // 禁止估算器强制输出
        ClrBit(FOC_CR1, RFAE);                      // 使能强拉
        ClrBit(FOC_CR1, ANGM);                      // 禁止估算器输出
    }
    #endif
    #elif (Open_Start_Mode == Open_Start)
    FOC_RTHEACC      = Motor_Open_Ramp_ACC;      // 爬坡函数的初始加速度
    FOC__RTHESTEP    = Motor_Open_Ramp_Min;      // 0.62 degree acce speed
    FOC_RTHECNT      = MOTOR_OPEN_ACC_CNT;       // acce time
    SetReg(FOC_CR1, EFAE | RFAE | ANGM, RFAE);
    #elif (Open_Start_Mode == Open_Omega_Start)
    FOC_RTHEACC   = Motor_Open_Ramp_ACC;         // 爬坡函数的初始加速度
    FOC__RTHESTEP = Motor_Open_Ramp_Min;         // 0.62 degree acce speed
    FOC_RTHECNT   = MOTOR_OPEN_ACC_CNT;          // acce time
    FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
    FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
    FOC_EFREQHOLD = Motor_Omega_Ramp_End;
    SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | RFAE | ANGM);
    #endif //end Open_Start_Mode
    /*不同启动方式下，切换到MCRUN状态*/
    #if (Open_Start_Mode == Open_Start)         //OPEN状态启动时拖动多次
    
    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;                            // 估算器里的PI的KP
    FOC_EKI = OBSW_KI_GAIN_RUN4;                            // 估算器里的PI的KI
    #elif (Open_Start_Mode == Open_Omega_Start)
    mcFocCtrl.State_Count = 2600;
    mcState = mcRun;
    #elif (Open_Start_Mode == Omega_Start)
    /*********PLL或SMO**********/
    mcFocCtrl.State_Count = 1200;
    mcState   = mcRun;
    #endif
    FOC_IQREF = mcFocCtrl.mcIqref;                          // Q轴启动电流
}





/**
    @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;
            }
        }
    }
}