/*  --------------------------- (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_5.h"
#include <Myproject.h>

VOLCOMP  xdata VoltageComp;
/*  -------------------------------------------------------------------------------------------------
    Function Name  : VoltageCompensation
    Description    : ΅ηΡΉ²Ή³₯
    Date           : 2021-12-08
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void VoltageCompensation(void)
{
    if ((mcFocCtrl.CtrlMode == 1) && (mcFaultSource == FaultNoSource) && (mcFocCtrl.SpeedFlt > _Q15(2000 / MOTOR_SPEED_BASE)))
    {
        //SetBit(ADC_CR, ADCBSY);
        while (ReadBit(ADC_CR, ADCBSY));
        
        VoltageComp.testUq = FOC__UQ;
        VoltageComp.testUq1 = VoltageComp.testUq >> 4 ;
        VoltageComp.mcDcbus          = ADC2_DR;
        VoltageComp.mcDcbuschazhi    = VoltageComp.mcDcbus - mcFocCtrl.mcDcbus_min;
        DIV0_DAH = VoltageComp.testUq1;           // rw--
        DIV0_DAL = 0;                           // rw--
        DIV0_DB  = mcFocCtrl.mcDcbus_min;        // rw--
        SMDU_RunBlock(0, DIV);                  // rw--
        VoltageComp.testUq2 = DIV0_DQL;           // rw--
        MUL0_MA = VoltageComp.testUq2;            // rw--
        MUL0_MB = mcFocCtrl.mcDcbus_min + mcFocCtrl.mcDcbus_chazhi + VoltageComp.mcDcbuschazhi; // rw--
        SMDU_RunBlock(0, S1MUL);                       //
        VoltageComp.testUq3 = MUL0_MCH;                  // rw--
        //              if(VoltageComp.testUq3>=4095)                    // rw--
        //              {
        //                      VoltageComp.testUq3=4095;
        //              }
        VoltageComp.testUq4 = (VoltageComp.testUq3 << 3) - VoltageComp.testUq;  //
        
        if (VoltageComp.testUq4 >= 3000)
        {VoltageComp.testUq4 = 3000;}
        
        if (VoltageComp.testUq4 < -3000)
        {VoltageComp.testUq4 = -3000;}
        
        FOC_UQCPS = VoltageComp.testUq4;                                     //
    }
}