//#############################################################################
// $Copyright:
// Copyright (C) 2017-2023 Texas Instruments Incorporated - http://www.ti.com/
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//   Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.
//
//   Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the
//   distribution.
//
//   Neither the name of Texas Instruments Incorporated nor the names of
//   its contributors may be used to endorse or promote products derived
//   from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// $
//#############################################################################


//! \file   /solutions/tidm_02010_dmpfc/common/source/pfc_ctrl.c
//!
//! \brief  This project is used to control two motor and pfc with one MCU
//!         Support for dmpfc board with F28002x/F28003x/F280013x
//!

//
// include the related header files
//
#include "sys_settings.h"
#include "sys_main.h"

#pragma CODE_SECTION(pfcCtrlISR, ".TI.ramfunc");

#pragma INTERRUPT(pfcCtrlISR, HPI)

// the globals
HAL_PFC_Handle halPFCHandle;
HAL_PFC_Obj    halPFC;
#pragma DATA_SECTION(halPFC, "pfc_data");
#pragma DATA_SECTION(halPFCHandle, "pfc_data");

volatile PFC_Vars_t pfcVars;
#pragma DATA_SECTION(pfcVars, "pfc_data");

volatile PFC_CtrlVars_t pfcCtrlVars;
#pragma DATA_SECTION(pfcCtrlVars, "pfc_data");

HAL_PFC_ADCData_t adcDataPFC;
#pragma DATA_SECTION(adcDataPFC, "pfc_data");

HAL_PFC_PWMData_t pwmDataPFC;
#pragma DATA_SECTION(pwmDataPFC, "pfc_data");

DCL_DF22 gi;
DCL_PI   gv;
#pragma DATA_SECTION(gi, "pfc_data");
#pragma DATA_SECTION(gv, "pfc_data");

// Sine analyzer block for RMS Volt, Curr and Power measurements
POWER_MEAS_FAST measPower;
#pragma DATA_SECTION(measPower, "pfc_data");

#if defined(NOTCH_FILTER)
// notch filter
FILTER_NOTCH_Obj    filterNotch;
FILTER_NOTCH_Handle filterNotchHandle;     //!< the handle for the notch filter
#pragma DATA_SECTION(filterNotch, "pfc_data");
#pragma DATA_SECTION(filterNotchHandle, "pfc_data");

FILTER_COEFF_Obj    filterCoeff;
FILTER_COEFF_Handle filterCoeffHandle;     //!< the handle for the filter coefficient
#pragma DATA_SECTION(filterCoeff, "pfc_data");
#pragma DATA_SECTION(filterCoeffHandle, "pfc_data");
#endif  // NOTCH_FILTER


// the control variables for motor 1

void initPFCCtrlParameters(void)
{
    // initialize the driver
    halPFCHandle = HAL_PFC_init(&halPFC, sizeof(halPFC));

    // sets up peripherals for PFC
    HAL_PFC_setParams(halPFCHandle);

    adcDataPFC.currentAC_sf = USER_PFC_ADC_IAC_PU_SF;
    adcDataPFC.voltageAC_sf = USER_PFC_ADC_VAC_PU_SF;
    adcDataPFC.voltageDC_sf = USER_PFC_ADC_VDC_PU_SF;

    pfcVars.lpfVdcCoeff = USER_LPF_VDC_COEFF;

    pfcVars.invRmsSqrVminOverVmax = USER_PFC_VOLTAGE_AC_IN_MIN_V / USER_PFC_ADC_FULL_SCALE_AC_VOLTAGE_V;
    pfcVars.invRmsSqrVmin = USER_PFC_VOLTAGE_AC_IN_MIN_V / USER_PFC_ADC_FULL_SCALE_AC_VOLTAGE_V;
    pfcVars.IacRefVmaxOverVmin = USER_PFC_VOLTAGE_AC_IN_MAX_V / USER_PFC_VOLTAGE_AC_IN_MIN_V;

    pfcVars.invRmsSqrCoef = (USER_PFC_VOLTAGE_AC_IN_MIN_V * USER_PFC_VOLTAGE_AC_IN_MAX_V) /
                            (USER_PFC_ADC_FULL_SCALE_AC_VOLTAGE_V * USER_PFC_ADC_FULL_SCALE_AC_VOLTAGE_V);

    pfcVars.VdcBusShutDown = USER_SHUTDOWN_VOLTAGE_PU;

    pfcVars.overCurrent_A = USER_PFC_OVER_CURRENT_A;
    pfcVars.IacPeak_pu = USER_PFC_PEAK_CURRENT_PU;

    pfcVars.IdcRef = 0.0f;

    pfcVars.IdcRefSwILOff = USER_PFC_IL_MODE_OFF_VREF;
    pfcVars.IdcRefSwILOn = USER_PFC_IL_MODE_ON_VREF;

    pfcVars.VdcSet_V = USER_PFC_VOLTAGE_TRAGET_V;
    pfcVars.VdcTrajTraget = USER_PFC_VOLTAGE_TRAGET_V / USER_PFC_ADC_FULL_SCALE_DC_VOLTAGE_V;
    pfcVars.VdcTrajInt = 0.0f;
    pfcVars.VdcTrajRate = USER_PFC_VOLTAGE_TRAJ_RATE_PU;

    pfcVars.dacCMPValH = 1024U;
    pfcVars.dacCMPValH = 0U;

    pfcVars.faultPFCMask.all = PFC_FAULT_MASK_SET;

    pfcVars.flagEnablePFC = false;
    pfcVars.flagEnableCurrentLoop = false;
    pfcVars.flagEnableVoltageLoop = false;
    pfcVars.flagFirstVoltageLoop = false;
    pfcVars.flagEnableChangeGain = true;

    pfcVars.flagEnableILMode = true;

    pfcCtrlVars.overVoltageFault_ACV = USER_OVER_VOLTAGE_FAULT_ACV;
    pfcCtrlVars.overVoltageNorm_ACV = USER_OVER_VOLTAGE_NORM_ACV;
    pfcCtrlVars.underVoltageFault_ACV = USER_UNDER_VOLTAGE_FAULT_ACV;
    pfcCtrlVars.underVoltageNorm_ACV = USER_UNDER_VOLTAGE_NORM_ACV;

    pfcCtrlVars.overVoltageFault_DCV = USER_OVER_VOLTAGE_FAULT_DCV;
    pfcCtrlVars.overVoltageNorm_DCV = USER_OVER_VOLTAGE_NORM_DCV;
    pfcCtrlVars.underVoltageFault_DCV = USER_UNDER_VOLTAGE_FAULT_DCV;
    pfcCtrlVars.underVoltageNorm_DCV = USER_UNDER_VOLTAGE_NORM_DCV;

    pfcCtrlVars.overCurrentFault_A = USER_PFC_OVER_CURRENT_A;
    pfcCtrlVars.overLoadFault_W = USER_PFC_OVER_LOAD_W;

    pfcCtrlVars.voltageAcFaultTimeSet = USER_PFC_VOLTAGE_AC_FAULT_TIME_SET;
    pfcCtrlVars.voltageDcFaultTimeSet = USER_PFC_VOLTAGE_DC_FAULT_TIME_SET;
    pfcCtrlVars.voltageDcFaultTimeSet = USER_PFC_OVER_LOAD_FAULT_TIME_SET;

    pfcCtrlVars.overCurrentFaultTimesSet = USER_PFC_OVER_CURRENT_FAULT_TIMES_SET;

    pfcVars.VdcBus_pu = 0.0f;

    pfcVars.VacComp_V = 180.0f;
    pfcVars.IacCompFactor = 1.0f;
    pfcVars.IdcmCompFactor = 1.0f;

    pfcVars.VacRmsGainThs_V = 200.0f;

    pfcVars.IdcRefGainThsHV = 0.45f;
    pfcVars.IdcRefGainThsLV = 0.22f;

    pfcVars.IdcRefMaxSet = 0.95f;       // 0~95% of maximum current(PU)
    pfcVars.dutyOutMaxSet = 0.90f;      // 0~90% of maximum duty cycle

    //sine analyzer initialization
    POWER_MEAS_FAST_reset(&measPower);

    measPower.sampleFreq = USER_PFC_MEASURE_FREQ_Hz;
    measPower.threshold  = USER_PFC_MEASURE_THRESHOLD_PU;

    measPower.nSamplesMax = USER_PFC_MEASURE_FREQ_Hz /
                            USER_UNIVERSAL_GRID_MIN_FREQ;

    measPower.nSamplesMin = USER_PFC_MEASURE_FREQ_Hz /
                            USER_UNIVERSAL_GRID_MAX_FREQ;

    DCL_resetPI(&gv);

    gv.Kp = (CNTL_2p2z_B0_10 - CNTL_2p2z_B1_10) * 1.0f;
    gv.Ki = (CNTL_2p2z_B0_10 + CNTL_2p2z_B1_10) * 1.0f;

    gv.Umax = pfcVars.IdcRefMaxSet;
    gv.Umin = 0.0f;

    DCL_resetDF22(&gi);

    gi.b2   = CNTL_2p2z_B2_3;
    gi.b1   = CNTL_2p2z_B1_3;
    gi.b0   = CNTL_2p2z_B0_3;
    gi.a2   = CNTL_2p2z_A2_3;
    gi.a1   = CNTL_2p2z_A1_3;

    pfcVars.indexCurrentGain = 3;

#if defined(NOTCH_FILTER)
    // sets noth filter
    pfcVars.FreqGrid_Hz  = USER_GRID_AC_FREQ_Hz;
    pfcVars.TsFilter_sec = USER_PFC_VOLTAGE_PERIOD_sec;

    filterNotchHandle = FILTER_NOTCH_init(&filterNotch, sizeof(filterNotch));
    filterCoeffHandle = FILTER_COEFF_init(&filterCoeff, sizeof(filterCoeff));

    FILTER_NOTCH_reset(filterNotchHandle);
    FILTER_COEFF_update(filterCoeffHandle,
                        pfcVars.TsFilter_sec, pfcVars.FreqGrid_Hz,
                        0.00001f, 0.1f);
#endif  // NOTCH_FILTER

    return;
}

void runPFCMainControl(void)
{
    // set the comparator value
    HAL_setPFCCMPSSDACValue(halPFCHandle,
                            pfcVars.dacCMPValH, pfcVars.dacCMPValL);

    if(HAL_getPFCPwmEnableStatus(halPFCHandle) == true)
    {
        if(HAL_getPFCTripFaults(halPFCHandle) != 0)
        {
            pfcVars.faultPFCNow.bit.moduleOverCurrent = 1;
        }
    }

    pfcVars.faultPFCPrev.all |= pfcVars.faultPFCNow.all;

    pfcVars.faultPFCUse.all = pfcVars.faultPFCNow.all &
            pfcVars.faultPFCMask.all;

    if(pfcVars.faultPFCUse.all != 0)
    {
        pfcVars.flagEnablePFC = false;

        pfcVars.VdcTrajTraget = 0.0f;
        pfcVars.VdcTrajInt = 0.0f;
        pfcVars.flagFirstVoltageLoop = true;
    }

    if(pfcVars.flagClearFaults == true)
    {
        HAL_clearPFCFaultStatus(halPFCHandle);

        pfcVars.faultPFCNow.all = 0;
        pfcVars.flagClearFaults = false;
    }

    pfcVars.IacRms_A = pfcVars.IacRms_pu * USER_PFC_ADC_FULL_SCALE_CURRENT_A;
    pfcVars.VacRms_V = pfcVars.VacRms_pu * USER_PFC_ADC_FULL_SCALE_AC_VOLTAGE_V;
    pfcVars.PowerRms_W = pfcVars.IacRms_A * pfcVars.VacRms_V;

    if(pfcVars.flagEnablePFC == true)
    {
        if(HAL_getPFCPwmEnableStatus(halPFCHandle) == false)
        {
#if(PFC_BUILDLEVEL >= PFC_LEVEL_2)
            // enable the PWM
            HAL_enablePFCPWM(halPFCHandle);
#elif(PFC_BUILDLEVEL >= PFC_LEVEL_1)
            // The PWM output is enable only when the dc bus power supply
            //         is very low in this build level
            if(pfcVars.VdcBus_V <= 16.5f)   // <16.5V
            {
                // enable the PWM
                HAL_enablePFCPWM(halPFCHandle);
            }
            else
            {
                pfcVars.flagEnablePFC = 0;

                // disable the PWM
                HAL_disablePFCPWM(halPFCHandle);
            }
#endif  // PFC_BUILDLEVEL
            // switch PFC mode between interleave and single phase
            if(pfcVars.flagEnableILMode == false)
            {
#if defined(SPPFC_CHA_EN)
                HAL_disableILPFCPowerChB(halPFCHandle);
#elif defined(SPPFC_CHB_EN)
                HAL_disableILPFCPowerChA(halPFCHandle);
#endif      // SPPFC_CHA_EN
            }
        }

        pfcVars.VdcTrajTraget = pfcVars.VdcSet_V * USER_PFC_VOLTAGE_DCINV_SF;
    }
    else
    {
        // disable the PWM
        HAL_disablePFCPWM(halPFCHandle);
    }

    return;
}

void runPFCMonitor(void)
{
    //Adaptive current loop control coefficients
    //if Input greater than 200V
    if(pfcVars.flagEnableChangeGain == true)
    {
        if(pfcVars.VacRms_V > pfcVars.VacRmsGainThs_V)
        {
            //Load higher than 50%
            if(pfcVars.IdcRef > pfcVars.IdcRefGainThsHV)
            {
                gi.b2  = CNTL_2p2z_B2_4;
                gi.b1  = CNTL_2p2z_B1_4;
                gi.b0  = CNTL_2p2z_B0_4;
                gi.a2  = CNTL_2p2z_A2_4;
                gi.a1  = CNTL_2p2z_A1_4;

                pfcVars.indexCurrentGain = 4;
            }
            else if(pfcVars.IdcRef > 0.35f)
            {
                gi.b2  = CNTL_2p2z_B2_5;
                gi.b1  = CNTL_2p2z_B1_5;
                gi.b0  = CNTL_2p2z_B0_5;
                gi.a2  = CNTL_2p2z_A2_5;
                gi.a1  = CNTL_2p2z_A1_5;

                pfcVars.indexCurrentGain = 5;
            }
            else if(pfcVars.IdcRef > 0.22f)
            {
                gi.b2  = CNTL_2p2z_B2_6;
                gi.b1  = CNTL_2p2z_B1_6;
                gi.b0  = CNTL_2p2z_B0_6;
                gi.a2  = CNTL_2p2z_A2_6;
                gi.a1  = CNTL_2p2z_A1_6;

                pfcVars.indexCurrentGain = 6;
            }
            //Load 20% to 10%
            else if(pfcVars.IdcRef > 0.1f)
            {
                gi.b2  = CNTL_2p2z_B2_7;
                gi.b1  = CNTL_2p2z_B1_7;
                gi.b0  = CNTL_2p2z_B0_7;
                gi.a2  = CNTL_2p2z_A2_7;
                gi.a1  = CNTL_2p2z_A1_7;

                pfcVars.indexCurrentGain = 7;
            }
            //Load 10% to 4%
            else if(pfcVars.IdcRef > 0.04f)
            {
                gi.b2  = CNTL_2p2z_B2_8;
                gi.b1  = CNTL_2p2z_B1_8;
                gi.b0  = CNTL_2p2z_B0_8;
                gi.a2  = CNTL_2p2z_A2_8;
                gi.a1  = CNTL_2p2z_A1_8;

                pfcVars.indexCurrentGain = 8;
            }
            //Load less than 4%
            else // (pfcVars.IdcRef <= 0.04f)
            {
                gi.b2  = CNTL_2p2z_B2_9;
                gi.b1  = CNTL_2p2z_B1_9;
                gi.b0  = CNTL_2p2z_B0_9;
                gi.a2  = CNTL_2p2z_A2_9;
                gi.a1  = CNTL_2p2z_A1_9;

                pfcVars.indexCurrentGain = 9;
            }
        }
        else
        {
            //Load higher than 20%
            if(pfcVars.IdcRef > pfcVars.IdcRefGainThsLV)
            {
                gi.b2  = CNTL_2p2z_B2_1;
                gi.b1  = CNTL_2p2z_B1_1;
                gi.b0  = CNTL_2p2z_B0_1;
                gi.a2  = CNTL_2p2z_A2_1;
                gi.a1  = CNTL_2p2z_A1_1;

                pfcVars.indexCurrentGain = 1;
            }
            //Load 20% or lower, higher than 10%
            else if(pfcVars.IdcRef > 0.1f)
            {
                gi.b2  = CNTL_2p2z_B2_2;
                gi.b1  = CNTL_2p2z_B1_2;
                gi.b0  = CNTL_2p2z_B0_2;
                gi.a2  = CNTL_2p2z_A2_2;
                gi.a1  = CNTL_2p2z_A1_2;

                pfcVars.indexCurrentGain = 2;
            }
            //Load 10% or lower
            else
            {
                gi.b2  = CNTL_2p2z_B2_3;
                gi.b1  = CNTL_2p2z_B1_3;
                gi.b0  = CNTL_2p2z_B0_3;
                gi.a2  = CNTL_2p2z_A2_3;
                gi.a1  = CNTL_2p2z_A1_3;

                pfcVars.indexCurrentGain = 3;
            }
        }
    }

#if defined(PFC_DISABLE)
    // enable the ADC interrupts for PFC
    pfcVars.VacRms_V = pfcVars.VdcBus_V;
    pfcVars.VdcBusLPF_V = pfcVars.VdcBus_V;
#endif  // PFC_DISABLE

    // Check if AC bus voltage is over threshold
    if(pfcVars.VacRms_V > pfcCtrlVars.overVoltageFault_ACV)
    {
        if(pfcCtrlVars.overVoltageAcTimeCnt > pfcCtrlVars.voltageAcFaultTimeSet)
        {
            pfcVars.faultPFCNow.bit.overVoltageAC = 1;
        }
        else
        {
            pfcCtrlVars.overVoltageAcTimeCnt++;
        }
    }
    else if(pfcVars.VacRms_V < pfcCtrlVars.overVoltageNorm_ACV)
    {
        if(pfcCtrlVars.overVoltageAcTimeCnt == 0)
        {
            pfcVars.faultPFCNow.bit.overVoltageAC = 0;
        }
        else
        {
            pfcCtrlVars.overVoltageAcTimeCnt--;
        }
    }

    // Check if AC bus voltage is under threshold
    if(pfcVars.VacRms_V < pfcCtrlVars.underVoltageFault_ACV)
    {
        if(pfcCtrlVars.underVoltageAcTimeCnt > pfcCtrlVars.voltageAcFaultTimeSet)
        {
            pfcVars.faultPFCNow.bit.underVoltageAC = 1;
        }
        else
        {
            pfcCtrlVars.underVoltageAcTimeCnt++;
        }
    }
    else if(pfcVars.VacRms_V > pfcCtrlVars.underVoltageNorm_ACV)
    {
        if(pfcCtrlVars.underVoltageAcTimeCnt == 0)
        {
            pfcVars.faultPFCNow.bit.underVoltageAC = 0;
        }
        else
        {
            pfcCtrlVars.underVoltageAcTimeCnt--;
        }
    }

    // Check if DC bus voltage is over threshold
    if(pfcVars.VdcBusLPF_V > pfcCtrlVars.overVoltageFault_DCV)
    {
        if(pfcCtrlVars.overVoltageDcTimeCnt > pfcCtrlVars.voltageDcFaultTimeSet)
        {
            pfcVars.faultPFCNow.bit.overVoltageDC = 1;
        }
        else
        {
            pfcCtrlVars.overVoltageDcTimeCnt++;
        }
    }
    else if(pfcVars.VdcBusLPF_V < pfcCtrlVars.overVoltageNorm_DCV)
    {
        if(pfcCtrlVars.overVoltageDcTimeCnt == 0)
        {
            pfcVars.faultPFCNow.bit.overVoltageDC = 0;
        }
        else
        {
            pfcCtrlVars.overVoltageDcTimeCnt--;
        }
    }

    // Check if DC bus voltage is under threshold
    if(pfcVars.VdcBusLPF_V < pfcCtrlVars.underVoltageFault_DCV)
    {
        if(pfcCtrlVars.underVoltageDcTimeCnt > pfcCtrlVars.voltageDcFaultTimeSet)
        {
            pfcVars.faultPFCNow.bit.underVoltageDC = 1;
        }
        else
        {
            pfcCtrlVars.underVoltageDcTimeCnt++;
        }
    }
    else if(pfcVars.VdcBusLPF_V > pfcCtrlVars.underVoltageNorm_DCV)
    {
        if(pfcCtrlVars.underVoltageDcTimeCnt == 0)
        {
            pfcVars.faultPFCNow.bit.underVoltageDC = 0;
        }
        else
        {
            pfcCtrlVars.underVoltageDcTimeCnt--;
        }
    }

    // Check if PFC is over load
    if(pfcVars.PowerRms_W > pfcCtrlVars.overLoadFault_W)
    {
        if(pfcCtrlVars.overLoadTimeCnt > pfcCtrlVars.overLoadFaultTimeSet)
        {
            pfcVars.faultPFCNow.bit.overLoad = 1;
        }
        else
        {
            pfcCtrlVars.overLoadTimeCnt++;
        }
    }
    else  if(pfcCtrlVars.overLoadTimeCnt == 0)
    {
        pfcVars.faultPFCNow.bit.overLoad = 0;
    }
    else
    {
        pfcCtrlVars.overLoadTimeCnt--;
    }

    return;
}

__interrupt void pfcCtrlISR(void)
{
    pfcVars.ISRCount++;

    // acknowledge the interrupt of PFC
    HAL_ackPFCADCInt();

    // Interleaved PFC using DMA for Iac, 4 times average
    // read the ADC data with offsets
    HAL_readPFCADCData(&adcDataPFC);        // ILPFC

#if defined(ILPFC_DCH_EN)
    // convert acbus current
     if(pfcVars.flagEnableILMode == true)
     {
         adcDataPFC.IacBus = adcDataPFC.currentAC_sf *
                 ((float32_t)(ADC_readPPBResult(PFC_IAC_ADCRES_BASE, PFC_IAC1_ADC_PPB_NUM) +
                 ADC_readPPBResult(PFC_IAC_ADCRES_BASE, PFC_IAC2_ADC_PPB_NUM)));
     }
     else
     {
#if defined(SPPFC_CHA_EN)
         adcDataPFC.IacBus = adcDataPFC.currentAC_sf * 2.0f *
                 ((float32_t)(ADC_readPPBResult(PFC_IAC_ADCRES_BASE, PFC_IAC1_ADC_PPB_NUM)));
#elif defined(SPPFC_CHB_EN)
         adcDataPFC.IacBus = adcDataPFC.currentAC_sf * 2.0f *
                 ((float32_t)(ADC_readPPBResult(PFC_IAC_ADCRES_BASE, PFC_IAC2_ADC_PPB_NUM)));
#endif      // SPPFC_CHA_EN || SPPFC_CHB_EN
     }
#elif defined(SPPFC_CHA_EN)
     // convert acbus current
      adcDataPFC.IacBus = adcDataPFC.currentAC_sf * 2.0f *
              ((float32_t)(ADC_readPPBResult(PFC_IAC_ADCRES_BASE, PFC_IAC1_ADC_PPB_NUM)));
#elif defined(SPPFC_CHB_EN)
      // convert acbus current
       adcDataPFC.IacBus = adcDataPFC.currentAC_sf * 2.0f *
               ((float32_t)(ADC_readPPBResult(PFC_IAC_ADCRES_BASE, PFC_IAC2_ADC_PPB_NUM)));
#else      // !(SPPFC_CHA_EN || SPPFC_CHB_EN)
#error Not select a right PFC output mode
#endif      // !(SPPFC_CHA_EN || SPPFC_CHB_EN)

    adcDataPFC.IacBus = (adcDataPFC.IacBus < USER_PFC_ADC_PU_SF) ?
            USER_PFC_ADC_PU_SF : adcDataPFC.IacBus; // minimum positive current

    // calculate AC bus voltage
#if defined(HVMTRPFC_REV1P1) || defined(DMCPFC_REV0P1)
    adcDataPFC.ValBus = adcDataPFC.ValBus - adcDataPFC.offset_Vac;
    adcDataPFC.VanBus = adcDataPFC.VanBus - adcDataPFC.offset_Van;

    if(adcDataPFC.ValBus > adcDataPFC.VanBus)
    {
        adcDataPFC.VacBus = adcDataPFC.ValBus - adcDataPFC.VanBus;
    }
    else
    {
        adcDataPFC.VacBus = adcDataPFC.VanBus - adcDataPFC.ValBus;
    }
#else
    adcDataPFC.VacBus = fabsf(adcDataPFC.VacSen);
#endif  // !(HVMTRPFC_REV1P1 | DMCPFC_REV0P1)

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_PFC)
    sfraNoiseInj_pu = 0.0f;
    sfraNoiseVpfc = 0.0f;
    sfraNoiseIpfc = 0.0f;

    sfraNoiseInj_pu = SFRA_F32_inject(0.0f);

    if(sfraTestLoop == SFRA_TEST_PFC_VOLTAGE)
    {
        sfraNoiseVpfc = sfraNoiseInj_pu;
    }
    else if(sfraTestLoop == SFRA_TEST_PFC_CURRENT)
    {
        sfraNoiseIpfc = sfraNoiseInj_pu;
    }
#endif  // (SFRA_ENABLE && SFRA_TEST_PFC)

    pfcVars.counterVoltage++;

    switch(pfcVars.counterVoltage)
    {
        case USER_PFC_NUM_ISR_TICKS_PER_VOLTAGE_TICK:       // 36kHz/5=7.2kHz
            // voltage loop
            if(pfcVars.flagEnableVoltageLoop == true)
            {
#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_PFC)
                pfcVars.IdcRefOut =
                    DCL_runPI_C4(&gv, pfcVars.VdcTrajInt + sfraNoiseVpfc, pfcVars.VdcBusLPF);
#else   // !(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_PFC)
                pfcVars.IdcRefOut =
                       DCL_runPI_C4(&gv, pfcVars.VdcTrajInt, pfcVars.VdcBus_pu);
#endif  // !(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_PFC)

            }
            else    // pfcVars.flagEnableVoltageLoop == false
            {
                gv.i6 = 1.0f;
                gv.i10 = gv.i11 = 0.0f;

                pfcVars.IdcRefOut = 0.0;
            }

#if(PFC_BUILDLEVEL == PFC_LEVEL_4)
            pfcVars.IdcRef = pfcVars.IdcRefOut;
#endif
            pfcVars.counterVoltage = 0;

            break;

        case 1: // twice voltage control loop frequency
            // 3.130 = (1.0 / minimum voltage)^2, 48VAC
            pfcVars.VinvSqr = pfcVars.invRmsSqrCoef / (pfcVars.VacRms_pu * pfcVars.VacRms_pu);
            pfcVars.VinvSqr = (pfcVars.VinvSqr > 3.130f) ? 3.130f : pfcVars.VinvSqr;

            break;

        case 2: // twice voltage control loop frequency
            //Calculate RMS input voltage and input frequency, 7.2kHz
            measPower.i = adcDataPFC.IacBus;
            measPower.v = adcDataPFC.VacBus;

            POWER_MEAS_FAST_run(&measPower);

            pfcVars.IacRms_pu = measPower.iRms;
            pfcVars.VacRms_pu = measPower.vRms;

            //Divided by 2 because the signal is rectified
            pfcVars.FreqAc_Hz = measPower.acFreq * 0.5f;

            break;

        case 3:
            pfcVars.VdcBus_V = adcDataPFC.VdcBus * USER_PFC_ADC_FULL_SCALE_DC_VOLTAGE_V;

            // convert dc bus voltage from pu to SI unit
            pfcVars.VdcBusLPF = pfcVars.VdcBusLPF +
                    (adcDataPFC.VdcBus - pfcVars.VdcBusLPF) * pfcVars.lpfVdcCoeff;

            pfcVars.VdcBusLPF_V = pfcVars.VdcBusLPF * USER_PFC_ADC_FULL_SCALE_DC_VOLTAGE_V;

            // run notch filter
#if !defined(NOTCH_FILTER)
            pfcVars.VdcBus_pu = pfcVars.VdcBusLPF;
#else   // NOTCH_FILTER
            FILTER_NOTCH_set_in(filterNotchHandle, pfcVars.VdcBusLPF);
            FILTER_NOTCH_run(filterNotchHandle, filterCoeffHandle);
            pfcVars.VdcBusNotch = FILTER_NOTCH_get_out(filterNotchHandle);
            pfcVars.VdcBus_pu = pfcVars.VdcBusNotch;
#endif   // NOTCH_FILTER

#if (PFC_BUILDLEVEL == PFC_LEVEL_4) && defined(ILPFC_DCH_EN) && defined(ILPFC_SWM_EN)
            // switch PFC mode between interleave and single phase
            if(pfcVars.IdcRef > pfcVars.IdcRefSwILOn)
            {
                if(pfcVars.flagEnableILMode == false)
                {
                    pfcVars.flagEnableILMode = true;
                    gi.x1 = gi.x1 * 0.5f;
                    gi.x2 = gi.x2 * 0.5f;

                    HAL_enablePFCILPWM(halPFCHandle);
                }
            }
            else if(pfcVars.IdcRef < pfcVars.IdcRefSwILOff)
            {
                if(pfcVars.flagEnableILMode == true)
                {
                    pfcVars.flagEnableILMode = false;
                    gi.x1 = gi.x1 * 2.0f;
                    gi.x2 = gi.x2 * 2.0f;

#if defined(SPPFC_CHA_EN)
            HAL_disableILPFCPowerChB(halPFCHandle);
#elif defined(SPPFC_CHB_EN)
            HAL_disableILPFCPowerChA(halPFCHandle);
#else
            HAL_disableILPFCPowerChB(halPFCHandle);
#endif      // SPPFC_CHA_EN || SPPFC_CHB_EN
                }
            }
#endif  // (PFC_BUILDLEVEL == PFC_LEVEL_4) && ILPFC_DCH_EN && ILPFC_SWM_EN_N

            break;

        case 4:
            // ramp control
            if(pfcVars.flagEnablePFC == true)
            {
    #if(PFC_BUILDLEVEL >= PFC_LEVEL_3)
                if(pfcVars.VdcBus_V > USER_PFC_ENABLE_CURRENT_LOOP_V)
                {
                    pfcVars.flagEnableCurrentLoop = true;
                }
                else if(pfcVars.VdcBus_V < USER_PFC_DISABLE_CURRENT_LOOP_V)
                {
                    pfcVars.flagEnableCurrentLoop = false;
                }
    #endif

    #if(PFC_BUILDLEVEL == PFC_LEVEL_4)
                if(pfcVars.VacRms_V >= USER_PFC_ENABLE_VOLTAGE_LOOP_V)
                {
                    pfcVars.flagEnableVoltageLoop = true;
                }
                else if(pfcVars.VacRms_V < USER_PFC_DISABLE_VOLTAGE_LOOP_V)
                {
                    pfcVars.flagEnableVoltageLoop = false;
                }
    #endif

                if(pfcVars.flagFirstVoltageLoop == true)
                {
                    pfcVars.IacRef = adcDataPFC.IacBus;
                    pfcVars.VdcTrajInt = pfcVars.VdcBusLPF;

                    pfcVars.flagFirstVoltageLoop = false;
                }

                float32_t VdcError = pfcVars.VdcTrajTraget - pfcVars.VdcTrajInt;

                if(VdcError > pfcVars.VdcTrajRate)
                {
                    pfcVars.VdcTrajInt = pfcVars.VdcTrajTraget + pfcVars.VdcTrajRate;
                }
                else if(VdcError < -pfcVars.VdcTrajRate)
                {
                    pfcVars.VdcTrajInt = pfcVars.VdcTrajTraget - pfcVars.VdcTrajRate;
                }
                else
                {
                    pfcVars.VdcTrajInt = pfcVars.VdcTrajTraget;
                    pfcVars.flagStatePFC = true;
                }
            }
            else    // pfcVars.flagEnablePFC == false
            {
                pfcVars.IdcRef = 0.0f;

                pfcVars.IacRef = adcDataPFC.IacBus;
                pfcVars.VdcTrajInt = pfcVars.VdcBusLPF;
                pfcVars.dutyOut = 0.0;

                pfcVars.flagEnableCurrentLoop = false;
                pfcVars.flagEnableVoltageLoop = false;
                pfcVars.flagFirstVoltageLoop = true;
                pfcVars.flagStatePFC = false;
            }
            break;

        default:
            break;
    }

    pfcVars.IacRef = pfcVars.VinvSqr * adcDataPFC.VacBus * pfcVars.IdcRef;

    pfcVars.IacRef = (pfcVars.IacRef > pfcVars.IacPeak_pu) ?
            pfcVars.IacPeak_pu : pfcVars.IacRef;

    pfcVars.IacRef = (pfcVars.IacRef < 0.0f) ? 0.0f : pfcVars.IacRef;

    pfcVars.IacSen = adcDataPFC.IacBus;

    // run current loop controller
    if(pfcVars.flagEnableCurrentLoop == true)
    {
#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_PFC)

        pfcVars.IacError = (pfcVars.IacRef + sfraNoiseIpfc) - pfcVars.IacSen;

        pfcVars.giOut = DCL_runDF22_C1(&gi, pfcVars.IacError);
#else
        pfcVars.IacError = pfcVars.IacRef - pfcVars.IacSen;

        pfcVars.giOut = DCL_runDF22_C1(&gi, pfcVars.IacError);
#endif  // (SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_PFC)

        pfcVars.dutyOut = (pfcVars.giOut > pfcVars.dutyOutMaxSet) ? pfcVars.dutyOutMaxSet : pfcVars.giOut;
        pfcVars.dutyOut = (pfcVars.giOut < 0.0f) ? 0.0f : pfcVars.giOut;

        gi.x1 = pfcVars.dutyOut;        // store the output for next cycle
    }
    else
    {
        gi.x1 = gi.x2 = 0.0f;

#if(PFC_BUILDLEVEL >= PFC_LEVEL_3)
        pfcVars.dutyOut = 0.0;
#endif
    }

#if(PFC_BUILDLEVEL >= PFC_LEVEL_3)
    // over peak voltage protection
    if(adcDataPFC.VdcBus > pfcVars.VdcBusShutDown)      // 410.0VDC, set a right value according to the system
    {
        pfcVars.faultPFCNow.bit.shutDownVoltage = 1;
        pfcVars.dutyOut = 0.0f;

        gv.i10 = gv.i10 * 0.75f;
        gv.i11 = 0.0f;
    }
#endif

#if defined(SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_PFC)
    // Collect the SFRA data
    if(sfraTestLoop == SFRA_TEST_PFC_CURRENT)
    {
        sfraNoiseOut = pfcVars.dutyOut;
        sfraNoiseFdb = pfcVars.IacSen;
    }
    else if(sfraTestLoop == SFRA_TEST_PFC_VOLTAGE)
    {
        sfraNoiseOut = pfcVars.IdcRefOut;
        sfraNoiseFdb = pfcVars.VdcBusLPF;
    }

    SFRA_COLLECT((float32_t*)&sfraNoiseOut, (float32_t*)&sfraNoiseFdb);
#endif  // (SFRA_ENABLE) && (SFRA_TEST_TYPE == SFRA_TEST_PFC)

#if(PFC_BUILDLEVEL == PFC_LEVEL_1)
    // output 50%
    pwmDataPFC.dutyValue = 0.5f;
#elif(PFC_BUILDLEVEL >= PFC_LEVEL_2)
    // manual set the duty
    pwmDataPFC.dutyValue = pfcVars.dutyOut;
#endif

    // write the PWM compare values
    HAL_writePFCPWMData(halPFCHandle, &pwmDataPFC);



#if defined(DATALOGF2_EN) && defined(DATALOG_PFC)
    DATALOGIF_updateWithDMA(datalogHandle);

    // Force trig DMA channel to save the data
    HAL_trigDMAforDLOG(halHandle, 0);
    HAL_trigDMAforDLOG(halHandle, 1);

#endif  // DATALOGF2_EN

#if defined(DAC128S_ENABLE) && defined(DAC_FASTUPDATE)
#if defined(DAC80504_SEL)
    DAC80504_writeData(dac128sHandle);
#else   // DAC128S805
    DAC128S_writeData(dac128sHandle);
#endif  // DAC128S805
#endif  // DAC128S_ENABLE

#if defined(EPWMDAC_MODE) && defined(PWMDAC_PFC)
    // connect inputs of the PWMDAC module.
    HAL_writePWMDACData(halHandle, &pwmDACData);
#endif  // EPWMDAC_MODE

#ifdef DEBUG_MONITOR_EN
    if(pfcVars.flagClearRecord == 1)
    {
        pfcVars.VdcBusMax_V = 0.0f;
        pfcVars.VdcBusMin_V = 500.0f;
        pfcVars.flagClearRecord = 0;
    }
    else
    {
        if(pfcVars.VdcBus_V > pfcVars.VdcBusMax_V)
        {
            pfcVars.VdcBusMax_V = pfcVars.VdcBus_V;
        }

        if(pfcVars.VdcBus_V < pfcVars.VdcBusMin_V)
        {
            pfcVars.VdcBusMin_V = pfcVars.VdcBus_V;
        }
    }
#endif  // DEBUG_MONITOR_EN

    return;
} // end of mainISR() function

void runPFCOffsetsCalculation(void)
{
    HAL_PFC_Obj *obj = (HAL_PFC_Obj *)halPFCHandle;

    adcDataPFC.offset_Iac = USER_PFC_IAC_OFFSET;
    adcDataPFC.offset_Vac = USER_PFC_VAC_OFFSET;

#if defined(HVMTRPFC_REV1P1) || defined(DMCPFC_REV0P1)
    adcDataPFC.offset_Van = USER_PFC_VAN_OFFSET;
#endif

    adcDataPFC.currentAC_sf = USER_PFC_ADC_IAC_PU_SF;
    adcDataPFC.voltageAC_sf = USER_PFC_ADC_VAC_PU_SF;
    adcDataPFC.voltageDC_sf = USER_PFC_ADC_VDC_PU_SF;

    EPWM_enableInterrupt(obj->pwmHandle);

    if(pfcVars.flagEnableOffsetCalc == true)
    {
        float32_t offsetK1 = 0.998001;  // Offset filter coefficient K1: 0.05/(T+0.05);
        float32_t offsetK2 = 0.001999;  // Offset filter coefficient K2: T/(T+0.05);

        uint16_t offsetCnt = 0;

        ADC_setPPBReferenceOffset(PFC_IAC_ADC_BASE, PFC_IAC1_ADC_PPB_NUM, 0);
        ADC_setPPBReferenceOffset(PFC_IAC_ADC_BASE, PFC_IAC2_ADC_PPB_NUM, 0);

#if !defined(HVMTRPFC_REV1P1) && !defined(DMCPFC_REV1P0)
        ADC_setPPBReferenceOffset(PFC_VAC_ADC_BASE, PFC_VAC1_ADC_PPB_NUM, 0);
        ADC_setPPBReferenceOffset(PFC_VAC_ADC_BASE, PFC_VAC2_ADC_PPB_NUM, 0);
#endif // !HVMTRPFC_REV1P1 && !DMCPFC_REV1P0

        adcDataPFC.offset_Iac = 0.0;
        adcDataPFC.offset_Vac = 0.0;

        // disable the PFC PWM
        HAL_disablePFCPWM(halPFCHandle);

        for(offsetCnt = 0; offsetCnt < 11000; offsetCnt++)
        {
            // clear PWM interrupt
            EPWM_clearEventTriggerInterruptFlag(obj->pwmHandle);

            while(EPWM_getEventTriggerInterruptStatus(obj->pwmHandle) == false);

            // read the ADC data with offsets
            HAL_readPFCADCData(&adcDataPFC);

            adcDataPFC.IacBus = (float32_t)ADC_readResult(PFC_IAC_ADCRES_BASE,
                                  PFC_IAC_ADC_SOC_NUM) * USER_PFC_ADC_PU_SF;

#if !defined(HVMTRPFC_REV1P1) && !defined(DMCPFC_REV1P0)
            adcDataPFC.VacBus = (float32_t)ADC_readResult(PFC_VAC_ADCRES_BASE,
                                      PFC_VAC_ADC_SOC_NUM) * USER_PFC_ADC_PU_SF;
#endif // !HVMTRPFC_REV1P1 && !DMCPFC_REV1P0

            if(offsetCnt > 1000)
            {
                // Offsets in current sensing
                adcDataPFC.offset_Iac = offsetK1 * adcDataPFC.offset_Iac +
                        adcDataPFC.IacBus * offsetK2;

#if !defined(HVMTRPFC_REV1P1) && !defined(DMCPFC_REV1P0)
                // Offsets in AC/ACL voltage sensing
                adcDataPFC.offset_Vac = offsetK1 * adcDataPFC.offset_Vac +
                        adcDataPFC.VacBus * offsetK2;
#endif // !HVMTRPFC_REV1P1 && !DMCPFC_REV1P0
            }
        }

        pfcVars.flagEnableOffsetCalc = false;
    }

    EPWM_disableInterrupt(obj->pwmHandle);

    ADC_setPPBReferenceOffset(PFC_IAC_ADC_BASE, PFC_IAC1_ADC_PPB_NUM,
                       (uint16_t)(adcDataPFC.offset_Iac * USER_PFC_ADC_INV_SF));

    ADC_setPPBReferenceOffset(PFC_IAC_ADC_BASE, PFC_IAC2_ADC_PPB_NUM,
                       (uint16_t)(adcDataPFC.offset_Iac * USER_PFC_ADC_INV_SF));

#if !defined(HVMTRPFC_REV1P1) && !defined(DMCPFC_REV1P0)
    ADC_setPPBReferenceOffset(PFC_VAC_ADC_BASE, PFC_VAC1_ADC_PPB_NUM,
                       (uint16_t)(adcDataPFC.offset_Vac * USER_PFC_ADC_INV_SF));

    ADC_setPPBReferenceOffset(PFC_VAC_ADC_BASE, PFC_VAC2_ADC_PPB_NUM,
                       (uint16_t)(adcDataPFC.offset_Vac * USER_PFC_ADC_INV_SF));
#endif  // !HVMTRPFC_REV1P1 && !DMCPFC_REV1P0

    return;
} // end of runPFCOffsetsCalculation() function
//
//-- end of this file ----------------------------------------------------------
//
