/* ============================================================================
System Name:    Power Factor Correction for single phase AC input with two phase
                interleaved transition mode totem pole configuration

File Name:      pfc2philtrmttpl_main.c

Target:         F28004x

Author:         Hrishi Nene

Description:    Project implements power factor correction software that runs on
                C2000 MCU for a single phase AC input with two phase interleaved
                transition mode totem pole configuration

                Current and voltages on the design are sensed using the ADC

                The control loop switching frequency is 50Khz and is triggered
                by EPWM1

                following is the decription of other files that are used by this
                system
                <solution name>.c -> This file, which has the main.c of the
                project
                <solution name>.h -> This is the main header file for the
                project
                <solution name>_settings.h -> settings
                <boad name>_board.c -> This is the file that is used to keep
                                       common functions related to the board
                                       hardware like setting up the PWM, ADC,
                                       SDFM, reading data and common variables
                                       liked current, voltage etc.
===================================================  */
//#############################################################################
// $TI Release: TIDA_00961 v2.00.02.00 $
// $Release Date: Tue Mar 23 14:55:26 CDT 2021 $
// $Copyright:
// Copyright (C) 2021 Texas Instruments Incorporated - http://www.ti.com/
//
// ALL RIGHTS RESERVED
// $
//#############################################################################

#include "pfc2philtrmttpl_main.h"


//*********************** C2000 SYSTEM SW FRAMEWORK  *************************//

//--------------------------- State Machine Related ----------------------------

int16_t vTimer0[4];         // Virtual Timers slaved off CPU Timer 0 (A events)
int16_t vTimer1[4];         // Virtual Timers slaved off CPU Timer 1 (B events)

//*********************** USER Variables *************************************//
// Global variables used in this system specific to this control method
// common variables are kept in <system_name>_board.c
//****************************************************************************//
SPLL_1PH_SOGI_FLL spll3;

// current, voltage controllers
DCL_PI gi, gv;

DCL_DF22 notch1, notch2;

//--------------------------------- SFRA Related Variables ---------------------
float32_t plantMagVect[SFRA_FREQ_LENGTH];
float32_t plantPhaseVect[SFRA_FREQ_LENGTH];
float32_t olMagVect[SFRA_FREQ_LENGTH];
float32_t olPhaseVect[SFRA_FREQ_LENGTH];
float32_t freqVect[SFRA_FREQ_LENGTH];

SFRA_F32 sfra1;

//extern to access tables in ROM
extern long FPUsinTable[];

int16_t slewState;

volatile enum enum_BuildLevel buildInfo = BuildLevel1_OpenLoop_AC ;
volatile enum enum_boardState boardState = boardState_OFF;
volatile enum enum_boardStatus boardStatus = boardStatus_Idle;

//TODO Control Variables
// Measurement Variables
// Inductor Current Measurement

// AC Current Measurement
volatile float32_t ac_cur_sensed, ac_cur_ADC_ref, ac_cur_sensed_1, ac_cur_sensed_2;
volatile float32_t ac_cur_sensedOffset;
volatile float32_t gv_out_dis_shed, ac_cur_shed, gv_out_shed, ac_cur_comp;
volatile float32_t ac_cur_th, ac_cur_mult;
volatile float32_t ac_cur_hysteresis = 0.8f, ac_cur_max_inv, ac_cur_max;

volatile float32_t thetaOffset;

// Output Voltage measurement
volatile float32_t vBus_sensed, vBus_in_volts;
volatile float32_t vBus_sensedOffset;

//Input voltage measurement
volatile float32_t acL_sensed, acN_sensed, ac_vol_sensed, ac_vol_in_volts;
volatile float32_t ac_cur_in_amps, ac_vol_sns_rect, ac_vol_sensed_temp;
volatile float32_t acL_sensedOffset, acN_sensedOffset;

//Filtered DC bus measurement
volatile float32_t vBusAvg;
volatile float32_t vRmsAvg;

// Reference variables
//  current set point
volatile float32_t ac_cur_ref,ac_cur_ref_inst, ac_cur_ref_inst_prev;
volatile float32_t ac_cur_ref_temp;
volatile float32_t temp_cur_ref, Vin_pu;

// voltage set point
volatile float32_t vBusRef, vBusRefSlewed;

//Dead-time calculations
volatile float32_t inter_calc, inter_calc2, dead_time;
volatile float32_t usDB_Rise_Mult;

// Display Values
float32_t guiVbus, guiVbusMax;
float32_t guiACFreq;
float32_t guiPrms;
float32_t guiIrms;
volatile float32_t guiVrms;
float32_t guiVavg;
float32_t guiVin;
float32_t guiIi;
float32_t guiIo;
float32_t guiPowerFactor;
float32_t guiVA;
float32_t guiFreqAvg;
float32_t guiVema;

volatile float32_t guiVrmsEMAvg, Vac_Avg, guiIrmsEMAvg, Iac_Avg;

uint16_t ocpThreshold = 3800;
volatile float32_t pwrste_ocpThreshold, pwrste_ovpThreshold;

volatile uint16_t zvs_Detect_threshold_neg = 300;
volatile uint16_t zvs_Detect_threshold_pos = 300;

// Variables used to calibrate measurement offsets
//Offset filter coefficient K1: 0.05/(T+0.05);
float32_t k1 = 0.998f;
//Offset filter coefficient K2: T/(T+0.05)
float32_t k2 = 0.001999f;

float32_t offset165;

// Duty variables
// per unit duty value
volatile float32_t dutyPU,dutyPUPrev,dutyPU_DC, temp_dutyPU;

// Flags for clearing trips and closing the loops and the Relay
volatile int16_t closeGiLoop,closeGvLoop, clearTrip, rlyConnect;
volatile int16_t firstTimeGvLoop, enableiLoop, enableiLoop_Set;

volatile float32_t acSine, acSinePrev;
volatile int16_t zeroCrossDetectFlag;

uint16_t usDeadBandAdj_Enable,usDB_RiseSource,usDB_FallSource;
uint16_t temp_DB, temp_CMP;
int16_t usDeadBand_Rise,usDeadBand_Fall, temp_usDeadBand_Rise;
int16_t temp_usDeadBand_Fall;
volatile float32_t tempDB_fall = 4;

int16_t count_goodVin = 0;

uint16_t usEnablePwms;
volatile uint16_t softEnable_PWMs = 0;
int16_t dead_time_adj = 0, temp_win_adj = -3;
volatile float32_t oneminusD_adj = 0.0, max_zvs_adj = 30;
volatile uint16_t turn_OFF_sync = 0, sync_Enable = 0, prev_zvs = 0;
volatile uint16_t count_filt=0;
volatile float32_t zvs_adj_factor;

volatile uint16_t phase_shedding_EN = 0, phase_OFF = 0, phase_OFF_command = 0;
volatile uint16_t auto_adj_params = 1, shedding_OFF_dbg = 0;

volatile int16_t tempChange=0;
volatile int16_t period;

//Period Calculations
volatile float32_t ton_calc = 20.0, new_ton_calc = 20.0, toff_calc = 125.0;
volatile float32_t vout_vin, mult_factor = 1.0, ton_calc_soft = 10;
volatile float32_t ton_calc_inc_pos = 4.0, ton_calc_inc_neg = 4.0;
volatile float32_t ton_calc_soft_init = 5.0, ton_iloop_adj = 0.0;
volatile float32_t new_ton_temp = 20.0;

volatile float32_t ton_weightage_factor, ton_adj_dbg = 0.0, asym_fact = 0.25;
volatile float32_t vdiode = 0.0;
volatile uint16_t period_calc, ton_calc_adjusted, phase_command;
volatile uint16_t old_phase, old_period_calc, old_Ton;
uint16_t phs_chng_var = 60, chng_var = 40;
volatile int16_t update_flag, phase_changed, phase_fixed;
volatile float32_t vin_scaler = 5.0;

volatile uint16_t period_calc_fast, ton_calc_fast;
volatile float32_t toff_calc_fast = 125.0, duty_loop = 0.0;


// Stand Alone Flash Image Instrumentation
// GPIO toggles for different states
int16_t ledBlinkCnt1, ledBlinkCnt2;
int16_t timer1;

//volatile enum enum_pwmSwState pwmSwState;
volatile enum enum_crossoverState crossoverState;
volatile int16_t state_slew=0;
volatile int16_t state_slew_max=10;

//+ve/-ve input cycle
volatile int16_t neg_cycle = 0; // '1' indicates a negative input half cycle
volatile int16_t no_switching = 1, si_FET_on = 0; // '1' indicates no switching
volatile float32_t cur_sync_off_lo = -0.0022f, cur_sync_off_hi = 0.0022f;
volatile float32_t cur_sync_on_lo = -0.0022f, cur_sync_on_hi = 0.0022f;
volatile int16_t state_int_done;
volatile int16_t force_end_cycle = 0, count_sync = 0, count_sync_on = 0;

//ZVS LOST signal
volatile int16_t zvs_lost = 0;
volatile int16_t zvs_adj_on = 0, zvs_dont_adj = 0;

volatile int16_t neg_ref = 0, pos_ref = 0;

volatile int16_t coeff_change = 0; // Flag to indicate change of coefficients

volatile int16_t state_flag = 0, start_flag = 0, count_ovp = 0;
volatile int16_t ovp_Fault = 0, ocp_Fault = 0;

volatile int16_t softtransition_enable = 0, softtransition = 0;
volatile int16_t db_decrement_rise = 0, db_decrement_fall = 0;
volatile int16_t transit_count = 0, si_delay_count = 0, soft_start_count = 10;
volatile int16_t si_delay_thrs = 40;

volatile int16_t count_after_soft = 0, si_FET_on_thrs = 0;
volatile int16_t sync_FET_on_thrs = 5;//sync_FET_on_thrs > than si_FET_on_thrs

volatile int16_t soft_adj_rise = PFC_DEADBAND_CNTMIN;
volatile int16_t soft_adj_fall = 0; // Soft transition (zero crossing) variables

volatile float32_t gi_out, gv_out, new_gi_Kp, new_gi_Ki, new_gv_Kp, new_gv_Ki;
volatile float32_t gi_pi_max_dbg, gi_pi_min_dbg, gi_pi_min_Set;

volatile float32_t vBusSensedBuff[10], ac_cur_ref_Buff[10];
volatile float32_t vBus_sensedFiltered, ac_cur_ref_avg, ac_cur_ref_avg_temp;
volatile int16_t vBusSensedBuffIndex=0, ac_cur_ref_Buff_Index = 0;

volatile float32_t vBus_sensedFiltered_notch1,vBus_sensedFiltered_notch2;

volatile uint16_t phase_shed_count = 0, phase_en_count = 0;
volatile uint16_t phase_shed_count_thres = 200;
volatile uint16_t phase_shed_count_delay = 0;

volatile float32_t nphigh_var = 0.03f, pnlow_var = 0.03f;
volatile float32_t pnhigh_var = -0.03f, nplow_var = -0.03f;
volatile float32_t nphigher_var = 0.08f, nphighest_var = 0.12f;
volatile float32_t pnhigher_var = -0.08f, pnhighest_var = -0.12f;

volatile float32_t vin_2xboost_calc = 10, acSine_2xboost = 0;
volatile float32_t acSine_diff = 0, toff_adj_percentage = 0.05f;

volatile int16_t acCurGain_ON = 0, acCurGain_ON_Set = 1;    // For v3 and v2

// Variable declarations for state machine
void (*Alpha_State_Ptr)(void);  // Base States pointer
void (*A_Task_Ptr)(void);       // State pointer A branch
void (*B_Task_Ptr)(void);       // State pointer B branch

// State Machine function prototypes
//------------------------------------
// Alpha states
void A0(void);  //state A0
void B0(void);  //state B0

// A branch states
void A1(void);  //state A1
void A2(void);  //state A2
void A3(void);  //state A3

// B branch states
void B1(void);  //state B1
void B2(void);  //state B2
void B3(void);  //state B3

volatile float32_t Ics_G_fact = 0.0;

//TODO Main
void main(void)
{
    // This routine sets up the basic device config such as initializing PLL
    // copying code from FLASH to RAM. This routine will also initialize the
    // CPU timers that are used in the background task for this system
    setupDevice();// Make sure LSP clock is set to divide by 2 to get get 50MHz.

    // Tasks State-machine init
    Alpha_State_Ptr = &A0;
    A_Task_Ptr = &A1;
    B_Task_Ptr = &B1;

    //Setup all pwms
    setupPwms();

    // Setup ADC on the device
    setupADC();

    //Setup Gpios
    setupGpios();

    // Initialize global variables generic to the board
    globalVariablesInit();

    setupSFRA();

    // Enable PWM Clocks
    enablePWMCLKCounting();

    setupADCSOCTrigger(LOW_FREQ_PWM_BASE);
    setupADCSOCTrigger(ADC_OVERSAMPLE_PWM_BASE);

    ac_cur_sensedOffset = 0.024f;

    // setup protection and trips for the board
    // over-current protection
    initCmpssAcCur(); // Function to configure CMPSS3 for AC_I OCP

    initCmpssZvs();   // Function to configure CMPSS4 for ZVS loss detect

    setupBoardProtection(LOW_FREQ_PWM_BASE,HIGH_FREQ_PWM1_BASE,
                         HIGH_FREQ_PWM2_BASE);

    // safe to setup PWM pins
    setPinsAsPWM();

    // ISR Mapping
    setupInterrupt();

    EPWM_setGlobalLoadOneShotLatch(HIGH_FREQ_PWM1_BASE);         // Initial
    EPWM_setInterruptEventCountInitValue(HIGH_FREQ_PWM1_BASE, 0);// Initial

    // IDLE loop. Just sit and loop forever, periodically will branch into A0-A3
    // B0-B3 tasks. Frequency of this branching is set in setupDevice routine.
    for(;;)  //infinite loop
    {
        // State machine entry & exit point
        //===========================================================
        (*Alpha_State_Ptr)();   // jump to an Alpha state (A0,B0,...)
        //===========================================================

    }
} //END MAIN CODE

//****************************************************************************//
//              ISR roots
//****************************************************************************//
__attribute__((ramfunc))
interrupt void pwmISR(void)
{
    if (update_flag)// When Set implies a new command from the control ISR.
                    // In some cases it might also imply phase was changed
                    // temporarily to avoid skipping CMP values.
    {
       // Need to use global load as the switching frequency could be much
       // higher than the control ISR frequency.
       if(phase_fixed)
       {
            ton_calc_adjusted = old_phase - 1;
            // Following will make sure that CMP value is not skipped when
            // phase value is changed back to PRD/2 after phase_changed.
            EPWM_setCounterCompareValue(HIGH_FREQ_PWM2_BASE,
                 EPWM_COUNTER_COMPARE_A, ton_calc_adjusted);
            // Allow loading on CTR = PRD
            EPWM_setGlobalLoadTrigger(HIGH_FREQ_PWM2_BASE,
                                      EPWM_GL_LOAD_PULSE_SYNC_OR_CNTR_PERIOD);
            // Regular CMPC value
            EPWM_setCounterCompareValue(HIGH_FREQ_PWM1_BASE,
                EPWM_COUNTER_COMPARE_C, (period_calc-chng_var));
       }
       else if (phase_changed)
       {
           EPWM_setGlobalLoadTrigger(HIGH_FREQ_PWM2_BASE,
                                     EPWM_GL_LOAD_PULSE_SYNC);
           // Following is in preparation for phase_fixed code in next ISR run
           EPWM_setCounterCompareValue(HIGH_FREQ_PWM1_BASE,
               EPWM_COUNTER_COMPARE_C, (period_calc-phs_chng_var));
       }
       else
       {
           EPWM_setGlobalLoadTrigger(HIGH_FREQ_PWM2_BASE,
                                     EPWM_GL_LOAD_PULSE_SYNC);
           //Regular CMPC value
           EPWM_setCounterCompareValue(HIGH_FREQ_PWM1_BASE,
               EPWM_COUNTER_COMPARE_C, (period_calc-chng_var));
       }
        EPWM_setGlobalLoadOneShotLatch(HIGH_FREQ_PWM1_BASE);
        EPWM_setPhaseShift(HIGH_FREQ_PWM2_BASE, phase_command);

        if (phase_changed)// Phase was changed temporarily to avoid CMP skipping
        {
            phase_command = period_calc>>1;//Correct phase command for next ISR
            phase_changed = 0;
            // if phase has been fixed, adjust Ton to below the old phase value
            // (which should be equal to or 1 less than the desired Ton from
            // the loop (CTRL ISR)
            phase_fixed = 1;
        }
        else
        {
            if(phase_fixed)
            {
                phase_fixed = 0;
                // Following two lines of code are needed to make sure latest
                // values are saved for next control ISR
                old_Ton = ton_calc_adjusted;
                old_phase = phase_command;
                zvs_dont_adj = 1;
            }
            else
            {
                zvs_dont_adj = 0;
            }
            update_flag = 0;
        }
    }

// Start - ZVS extension code (oneminusD_adj)

#if ((INCR_BUILD == 2) || (INCR_BUILD == 3))
    if (zvs_adj_on && (!zvs_dont_adj) && (phase_OFF))
    {
     if (vBus_in_volts > 370.0f)// Check if we need to run ZVS adjustment or not
     {
        if (vout_vin < (ac_vol_in_volts))
        {
            //Check DCAEVT1 flag - Lost ZVS
            if (((EPWM_getTripZoneFlagStatus(ACTIVE_PWM_DUPLICATE_PWM_BASE))&(0x8))>>3)
            {
               // Check if ZVS was achieved in the previous cycle
               if(prev_zvs)
               {
                   count_filt = 0;
                   prev_zvs = 0;
               }

                   zvs_lost = 1;
                   // Clear DCAEVT1 flag
                   EPWM_clearTripZoneFlag(ACTIVE_PWM_DUPLICATE_PWM_BASE,
                                          EPWM_TZ_FLAG_DCAEVT1);
                   // Clamp Max oneminusD_adj
                   oneminusD_adj = (oneminusD_adj>=max_zvs_adj)?oneminusD_adj:
                                   (oneminusD_adj+zvs_adj_factor);
                   count_filt++;
            }
            else
            {
                // Check if ZVS was lost in the previous cycle
                if(!prev_zvs)
                {
                    count_filt = 0;
                    prev_zvs = 1;
                }
                    zvs_lost = 0;
                    // Clamp Min oneminusD_adj
                    oneminusD_adj = (oneminusD_adj<=-10)?oneminusD_adj:
                                    (oneminusD_adj-zvs_adj_factor);
            }
        }
        else        // No need to run ZVS adjustment
        {
            oneminusD_adj = 0;
        }
        toff_calc_fast = toff_calc + oneminusD_adj;
        period_calc_fast = ton_calc_adjusted + toff_calc_fast;

        ton_calc_fast = duty_loop*period_calc_fast;

         update_constONtime(HIGH_FREQ_PWM1_BASE, HIGH_FREQ_PWM2_BASE,
                           ACTIVE_PWM_DUPLICATE_PWM_BASE,
                           (uint16_t)ton_calc_fast, period_calc_fast);
        old_Ton = ton_calc_fast;
        update_flag = 1;
     }
    }
    else
    {
        if (zvs_dont_adj)
            zvs_dont_adj = 0;
    }


#endif
// End - ZVS extension code (oneminusD_adj)

    if(phase_fixed)
        {
            EPWM_enableInterruptEventCountInit(HIGH_FREQ_PWM1_BASE);
            // Force INT CNT to 0 - 0 value set in ETCNTINIT at initialization
            EPWM_forceInterruptEventCountInit(HIGH_FREQ_PWM1_BASE);
        }
    else
        {
            EPWM_disableInterruptEventCountInit(HIGH_FREQ_PWM1_BASE);
        }
    clearInterrupt(C28x_CONTROLISR_INTERRUPT_PIE_GROUP_NO);
    clearPWMInterruptFlag(HIGH_FREQ_PWM1_BASE);
}

//Control ISR Code
__attribute__((ramfunc))
interrupt void controlISR(void)
{
    EPWM_disableInterrupt(HIGH_FREQ_PWM1_BASE);
    HWREGH(HIGH_FREQ_PWM1_BASE + EPWM_O_ETSEL) = 0;

    pfcControlCode();

    clearInterrupt(C28x_CONTROLISR_INTERRUPT_PIE_GROUP_NO);
    clearPWMInterruptFlag(LOW_FREQ_PWM_BASE);

    EPWM_setInterruptSource(HIGH_FREQ_PWM1_BASE,EPWM_INT_TBCTR_U_CMPC);

    EPWM_enableInterrupt(HIGH_FREQ_PWM1_BASE);

    clearPWMInterruptFlag(HIGH_FREQ_PWM1_BASE);

}//Control ISR Ends Here

//10Khz ISR Code
__attribute__((ramfunc))
interrupt void tenKHzISR(void)
{
    EINT;
    instrumentationCode();

}//10Khz ISR Ends Here

//****************************************************************************//
//              ISR roots End
//****************************************************************************//

//==============================================================================
//  STATE-MACHINE SEQUENCING AND SYNCRONIZATION FOR SLOW BACKGROUND TASKS
//==============================================================================

//--------------------------------- SFRAMEWORK ---------------------------------
void A0(void)
{
    // loop rate synchronizer for A-tasks
    if(CPUTimer_getTimerOverflowStatus(CPUTIMER0_BASE) == 1)
    {
        CPUTimer_clearOverflowFlag(CPUTIMER0_BASE); // clear flag
        //-----------------------------------------------------------
        (*A_Task_Ptr)();        // jump to an A Task (A1,A2,A3,...)
        //-----------------------------------------------------------
        vTimer0[0]++;           // virtual timer 0, instance 0 (spare)

    }
    Alpha_State_Ptr = &B0;      // Comment out to allow only A tasks
}

void B0(void)
{
    // loop rate synchronizer for B-tasks
    if(CPUTimer_getTimerOverflowStatus(CPUTIMER1_BASE) == 1)
    {
        CPUTimer_clearOverflowFlag(CPUTIMER1_BASE);             // clear flag
        //-----------------------------------------------------------
        (*B_Task_Ptr)();        // jump to a B Task (B1,B2,B3,...)
        //-----------------------------------------------------------
        vTimer1[0]++;           // virtual timer 1, instance 0 (spare)
    }
    Alpha_State_Ptr = &A0;
}

//==============================================================================
//  A - TASKS
//==============================================================================
//--------------------------------------------------------
void A1(void) //
//--------------------------------------------------------
{
    if(rlyConnect==1)
    {
        closeRelay();
    }
    else
    {
        openRelay();
    }

    if(usEnablePwms == 0) // off
    {
        forceOSTPWMTrip(LOW_FREQ_PWM_BASE);
        forceOSTPWMTrip(HIGH_FREQ_PWM1_BASE);
        forceOSTPWMTrip(HIGH_FREQ_PWM2_BASE);
        EPWM_clearOneShotTripZoneFlag(LOW_FREQ_PWM_BASE,0xFF);
        EPWM_clearOneShotTripZoneFlag(HIGH_FREQ_PWM1_BASE,0xFF);
        EPWM_clearOneShotTripZoneFlag(HIGH_FREQ_PWM2_BASE,0xFF);
        crossoverState = crossoverState_default;
        start_flag = 0;
        softEnable_PWMs = 0;// Disable PWMs
        turnOffFan();
    }
    else
    {
        if((EPWM_getOneShotTripZoneFlagStatus(LOW_FREQ_PWM_BASE) == 0) &&
           (EPWM_getOneShotTripZoneFlagStatus(HIGH_FREQ_PWM1_BASE) == 0) &&
           (EPWM_getOneShotTripZoneFlagStatus(HIGH_FREQ_PWM2_BASE) == 0) &&
           (start_flag == 0))
        {
            softEnable_PWMs = 1; // Enable PWMs at the next zero crossing
            crossoverState = crossoverState_NPlow; //Re-enable for state machine
            start_flag = 1;
            turnOnFan();
        }
    }

    SFRA_F32_runBackgroundTask(&sfra1);
    SFRA_GUI_runSerialHostComms(&sfra1);

    //-------------------
    //the next time CpuTimer0 'counter' reaches Period value go to A2
    A_Task_Ptr = &A2;
    //-------------------
}

//--------------------------------------------------------
void A2(void) // Coefficient update
//--------------------------------------------------------
{
    int k;

    ac_cur_ref_Buff[ac_cur_ref_Buff_Index++]=AC_CUR_ADC_REF_FB;
    if(ac_cur_ref_Buff_Index>=10)
    {
        ac_cur_ref_Buff_Index=0;
    }

    ac_cur_ref_avg_temp = 0;

    for(k=0;k<10;k++)
    {
        ac_cur_ref_avg_temp+= ac_cur_ref_Buff[k];
    }

    ac_cur_ref_avg = ac_cur_ref_avg_temp*0.1f;

    //-------------------
    //the next time CpuTimer0 'counter' reaches Period value go to A1
    A_Task_Ptr = &A1;
    //-------------------
}

//==============================================================================
//  B - TASKS
//==============================================================================

//----------------------------------- USER -------------------------------------

//----------------------------------------
void B1(void) // Not used
//----------------------------------------
{
    //-----------------
    //the next time CpuTimer1 'counter' reaches Period value go to B2
    B_Task_Ptr = &B2;
    //-----------------
}

//----------------------------------------
void B2(void) //  SPARE
//----------------------------------------
{
    ocpThreshold = (pwrste_ocpThreshold*4096.0f/(2.0f*I_MAX_SENSE)) + 2048;

    CMPSS_setDACValueHigh(BOARD_PROTECTION_AC_CUR_CMPSS_BASE, ocpThreshold);
    CMPSS_setDACValueLow(BOARD_PROTECTION_AC_CUR_CMPSS_BASE, (4096-ocpThreshold));
    CMPSS_setDACValueHigh(ZVS_LOSS_DETECT_CMPSS_BASE, zvs_Detect_threshold_pos);

    //-----------------
    //the next time CpuTimer1 'counter' reaches Period value go to B3
    B_Task_Ptr = &B3;
    //-----------------
}

//----------------------------------------
void B3(void) //  SPARE
//----------------------------------------
{
    toggleLED();

#if AUTO_RUN
    if ((guiVrmsEMAvg > UNIVERSAL_GRID_MIN_VRMS) &&
        (guiVrmsEMAvg < UNIVERSAL_GRID_MAX_VRMS) &&
        (start_flag == 0) && (!ovp_Fault))
    {
        if (count_goodVin > 10)
        {
            usEnablePwms = 1;
            vBusRef = (float32_t)(VBUS_TYPICAL-3.0f)/VDCBUS_MAX_SENSE;
        }
        else
        {
            count_goodVin++;
        }
    }
    else
    {
        count_goodVin = 0;
    }

    if ((guiVrmsEMAvg < UNIVERSAL_GRID_MIN_VRMS) ||
        (guiVrmsEMAvg > UNIVERSAL_GRID_MAX_VRMS))
     {
        usEnablePwms = 0;
        count_goodVin = 0;
     }

#endif
    //-----------------
    //the next time CpuTimer1 'counter' reaches Period value go to B1
    B_Task_Ptr = &B1;
    //-----------------
}

void setupSFRA(void)
{
    SFRA_F32_reset(&sfra1);
    SFRA_F32_config(&sfra1,
                    SFRA_ISR_FREQ,
                    SFRA_AMPLITUDE,
                    SFRA_FREQ_LENGTH,
                    SFRA_FREQ_START,
                    SFRA_FREQ_STEP_MULTIPLY,
                    plantMagVect,
                    plantPhaseVect,
                    olMagVect,
                    olPhaseVect,
                    NULL,
                    NULL,
                    freqVect,
                    1);

    SFRA_F32_resetFreqRespArray(&sfra1);

    // Re-initialize the frequency array to make SFRA sweep go fast

    #if SFRA_TYPE==SFRA_CURRENT// current loop

    int16_t i=0;

          sfra1.freqVect[i++]=SFRA_FREQ_START;
         for(;i<sfra1.vecLength;i++)
         {
             if(sfra1.freqVect[i-1]<10)
                 sfra1.freqVect[i]=sfra1.freqVect[i-1]+2;
             else if(sfra1.freqVect[i-1]<50)
                 sfra1.freqVect[i]=sfra1.freqVect[i-1]+10;
             else
                 sfra1.freqVect[i]=sfra1.freqVect[i-1]*sfra1.freqStep;
         }
     #else
         sfra1.freqVect[0]=2;
         sfra1.freqVect[1]=4;
         sfra1.freqVect[2]=6;
         sfra1.freqVect[3]=8;
         sfra1.freqVect[4]=10;
         sfra1.freqVect[5]=12;
         sfra1.freqVect[6]=14;
         sfra1.freqVect[7]=16;
         sfra1.freqVect[8]=18;
         sfra1.freqVect[9]=20;
         sfra1.freqVect[10]=22;
         sfra1.freqVect[11]=24;
         sfra1.freqVect[12]=26;
         sfra1.freqVect[13]=28;
         sfra1.freqVect[14]=30;
         sfra1.freqVect[15]=35;
         sfra1.freqVect[16]=40;
         sfra1.freqVect[17]=45;
         sfra1.freqVect[18]=55;
         sfra1.freqVect[19]=65;
         sfra1.freqVect[20]=70;
         sfra1.freqVect[21]=80;
         sfra1.freqVect[22]=90;
         sfra1.freqVect[23]=130;
         sfra1.freqVect[24]=140;
         sfra1.freqVect[25]=150;
         sfra1.freqVect[26]=160;
         sfra1.freqVect[27]=170;
         sfra1.freqVect[28]=210;
         sfra1.freqVect[29]=250;
     #endif

         //
         //configures the SCI channel for communication with SFRA host GUI
         //to change SCI channel change #defines in the settings.h file
         //the GUI also changes a LED status, this can also be changed with #define
         //in the file pointed to above
         //
         SFRA_GUI_config(SFRA_GUI_SCI_BASE,
         SCI_VBUS_CLK,
         SFRA_GUI_SCI_BAUDRATE,
         SFRA_GUI_SCIRX_GPIO,
         SFRA_GUI_SCIRX_GPIO_PIN_CONFIG,
         SFRA_GUI_SCITX_GPIO,
         SFRA_GUI_SCITX_GPIO_PIN_CONFIG,
         SFRA_GUI_LED_INDICATOR,
         SFRA_GUI_LED_GPIO,
         SFRA_GUI_LED_GPIO_PIN_CONFIG,
         &sfra1,
         1);
}

void globalVariablesInit()
{
    coeff_change = 0;

    gi.Kp=GI_PI_KP;
    gi.Ki=GI_PI_KI;
    new_gi_Kp=GI_PI_KP;
    new_gi_Ki=GI_PI_KI;
    gi.Umax=GI_PI_MAX;
    gi.Umin=GI_PI_MIN;
    gi.i10=0;
    gi.i6=0;
    gi_out = 0;

    gi_pi_max_dbg = GI_PI_MAX;
    gi_pi_min_dbg = GI_PI_MIN;
    gi_pi_min_Set = GI_PI_MIN;

    new_gv_Ki = GV_PI_KI;
    new_gv_Kp = GV_PI_KP;
    gv.Ki=GV_PI_KI;
    gv.Kp=GV_PI_KP;
    gv.Umax=GV_PI_MAX;
    gv.Umin=GV_PI_MIN;
    gv.i10=0;
    gv.i6=0;
    gv_out = 0;

    //Ton weightage factor applied to voltage loop output
    ton_weightage_factor = 0.22f;

    SPLL_1PH_SOGI_FLL_reset(&spll3);
    SPLL_1PH_SOGI_FLL_config(&spll3, AC_FREQ, CONTROL_ISR_FREQUENCY,
                             (float32_t)(222.2862), (float32_t)(-222.034),
                             (float32_t) 0.5, (float32_t) 20000);

    computeNotchFltrCoeff( &notch1, (float32_t)(INSTRUMENTATION_ISR_FREQUENCY) ,
                           (float32_t)(GRID_FREQ*2.0) , 0.25f, 0.00001f);
    computeNotchFltrCoeff( &notch2, (float32_t)(INSTRUMENTATION_ISR_FREQUENCY) ,
                           (float32_t)(GRID_FREQ*2.0) , 0.25f, 0.00001f);

    // Gui Variables
    guiVbus=0;
    guiACFreq=0;
    guiPrms=0;
    guiIrms=0;
    guiVrms=0;
    guiVavg=0;
    guiFreqAvg=0;
    guiVema=0;
    guiVbusMax=420;
    guiVrmsEMAvg=0;

    vRmsAvg=0;
    vBusAvg=0;

    //Variable initialization
    ac_cur_sensed=0;
    ac_cur_sensed_1 = 0;
    ac_cur_sensed_2 = 0;
    ac_cur_sensedOffset=0;

    pwrste_ovpThreshold = VBUS_OVP_THRESHOLD;

    pwrste_ocpThreshold = OCP_THRESHOLD;
    ocpThreshold = (pwrste_ocpThreshold*4096.0f/(2.0f*I_MAX_SENSE)) + 2048;

    vBus_sensed=0;
    vBus_sensedOffset=0;
    vBus_sensedFiltered=0;
//  vBusRef=0.72f;   // ~385V output
    vBusRef=0.0f;

    ac_vol_sns_rect = 0.0f;
    ac_cur_ref=0.03f;
    ac_cur_ref_inst = 0.0;
    acL_sensed=0;
    acL_sensedOffset=0;
    acN_sensed=0;
    acN_sensedOffset=0;
    dutyPU=0;
    dutyPU_DC=0;
    enableiLoop = 0;

#if (INCR_BUILD == 2 || INCR_BUILD==3)  //Closed Current Loop
    enableiLoop_Set = 1;
    zvs_adj_factor = 0.01f;
    phase_shedding_EN = PHASE_SHEDDING_ENABLED;
#else
    enableiLoop_Set = 0;
    zvs_adj_factor = 0.2f;
    phase_shedding_EN = 0;
#endif

    closeGiLoop=0;
    closeGvLoop=0;
    clearTrip=0;
    rlyConnect=0;
    firstTimeGvLoop=1;
    acSine=0;
    acSinePrev=0;
    zeroCrossDetectFlag=0;

    vBus_sensedFiltered_notch1=0;
    vBus_sensedFiltered_notch2=0;

    usDeadBandAdj_Enable = 0;
    usDeadBand_Rise = 6;        //Default - start with 30 ns
    usDeadBand_Fall = 6;        //Default - start with 30 ns
    usDB_RiseSource = 0;  // 0 --> A;  1--> B
    usDB_FallSource = 0;  // 0 --> A;  1--> B
    usEnablePwms = 0;

    usDB_Rise_Mult = sqrtf(2*PFC_INDUCTOR_VALUE*FET_COSS_VALUE);
    zvs_Detect_threshold_neg = 300;
    zvs_Detect_threshold_pos = 300;
    dutyPU_DC=0.0f;
    thetaOffset=0;
    rlyConnect = 1;
    crossoverState = crossoverState_default;
    start_flag = 0;
    softtransition_enable = 1;
    old_period_calc = PERIOD_MAX;
    old_phase = PERIOD_MAX/2;
    update_flag = 0;
    phase_changed = 0;
    phase_fixed = 0;
    state_int_done = 0;

#if AC_SOURCE_TYPE == LOW_1KW_POWER_SOURCE
//    ton_calc_inc_pos = 2.0;
//    ton_calc_inc_neg = 2.0;
//    ton_calc_soft_init = 3.0;

    ton_calc_inc_pos = 3.0f;
    ton_calc_inc_neg = 3.0f;
    ton_calc_soft_init = 1.0f;

    soft_start_count = 5;
#else
    ton_calc_inc_pos = 3.0f;
    ton_calc_inc_neg = 3.0f;
    ton_calc_soft_init = 3.0f;
    soft_start_count = 10;
#endif

    // gv_out of 0.35 ~ 7A. Low line phase re-enable threshold of AC current
    gv_out_dis_shed = AC_CUR_MIN_TH_IN_A*0.05f;
    ac_cur_shed = (gv_out_dis_shed*20.0f) - 0.5f;
    // 7A corresponds to gv_out of 0.35 with single phase enabled and
    // should correspond to 0.35/2 with both phases
    gv_out_shed = (ac_cur_shed*0.025f);
    ac_cur_th = AC_CUR_MIN_TH_IN_A;
    ac_cur_comp = 0.0f;

    ac_cur_max = (float32_t)I_MAX_SENSE;
    ac_cur_max_inv = 1.0f/((float32_t)I_MAX_SENSE);

    ac_cur_mult = ac_cur_max_inv;

}

void computeNotchFltrCoeff(DCL_DF22 *coeff, float32_t Fs, float32_t notch_freq,
                           float32_t c1, float32_t c2)
{
    float32_t temp1;
    float32_t temp2;
    float32_t wn2;
    float32_t Ts;
    Ts=1/Fs;

    // pre warp the notch frequency
    wn2=2*Fs*tanf(notch_freq* PI_VALUE*Ts);

    temp1= 4*Fs*Fs + 4* wn2 * c2 * Fs + wn2*wn2;
    temp2= 1/ ( 4*Fs*Fs + 4* wn2 * c1 * Fs + wn2*wn2);

    coeff->b0 = temp1* temp2;
    coeff->b1 = (-8*Fs*Fs + 2* wn2* wn2)* temp2;
    coeff->b2 = (4*Fs*Fs-4*wn2*c2*Fs+wn2*wn2)*temp2;
    coeff->a1 = (-8*Fs*Fs + 2* wn2* wn2)*temp2;
    coeff->a2 = (4*Fs*Fs-4*wn2*c1*Fs+wn2*wn2)*temp2;
    coeff->x1=0;
    coeff->x2=0;
}
//===========================================================================
// No more.
//===========================================================================




