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

/**
 * @function     FocDetectInit
 * @brief        顺逆风参数初始化
 * @param[in]    None
 * @return       None
 * @date         2024-08-26
*/
void FocDetectInit(void)
{
    MOE         = 0;
    FOC_Init();                     // FOC的初始化
    FOC_DKP = DQKP_TailWind;        // 顺逆风的电流环KP
    FOC_DKI = DQKI_TailWind;        // 顺逆风的电流环KI
    FOC_QKP = DQKP_TailWind;        // 顺逆风的电流环KP
    FOC_QKI = DQKI_TailWind;        // 顺逆风的电流环KI
    FOC_EKP = OBSW_KP_GAIN_WIND;    // 顺逆风速度估算的KP
    FOC_EKI = OBSW_KI_GAIN_WIND;    // 顺逆风速度估算的KI
    FOC_OMEKLPF = SPEED_KLPF_WIND;  // 顺逆风下的速度滤波系数
    SetBit(FOC_CR1, ANGM);          // 估算模式
    FOC_IDREF = 0;                  // D轴输出
    FOC_IQREF = 0;
    MOE = 1;                        // 打开MOE
}

/**
 * @function     FOCCloseLoopStart
 * @brief        顺风启动
 * @param[in]    None
 * @return       None
 * @date         2024-08-26
*/
void FOCCloseLoopStart(void)
{
    // 配置启动的参数，Omega模式
    FOC_EFREQACC    = MOTOR_OMEGA_RAMP_ACC;
    FOC_EFREQMIN    = MOTOR_OMEGA_RAMP_MIN;
    FOC_EFREQHOLD   = MOTOR_OMEGA_RAMP_END;
    SetBit(FOC_CR1, EFAE);                         // 估算器强制输出
    ClrBit(FOC_CR1, RFAE);                         // 禁止强拉
    SetBit(FOC_CR1, ANGM);                         // 估算模式
    // 电流环的PI和输出限赋值
    FOC_DKP         = DKP;
    FOC_DKI         = DKI;
    FOC_QKP         = QKP;
    FOC_QKI         = QKI;
    FOC_DMAX        = DOUTMAX;
    FOC_DMIN        = DOUTMIN;
    FOC_QMAX        = QOUTMAX;
    FOC_QMIN        = QOUTMIN;
    /*********PLL或SMO**********/
    #if (EstimateAlgorithm == SMO)
    {
        //根据不同转速确启动的ATO_BW值
        FOC_EKP               = OBSW_KP_GAIN_RUN3;
        FOC_EKI               = OBSW_KI_GAIN_RUN3;
        mcFocCtrl.IqRef     = IQ_Start_CURRENT;
        mcFocCtrl.State_Count = 800;
    }
    #elif (EstimateAlgorithm == PLL)
    mcFocCtrl.IqRef = IQ_RUN_CURRENT;
    #endif
    FOC_OMEKLPF = SPEED_KLPF;
    FOC_IDREF = ID_RUN_CURRENT;         // D轴启动电流
    FOC_IQREF = mcFocCtrl.IqRef;        // Q轴启动电流
    PI3_UKH = mcFocCtrl.IqRef;
    mcState = mcRun;
    mcFocCtrl.CtrlMode = 0;
}


/**
 * @function     FOC_TailWindDealwith
 * @brief        None
 * @param[in]    None
 * @return       None
 * @date         2024-08-26
*/
void FOC_TailWindDealwith(void)
{
    if (mcFocCtrl.SpeedFlt < S_Value(500))
    {
        MOE = 0;
        ClrBit(DRV_CR, FOCEN);                      //关闭FOC
        McStaSet.SetFlag.PosiCheckSetFlag = 0;
        mcFocCtrl.mcPosCheckAngle = 0;              // 角度赋初值
    }
}