/*  -------------------------- (C) COPYRIGHT 2020 Fortiortech ShenZhen ---------------------------*/
/*  File Name      : FOCTailDect.c
    /*  Author         : Fortiortech  Appliction Team
    /*  Version        : V1.0
    /*  Date           : 2021-11-07
    /*  Description    : This file contains Foc tailwind detection function used for Motor Control.
    /*  ----------------------------------------------------------------------------------------------*/
/*                                     All Rights Reserved
    /*  ----------------------------------------------------------------------------------------------*/

#include <MyProject.h>

/* Private typedef ------------------------------------------------------------------------------*/
/* Private define -------------------------------------------------------------------------------*/
/* Private macro --------------------------------------------------------------------------------*/
/* Private variables ----------------------------------------------------------------------------*/

/* Private function prototypes ------------------------------------------------------------------*/
/* Private functions ----------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*/
/*  Name     :   void FocDetect(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 顺逆风参数初始化
    /*---------------------------------------------------------------------------*/
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
}



/*---------------------------------------------------------------------------*/
/*  Name     :   void FOCCloseLoopStart(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 顺风启动
    /*---------------------------------------------------------------------------*/
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 //end    EstimateAlgorithm
    /*estimate parameter set*/
    FOC_OMEKLPF = SPEED_KLPF;
    FOC_IDREF   = ID_RUN_CURRENT;     // D轴启动电流
    FOC_IQREF   = mcFocCtrl.IqRef;  // Q轴启动电流
    PI3_UKH     = mcFocCtrl.IqRef;
    mcState            = mcRun;
    mcFocCtrl.CtrlMode = 0;
}


void FOC_TailWindDealwith(void)
{
    if (mcFocCtrl.SpeedFlt < S_Value(500))
    {
        MOE = 0;
        ClrBit(DRV_CR, FOCEN);    //关闭FOC
        McStaSet.SetFlag.PosiCheckSetFlag = 0;
        mcFocCtrl.mcPosCheckAngle         = 0;         // 角度赋初值
    }
}