#include "FU68xx_5.h"
#include <Myproject.h>

CurrentDecoupledControlTypeDef xdata mcCurrentDecoupledControl;
DQCurrentPITypeDef  xdata DQCurrentPI;
//void CurrentDecoupledControlFunctionInit(void)
//{
//    memset(&mcCurrentDecoupledControl,0, sizeof(CurrentDecoupledControlTypeDef));
//    mcCurrentDecoupledControl.DCurrentDecoupledControlK =  D_CURRENT_DECOUPLED_CONTROL_K;
//    mcCurrentDecoupledControl.QCurrentDecoupledControlK1 = Q_CURRENT_DECOUPLED_CONTROL_K1;
//    mcCurrentDecoupledControl.QCurrentDecoupledControlK2 = Q_CURRENT_DECOUPLED_CONTROL_K2;
//}

void CurrentDecoupledControlFunctionInit(float MotorLd, float MotorLq, float MotorPsi, float VoltageBase, float CurrentBase, float OmegaRadSec)
{
    memset(&mcCurrentDecoupledControl, 0, sizeof(CurrentDecoupledControlTypeDef));
    mcCurrentDecoupledControl.DCurrentDecoupledControlK =  _Q11(OmegaRadSec * MotorLq * CurrentBase / VoltageBase);
    mcCurrentDecoupledControl.QCurrentDecoupledControlK1 = _Q13(OmegaRadSec * MotorPsi / VoltageBase);
    mcCurrentDecoupledControl.QCurrentDecoupledControlK2 = _Q11(OmegaRadSec * MotorLd * CurrentBase / VoltageBase);
    mcCurrentDecoupledControl.QCurrentDecoupledControlK3 = _Q11( RS * CurrentBase / VoltageBase);
}


void CurrentDecoupledControlFunction(void)//DQ轴解耦处理
{
    //    if(LowSpeedObserver.LowSpeedObserverToAoObserverFlag)
    {
        mcCurrentDecoupledControl.Omega = mcFocCtrl.SpeedFlt1;//Q15格式
    }
    //      else
    //    {
    //        mcCurrentDecoupledControl.Omega = LowSpeedObserver.LowSpeedObserverOmegaFlt;//Q15格式
    //    }
    mcCurrentDecoupledControl.DCurrent0 = FOC__ID;//Q15格式
    mcCurrentDecoupledControl.QCurrent0 = FOC__IQ;//Q15格式
    mcCurrentDecoupledControl.DCurrent   =  LPF_realize2(mcCurrentDecoupledControl.DCurrent0, mcCurrentDecoupledControl.DCurrent, LPF_K(1.0));
    mcCurrentDecoupledControl.QCurrent   =  LPF_realize2(mcCurrentDecoupledControl.QCurrent0, mcCurrentDecoupledControl.QCurrent, LPF_K(1.0));
    //    mcCurrentDecoupledControl.DcVoltage = mcFocCtrl.mcDcbusFlt;//FOC__UDCFLT;//Q15格式
    mcCurrentDecoupledControl.DcVoltage = FOC__UDCFLT;//FOC__UDCFLT;//Q15格式
    MUL0_MA = mcCurrentDecoupledControl.Omega;//Q15格式
    MUL0_MB = mcCurrentDecoupledControl.QCurrent;//Q15格式
    SMDU_RunBlock(0, S1MUL);
    MUL0_MA = MUL0_MCH;//Q15格式
    MUL0_MB = mcCurrentDecoupledControl.DCurrentDecoupledControlK;//Q11格式
    SMDU_RunBlock(0, S1MUL);
    mcCurrentDecoupledControl.DVoltageComponent = MUL0_MCH << 4;//Q15格式
    
    if (mcCurrentDecoupledControl.DVoltageComponent < 0) //硬件除法器不可以为负，空载启动时转速超调可能导致Q轴电流为负，导致电流异常
    {
        mcCurrentDecoupledControl.DVoltageComponentAbs = -mcCurrentDecoupledControl.DVoltageComponent;
    }
    else
    {
        mcCurrentDecoupledControl.DVoltageComponentAbs = mcCurrentDecoupledControl.DVoltageComponent;
    }
    
    DIV0_DAH = mcCurrentDecoupledControl.DVoltageComponentAbs;//Q31格式
    DIV0_DAL = 0;
    DIV0_DB = mcCurrentDecoupledControl.DcVoltage;//Q15格式
    SMDU_RunBlock(0, DIV);
    
    if (mcCurrentDecoupledControl.DVoltageComponent < 0)
    {
        mcCurrentDecoupledControl.DDutyQ15 = DIV0_DQL >> 1;//Q15格式，该分量为负
    }
    else
    {
        mcCurrentDecoupledControl.DDutyQ15 = -(DIV0_DQL >> 1);//Q15格式，该分量为负
    }
    
    MUL0_MA = mcCurrentDecoupledControl.Omega;//Q15格式
    MUL0_MB = mcCurrentDecoupledControl.QCurrentDecoupledControlK1;//Q13格式
    SMDU_RunBlock(0, S1MUL);
    mcCurrentDecoupledControl.QVoltageComponent1 = MUL0_MCH << 2;//Q15格式
    MUL0_MA = mcCurrentDecoupledControl.Omega;//Q15格式
    MUL0_MB = mcCurrentDecoupledControl.QCurrentDecoupledControlK2;//Q11格式
    SMDU_RunBlock(0, S1MUL);
    MUL0_MA = MUL0_MCH;//Q11格式
    MUL0_MB = mcCurrentDecoupledControl.DCurrent;//Q15格式
    SMDU_RunBlock(0, S1MUL);
    mcCurrentDecoupledControl.QVoltageComponent2 = MUL0_MCH << 4;//Q15格式
    mcCurrentDecoupledControl.QVoltageComponent = mcCurrentDecoupledControl.QVoltageComponent1 + mcCurrentDecoupledControl.QVoltageComponent2;
    
    if (mcCurrentDecoupledControl.QVoltageComponent < 0) //硬件除法器不可以为负
    {
        mcCurrentDecoupledControl.QVoltageComponentAbs = -mcCurrentDecoupledControl.QVoltageComponent;
    }
    else
    {
        mcCurrentDecoupledControl.QVoltageComponentAbs = mcCurrentDecoupledControl.QVoltageComponent;
    }
    
    DIV0_DAH = mcCurrentDecoupledControl.QVoltageComponentAbs;//Q31格式
    DIV0_DAL = 0;
    DIV0_DB = mcCurrentDecoupledControl.DcVoltage;//Q15格式
    SMDU_RunBlock(0, DIV);
    mcCurrentDecoupledControl.QDutyQ15 = DIV0_DQL >> 1;//Q15格式
    
    if (mcCurrentDecoupledControl.QVoltageComponent < 0)
    {
        mcCurrentDecoupledControl.QDutyQ15 = -mcCurrentDecoupledControl.QDutyQ15;
    }
    
    FOC_UDCPS = mcCurrentDecoupledControl.DDutyQ15;
    FOC_UQCPS = mcCurrentDecoupledControl.QDutyQ15;
}

void DQCurrentKpKiCalInit(float MotorLd, float MotorLq, float Ts, float VoltageBase, float CurrentBase, float hValue)
{
    DQCurrentPI.DCurrentKik = _Q11(MotorLd * (hValue + 1.0) / 4.5 / hValue / hValue / Ts);
    DQCurrentPI.DCurrentKpk = MotorLd * (hValue + 1.0) / 3.0 / hValue / Ts;
    DQCurrentPI.QCurrentKik = _Q11(MotorLq * (hValue + 1.0) / 4.5 / hValue / hValue / Ts);
    DQCurrentPI.QCurrentKpk = MotorLq * (hValue + 1.0) / 3.0 / hValue / Ts;
    DQCurrentPI.KpwmkValue = VoltageBase / CurrentBase;
}
void DQCurrentKpKiCal(void)
{
    MUL1_MA = DQCurrentPI.KpwmkValue;
    MUL1_MB = FOC__UDCFLT;//Q15格式
    SMDU_RunBlock(1, S1MUL);
    DQCurrentPI.KpwmValue = MUL1_MCH;
    DIV1_DAH = 0;
    DIV1_DAL = DQCurrentPI.DCurrentKik;//Q11格式
    DIV1_DB = DQCurrentPI.KpwmValue;
    SMDU_RunBlock(1, DIV);
    DQCurrentPI.DCurrentKi = DIV1_DQL << 4;//Q15格式
    DIV1_DAH = 0;
    DIV1_DAL = DQCurrentPI.QCurrentKik;//Q11格式
    DIV1_DB = DQCurrentPI.KpwmValue;
    SMDU_RunBlock(1, DIV);
    DQCurrentPI.QCurrentKi = DIV1_DQL << 4;//Q15格式
    DIV1_DAH = DQCurrentPI.DCurrentKpk;//Q16格式
    DIV1_DAL = 0;
    DIV1_DB = DQCurrentPI.KpwmValue << 4;//Q4格式
    SMDU_RunBlock(1, DIV);
    
    if ((DIV1_DQH != 0) || (DIV1_DQL > 32767))
    {
        DQCurrentPI.DCurrentKp = 32767;//Q12格式
    }
    else
    {
        DQCurrentPI.DCurrentKp = DIV1_DQL;//Q12格式
    }
    
    DIV1_DAH = DQCurrentPI.QCurrentKpk;//Q16格式
    DIV1_DAL = 0;
    DIV1_DB = DQCurrentPI.KpwmValue << 4;//Q4格式
    SMDU_RunBlock(1, DIV);
    
    if ((DIV1_DQH != 0) || (DIV1_DQL > 32767))
    {
        DQCurrentPI.QCurrentKp = 32767;//Q12格式
    }
    else
    {
        DQCurrentPI.QCurrentKp = DIV1_DQL;//Q12格式
    }
    
    FOC_QKP       = DQCurrentPI.QCurrentKp;//Q轴电流环参数
    FOC_QKI       = DQCurrentPI.QCurrentKi;
    FOC_DKP        = DQCurrentPI.DCurrentKp;//D轴电流环参数
    FOC_DKI        = DQCurrentPI.DCurrentKi;
    //    FOC_DQKP       = DQKPStart;//Q轴电流环参数
    //    FOC_DQKI       = DQKIStart;
}
//void DQCurrentKpKiCal(void)
//{
//    MUL1_MA = Kpwmk;
//    MUL1_MB = FOC__UDCFLT;//Q15格式
//    SMDU_RunBlock(1,S1MUL);
//    DQCurrentPI.KpwmValue = MUL1_MCH;
//
//    DIV1_DAH = DCurrentKi_K;//Q16格式
//    DIV1_DAL = 0;
//    DIV1_DB = DQCurrentPI.KpwmValue;
//    SMDU_RunBlock(1,DIV);
//    DQCurrentPI.DCurrentKi = DIV1_DQL >> 1;//Q15格式
//
//    DIV1_DAH = QCurrentKi_K;//Q16格式
//    DIV1_DAL = 0;
//    DIV1_DB = DQCurrentPI.KpwmValue;
//    SMDU_RunBlock(1,DIV);
//    DQCurrentPI.QCurrentKi = DIV1_DQL >> 1;//Q15格式
//
//    DIV1_DAH = DCurrentKp_K;//Q16格式
//    DIV1_DAL = 0;
//    DIV1_DB = DQCurrentPI.KpwmValue << 4;//Q4格式
//    SMDU_RunBlock(1,DIV);
//
//    if((DIV1_DQH != 0) || (DIV1_DQL > 32767))
//    {
//        DQCurrentPI.DCurrentKp = 32767;//Q12格式
//    }else
//    {
//        DQCurrentPI.DCurrentKp = DIV1_DQL;//Q12格式
//    }
//
//    DIV1_DAH = QCurrentKp_K;//Q16格式
//    DIV1_DAL = 0;
//    DIV1_DB = DQCurrentPI.KpwmValue << 4;//Q4格式
//    SMDU_RunBlock(1,DIV);
//
//    if((DIV1_DQH != 0) || (DIV1_DQL > 32767))
//    {
//        DQCurrentPI.QCurrentKp = 32767;//Q12格式
//    }else
//    {
//        DQCurrentPI.QCurrentKp = DIV1_DQL;//Q12格式
//    }
//
//    FOC_DQKP       = DQCurrentPI.QCurrentKp;//Q轴电流环参数
//    FOC_DQKI       = DQCurrentPI.QCurrentKi;
//
//    FOC_DKP        = DQCurrentPI.DCurrentKp;//D轴电流环参数
//    FOC_DKI        = DQCurrentPI.DCurrentKi;
//
////    FOC_DQKP       = DQKPStart;//Q轴电流环参数
////    FOC_DQKI       = DQKIStart;

//}