/* --------------------------- (C) COPYRIGHT 2021 Fortiortech ShenZhen -----------------------------
    File Name      : MotorFiledWeaken.c
    Author         : Fortiortech  Appliction Team
    Version        : V1.0
    Date           : 2021-12-08
    Description    : This file contains motor filed weaken function used for Motor Control.
----------------------------------------------------------------------------------------------------  
                                       All Rights Reserved
------------------------------------------------------------------------------------------------- */

#include <FU68xx_2.h>
#include <Myproject.h>



FieldWeakeningTypeDef xdata mcFiledWeaken;



/* -------------------------------------------------------------------------------------------------
    Function Name  : FiledWeakenInit
    Description    : 弱磁函数初始化
    Date           : 2021-12-08
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void FiledWeakenInit(void)
{

    PI3_EK = 0;
    PI3_EK1 = 0;
				
	  PI3_KP = MotorFiledWeakenKp;
    PI3_KI = MotorFiledWeakenKi;

    PI3_UKH = 0;
    PI3_UKL = 0;

    PI3_UKMAX = 0;
    PI3_UKMIN = _Q15(-45.0/180.0);
    mcFiledWeaken.WeakenUsRef = MotorFiledWeakenUs >> 1;
}


/* -------------------------------------------------------------------------------------------------
    Function Name  : FiledSqrtUdUq
    Description    : 弱磁的时候计算Ud和Uq平方和开方Us
    Date           : 2021-12-08
    Parameter      : ud: [输入/出] 
**			 uq: [输入/出] 
------------------------------------------------------------------------------------------------- */
uint16 FiledSqrtUdUq(int16 ud, int16 uq)
{
    
    SCAT1_COS = ud;      //mcFiledWeaken.WeakenRunUd;
    SCAT1_SIN = uq;      //mcFiledWeaken.WeakenRunUq
    SMDU_RunBlock(1, ATAN);
    return SCAT1_RES1;
}


/* -------------------------------------------------------------------------------------------------
    Function Name  : GetFiledWeakenTheta
    Description    : 通过PI运算获取弱磁的角度
    Date           : 2021-12-08
    Parameter      : None
------------------------------------------------------------------------------------------------- */
int16 GetFiledWeakenTheta(void)
{
    PI3_EK =  mcFiledWeaken.WeakenUsRef - mcFiledWeaken.WeakenUsFed;                  //填入EK
    SMDU_RunBlock(3, PI);
    return PI3_UKH;
}



/* -------------------------------------------------------------------------------------------------
    Function Name  : GetFiledWeakenIqId
    Description    : 通过弱磁角度得到Iq和Id的参考值
    Date           : 2021-12-08
    Parameter      : Theta: [输入/出] 
**			 Is: [输入/出] 
------------------------------------------------------------------------------------------------- */
void GetFiledWeakenIqId(int16 Theta, int16 Is)
{
    SCAT1_SIN = 0;
    SCAT1_COS = Is;
    SCAT1_THE = Theta;
    SMDU_RunBlock(1, SIN_COS);
    mcFiledWeaken.IqRef = SCAT1_RES1;
    mcFiledWeaken.IdRef = SCAT1_RES2;
}


/* -------------------------------------------------------------------------------------------------
    Function Name  : FileWeakenControl
    Description    : 弱磁控制
    Date           : 2021-12-08
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void FileWeakenControl(void)
{
    mcFiledWeaken.WeakenRunUd = LPFFunction(FOC__UD, mcFiledWeaken.WeakenRunUd, 10);
    mcFiledWeaken.WeakenRunUq = LPFFunction(FOC__UQ, mcFiledWeaken.WeakenRunUq, 10);
    mcFiledWeaken.WeakenUsFed = FiledSqrtUdUq((mcFiledWeaken.WeakenRunUd >> 1), (mcFiledWeaken.WeakenRunUq >> 1));
    mcFiledWeaken.WeakenTheta = GetFiledWeakenTheta();  
    mcFiledWeaken.mcIsref = mcFocCtrl.IqRef;
    GetFiledWeakenIqId(mcFiledWeaken.WeakenTheta, mcFiledWeaken.mcIsref);
    FOC_IQREF = mcFiledWeaken.IqRef;
    FOC_IDREF = mcFiledWeaken.IdRef;
}