#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "DSP281x_SWPrioritizedIsrLevels.h"   // DSP281x Examples Include File
#include "DSP281x_Device.h"     // DSP281x Headerfile Include File
#include "IQmathLib.h"

#include <break_regul.h>
#include <params_pwm24.h>
#include <project_setup.h>
#include <PWMTMSHandle.h>
#include <v_pwm24_v2.h>
#include <vector.h>

#include "rmp_cntl_v1.h"
#include "svgen_mf.h"
#include "uf_alg_ing.h"
#include "vhzprof.h"
#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "DSP281x_Device.h"
#include "MemoryFunctions.h"
#include "Spartan2E_Adr.h"
#include "TuneUpPlane.h"
#include "x_wdog.h"
#include "xp_write_xpwm_time.h"








// Частота ШИМ в xilinx тиках (60000000 / 16 / FREQ_PWM = 3750000 / FREQ_PWM)
//#pragma DATA_SECTION(var_freq_pwm_xtics,".fast_vars1");
//int var_freq_pwm_xtics = DEF_FREQ_PWM_XTICS;

// Минимальное значение ШИМа в xilinx тиках
//#pragma DATA_SECTION(var_period_max_xtics,".fast_vars1");
//int var_period_max_xtics = DEF_FREQ_PWM_XTICS - DEF_PERIOD_MIN_XTICS;

// Минимальное значение ШИМа в xilinx тиках (mintime+deadtime) (Fпилы * Tмин.ключа.сек = (60 / 16 / 2) * Tмкс = (60  * Tмкс / 16 / 2))
//#pragma DATA_SECTION(var_period_min_xtics,".fast_vars1");
//int var_period_min_xtics = DEF_PERIOD_MIN_XTICS;//

// Минимальное значение ШИМа в xilinx тиках длЯ тормозных ключей (mintime) (Fпилы * Tмин.ключа.сек = (60 / 16 / 2) * Tмкс = (60  * Tмкс / 16 / 2))
#pragma DATA_SECTION(var_period_min_br_xtics,".fast_vars1");
int var_period_min_br_xtics = DEF_PERIOD_MIN_BR_XTICS;//




#define IQ_ALFA_SATURATION1 15099494//16441671//15099494
#define IQ_ALFA_SATURATION2 1677721//16441671//15099494


#define PI 3.1415926535897932384626433832795

//#pragma DATA_SECTION(iq_alfa_coef,".fast_vars");
//_iq iq_alfa_coef = 16777216;


//#pragma DATA_SECTION(pidCur_Ki,".fast_vars");
//_iq pidCur_Ki = 0;

//#pragma DATA_SECTION(ar_tph,".fast_vars");
//_iq ar_tph[7];

//#pragma DATA_SECTION(winding_displacement,".fast_vars");
//_iq winding_displacement = CONST_IQ_PI6;



#pragma DATA_SECTION(svgen_pwm24_1,".v_24pwm_vars");
SVGEN_PWM24 svgen_pwm24_1 = SVGEN_PWM24_DEFAULTS;
#pragma DATA_SECTION(svgen_pwm24_2,".v_24pwm_vars");
SVGEN_PWM24 svgen_pwm24_2 = SVGEN_PWM24_DEFAULTS;




ALG_PWM24 alg_pwm24 = ALG_PWM24_DEFAULTS;







#pragma CODE_SECTION(start_PWM24,".fast_run2")
void start_PWM24(int O1, int O2)
{
    if ((O1 == 1) && (O2 == 1))
    {
        soft_start_x24_pwm_1_2();
    }
    else
    {
        if ((O1 == 0) && (O2 == 1))
        {
            soft_start_x24_pwm_2();
        }
        if ((O1 == 1) && (O2 == 0))
        {
            soft_start_x24_pwm_1();
        }
    }
}




void InitPWM_Variables(int n_pch)
{

//  init_DQ_pid();
//  break_resistor_managment_init();


/////////////


//////////////
//    a.k = 0;
//    a.k1 = 0;
//    a.k2 = 0;

    alg_pwm24.k1 = 0;
    alg_pwm24.k2 = 0;

    alg_pwm24.freq1 = 0;

///////////////////


///////////////////
    svgen_pwm24_1.prev_level = V_PWM24_PREV_PWM_CLOSE;
    svgen_pwm24_1.saw_direct.all = xpwm_time.saw_direct.all & 0x3f;
    svgen_pwm24_1.Tclosed_saw_direct_0 = xpwm_time.Tclosed_saw_direct_0;//  xpwm_time.Tclosed_high;//var_freq_pwm_xtics + 1;
    svgen_pwm24_1.Tclosed_saw_direct_1 = xpwm_time.Tclosed_saw_direct_1;
    svgen_pwm24_1.Tclosed_high = xpwm_time.Tclosed_high;

    svgen_pwm24_2.prev_level = V_PWM24_PREV_PWM_CLOSE;
    svgen_pwm24_2.saw_direct.all = (xpwm_time.saw_direct.all >> 6) & 0x3f;
    svgen_pwm24_2.Tclosed_saw_direct_0 = xpwm_time.Tclosed_saw_direct_0;//  xpwm_time.Tclosed_high;//var_freq_pwm_xtics + 1;
    svgen_pwm24_2.Tclosed_saw_direct_1 = xpwm_time.Tclosed_saw_direct_1;
    svgen_pwm24_2.Tclosed_high = xpwm_time.Tclosed_high;


    svgen_pwm24_1.XilinxFreq = CONST_IQ_1 / xpwm_time.Tclosed_high;//(var_freq_pwm_xtics + 1);
    svgen_pwm24_2.XilinxFreq = svgen_pwm24_1.XilinxFreq;

    svgen_pwm24_1.number_svgen = 1;
    svgen_pwm24_2.number_svgen = 2;

    //  pwm_minimal_impuls_zero = DEF_PERIOD_MIN_XTICS_80;

    svgen_pwm24_1.pwm_minimal_impuls_zero_minus = (float)DEF_PERIOD_MIN_MKS*1000.0*FREQ_INTERNAL_GENERATOR_XILINX_TMS/1000000000.0;//   DEF_PERIOD_MIN_XTICS_100;//DEF_PERIOD_MIN_XTICS_80;
    svgen_pwm24_1.pwm_minimal_impuls_zero_plus  = (float)DEF_PERIOD_MIN_MKS*1000.0*FREQ_INTERNAL_GENERATOR_XILINX_TMS/1000000000.0;// DEF_PERIOD_MIN_XTICS_80;

    svgen_pwm24_2.pwm_minimal_impuls_zero_minus = svgen_pwm24_1.pwm_minimal_impuls_zero_minus;
    svgen_pwm24_2.pwm_minimal_impuls_zero_plus  = svgen_pwm24_1.pwm_minimal_impuls_zero_plus;


    if (n_pch==0)
    {
        svgen_pwm24_1.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_CBA;
        svgen_pwm24_2.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_CBA;
    }
    else
    {
//        svgen_pwm24_1.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_ACB; // не пошло
//        svgen_pwm24_2.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_ACB;
        svgen_pwm24_1.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_BAC;
        svgen_pwm24_2.phase_sequence = V_PWM24_PHASE_SEQ_REVERS_BAC;
    }


    InitVariablesSvgen_Ing(xpwm_time.freq_pwm);
}










void InitXPWM(unsigned int freq_pwm)
{
    int i;
    unsigned int pwm_t;//, freq_pwm_xtics;




    pwm_t =  (FREQ_INTERNAL_GENERATOR_XILINX_TMS / freq_pwm );
 //   freq_pwm_xtics = (FREQ_INTERNAL_GENERATOR_XILINX_TMS / freq_pwm );

// write init pwm
// ДлЯ первой пилы закрыто, когда выше заданного уровнЯ, длЯ второй ниже
    xpwm_time.Tclosed_saw_direct_1 = pwm_t + 2;//1; // для типа пилы=1 нужно выставить значение выше периода, чтоб шим не открылся
    xpwm_time.Tclosed_saw_direct_0 = 0; // для типа пилы=0 нужно выставить значение ниже периода, чтоб шим не открылся

    xpwm_time.Tclosed_high = pwm_t + 2;//1;

    // выбрали направление пил
    // "направление пил для ШИМа типа=0x0
    //Если SAW_DIRECTbit = 0 то значение пилы>уставки выставит активное включенное состояние=0
    //на выходе ШИМа
    //Если SAW_DIRECTbit = 1 то значение пилы<=уставки выставит активное включенное состояние=0
    //на выходе ШИМа
    xpwm_time.saw_direct.all = 0x0555;


// период в тиках
    xpwm_time.pwm_tics  = pwm_t;
    xpwm_time.freq_pwm  = freq_pwm;
    xpwm_time.half_pwm_tics = xpwm_time.pwm_tics >> 1;

    xpwm_time.one_or_two_interrupts_run = PWN_COUNT_RUN_PER_INTERRUPT;
    xpwm_time.init(&xpwm_time);

// write to xilinx regs
    xpwm_time.write_zero_winding_break_times(&xpwm_time);


//
    i_WriteMemory(ADR_PWM_DIRECT, 0xffff);
    i_WriteMemory(ADR_PWM_DRIVE_MODE, 0); //Choose PWM sourse PWMGenerator on Spartan 200e
    // DeadTime для этого типа ШИМа не используется
    i_WriteMemory(ADR_PWM_DEAD_TIME, 360); //Dead time in tics.
    stop_wdog();

    i_WriteMemory(ADR_PWM_PERIOD, pwm_t); // Saw period in tics. 1 tic = FREQ_INTERNAL_GENERATOR_XILINX_TMS
    // записали выбрали направление пил
    i_WriteMemory(ADR_PWM_SAW_DIRECT, xpwm_time.saw_direct.all);
    //"Маска на линии тк (15 по 0). 0 - маски нет, 1 - маска наложена.
    //Если используется ШИМгенератор Xilinx, то маска применяется либосразу 0x2006(15) = 0,
    //либо в момент прегиба пилы  0x2006(15) = 1, "
    i_WriteMemory(ADR_TK_MASK_0, 0);
    // "Маска на линии тк (31 по 16) Если используется ШИМгенератор Xilinx, то маска применяется либосразу 0x2006(15) = 0,
    // либо в момент прегиба пилы  0x2006(15) = 1, "
    i_WriteMemory(ADR_TK_MASK_1, 0xffff); //Turn off additional 16 tk lines


    i_WriteMemory(ADR_PWM_IT_TYPE, PWN_COUNT_RUN_PER_INTERRUPT);    //1 or 2 interrupt per PWM period

//
//#if (C_PROJECT_TYPE == PROJECT_BALZAM) || (C_PROJECT_TYPE == PROJECT_23550)
//    i_WriteMemory(ADR_PWM_IT_TYPE, 1);    //1 interrupt per PWM period
//#else
//    i_WriteMemory(ADR_PWM_IT_TYPE, 0);    //interrupt on each counter twist
//#endif

/* End оf PWM Gen init */
}



//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////

/*
void init_freq_array(void)
{
	unsigned int i = 0;
	//unsigned int j = 0;
	int var1 = 0;

	var1 = 32767 / (FREQ_PWM_MAX - FREQ_PWM_MIN);

	for (i = 0; i < COUNT_VAR_FREQ; i++)
	{
		//j = rand() / 1023;
		//freq_array[i] = array_optim_freq[j];
		//do
			freq_array[i] = FREQ_PWM_MIN + (rand() / var1);
		//while ((freq_array[i] < 945) && (freq_array[i] > 930));
	}

	//freq_array[0] = 991;
	//freq_array[1] = 1430;
}
*/


//#pragma CODE_SECTION(calc_freq_pwm,".v_24pwm_run");
//#pragma CODE_SECTION(calc_freq_pwm,".fast_run");
/*void calc_freq_pwm()
{
	static int prev_freq_pwm = 0;
	static float pwm_period = 0;
	static float var0 = 0;
	//static int line = 0;
	//static int i = 0;
	static unsigned int proc_ticks = 1;
	int var1 = 0;
	//static int i = 0;

	if ((f.flag_change_pwm_freq == 1) && (f.flag_random_freq == 1))
	{	
		if (proc_ticks >= 1)
		{
			proc_ticks = 0;
			
			
			if (line == 0)
			{
				VAR_FREQ_PWM_HZ = VAR_FREQ_PWM_HZ + 1;
				if (VAR_FREQ_PWM_HZ > FREQ_PWM_MAX)
				{
					VAR_FREQ_PWM_HZ = FREQ_PWM_MAX;
					line = 1;
				}
			}
			else
			{	
				VAR_FREQ_PWM_HZ = VAR_FREQ_PWM_HZ - 1;
				if (VAR_FREQ_PWM_HZ < FREQ_PWM)
				{
					VAR_FREQ_PWM_HZ = FREQ_PWM;
					line = 0;
				}
			}
			



			// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
			//VAR_FREQ_PWM_HZ = freq_array[i];
			//i_led2_on_off(1);

			var1 = 32767 / (freq_pwm_max_hz - freq_pwm_min_hz);
			VAR_FREQ_PWM_HZ = freq_pwm_min_hz + (rand() / var1);

			//i_led2_on_off(0);
			// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

			if (VAR_FREQ_PWM_HZ > freq_pwm_max_hz)
			{
				VAR_FREQ_PWM_HZ = freq_pwm_max_hz;
			}
			else
			{
				if (VAR_FREQ_PWM_HZ < freq_pwm_min_hz)
				{
					VAR_FREQ_PWM_HZ = freq_pwm_min_hz;
				}
			}
			//i++;

			//if (i >= COUNT_VAR_FREQ)
			//{
				//i = 0;
			//}
			
		}
		
		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
		//if (VAR_FREQ_PWM_HZ == FREQ_PWM_MIN)
		//{
			//VAR_FREQ_PWM_HZ = FREQ_PWM_MAX;
		//}
		//else
		//{
			//VAR_FREQ_PWM_HZ = FREQ_PWM_MIN;
		//}

		//if (f.Rele1 == 1)
		//{
			//if (i == 0)
			//{
				//VAR_FREQ_PWM_HZ = 1192;;
				//i = 1;
			//}
			//else
			//{
				//VAR_FREQ_PWM_HZ = 792;
			//}
		//}
		//else
		//{
			//i = 0;
			//VAR_FREQ_PWM_HZ = 1192;
		//}
		// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	}
	//else
	//{
		//VAR_FREQ_PWM_HZ = FREQ_PWM;
	//}

	
	if (prev_freq_pwm != VAR_FREQ_PWM_HZ)
	{
		prev_freq_pwm = VAR_FREQ_PWM_HZ;
		FREQ_MAX = _IQ(2.0*PI*F_STATOR_MAX/VAR_FREQ_PWM_HZ);
	
		var0 = (float)VAR_FREQ_PWM_HZ;
		//pwm_period = ((float64)HSPCLK) / ((float64)VAR_FREQ_PWM_HZ);
	
		pwm_period = HSPCLK / var0;
		
		pwm_period = pwm_period / 2.0;
	
		FREQ_PWM_XTICS = ((int) pwm_period) >> 3;
		
		XILINX_FREQ = 16777216/(FREQ_PWM_XTICS + 1);

		FLAG_CHANGE_FREQ_PWM = 1;
	}

	proc_ticks++;
}
*/
/*
#pragma CODE_SECTION(test_calc_pwm24_dq,".v_24pwm_run");
void test_calc_pwm24_dq(_iq U_zad1, _iq U_zad2,_iq teta)
{
	svgen_pwm24_1.Freq = 0;
	svgen_pwm24_2.Freq = 0;
	
	svgen_pwm24_1.Gain = U_zad1;
	svgen_pwm24_2.Gain = U_zad2;

	svgen_pwm24_1.Alpha = teta;
	svgen_pwm24_2.Alpha = teta;
	
	svgen_pwm24_1.delta_U = filter.iqU_1_fast - filter.iqU_2_fast;
	svgen_pwm24_2.delta_U = filter.iqU_1_fast - filter.iqU_2_fast;
//	svgen_pwm24_2.delta_U = filter.iqU_3_fast - filter.iqU_4_fast;
	
	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	svgen_pwm24_1.delta_U = 0;
	svgen_pwm24_2.delta_U = 0;
	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

	svgen_pwm24_1.Ia = analog.iqIu_1;
	svgen_pwm24_1.Ib = analog.iqIv_1;
	svgen_pwm24_1.Ic = analog.iqIw_1;;

	svgen_pwm24_2.Ia = analog.iqIu_2;
	svgen_pwm24_2.Ib = analog.iqIv_2;
	svgen_pwm24_2.Ic = analog.iqIw_2;

	svgen_pwm24_1.calc_dq(&svgen_pwm24_1);
	svgen_pwm24_2.calc_dq(&svgen_pwm24_2);

	
	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

	set_predel_dshim24(&svgen_pwm24_1.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	set_predel_dshim24(&svgen_pwm24_1.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	
	set_predel_dshim24(&svgen_pwm24_1.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	set_predel_dshim24(&svgen_pwm24_1.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	
	set_predel_dshim24(&svgen_pwm24_1.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	set_predel_dshim24(&svgen_pwm24_1.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	
	set_predel_dshim24(&svgen_pwm24_2.Ta_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	set_predel_dshim24(&svgen_pwm24_2.Ta_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);

	set_predel_dshim24(&svgen_pwm24_2.Tb_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	set_predel_dshim24(&svgen_pwm24_2.Tb_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);

	set_predel_dshim24(&svgen_pwm24_2.Tc_0,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);
	set_predel_dshim24(&svgen_pwm24_2.Tc_1,VAR_PERIOD_MIN_XTICS,VAR_FREQ_PWM_XTICS);


}
*/

/*
#pragma CODE_SECTION(svgen_pwm24_calc,".v_24pwm_run");
void svgen_pwm24_calc_dq(SVGEN_PWM24 *vt)
{
	
	if (vt->Alpha > CONST_IQ_2PI)
	{
		vt->Alpha -= CONST_IQ_2PI;
	}
	
	if (vt->Alpha < 0)
	{
		vt->Alpha += CONST_IQ_2PI;
	}


	calc_time_one_tk(vt->Gain, vt->Alpha, vt->delta_U,  vt->Ia,  vt->Ib, vt->Ic,
					 vt->number_svgen,vt->direct_rotor,   &vt->Ta_0, &vt->Ta_1,&vt->Tb_0, &vt->Tb_1,&vt->Tc_0, &vt->Tc_1);
	
	
	vt->Ta_0.Ti = vt->Ta_0.Ti/vt->XilinxFreq;
	vt->Ta_1.Ti = vt->Ta_1.Ti/vt->XilinxFreq;
	
	vt->Tb_0.Ti = vt->Tb_0.Ti/vt->XilinxFreq;
	vt->Tb_1.Ti = vt->Tb_1.Ti/vt->XilinxFreq;
	
	vt->Tc_0.Ti = vt->Tc_0.Ti/vt->XilinxFreq;
	vt->Tc_1.Ti = vt->Tc_1.Ti/vt->XilinxFreq;

}
*/




void svgen_set_time_keys_closed(SVGEN_PWM24 *vt)
{
//a
    if (vt->saw_direct.bits.bit0)
        vt->Ta_0 = vt->Tclosed_saw_direct_1;
    else
        vt->Ta_0 = vt->Tclosed_saw_direct_0;

    if (vt->saw_direct.bits.bit1)
        vt->Ta_1 = vt->Tclosed_saw_direct_1;
    else
        vt->Ta_1 = vt->Tclosed_saw_direct_0;
//b
    if (vt->saw_direct.bits.bit2)
        vt->Tb_0 = vt->Tclosed_saw_direct_1;
    else
        vt->Tb_0 = vt->Tclosed_saw_direct_0;

    if (vt->saw_direct.bits.bit3)
        vt->Tb_1 = vt->Tclosed_saw_direct_1;
    else
        vt->Tb_1 = vt->Tclosed_saw_direct_0;
//c
    if (vt->saw_direct.bits.bit4)
        vt->Tc_0 = vt->Tclosed_saw_direct_1;
    else
        vt->Tc_0 = vt->Tclosed_saw_direct_0;

    if (vt->saw_direct.bits.bit5)
        vt->Tc_1 = vt->Tclosed_saw_direct_1;
    else
        vt->Tc_1 = vt->Tclosed_saw_direct_0;

    vt->prev_level = V_PWM24_PREV_PWM_CLOSE;

}





#pragma CODE_SECTION(correct_balance_uzpt_pwm24,".fast_run2");
_iq correct_balance_uzpt_pwm24(_iq Tinput, _iq Kplus)
{
//_iq pwm_t, timpuls_corr;
    volatile _iq timpuls_corr;

    if (Tinput >= (-Kplus))
        timpuls_corr = CONST_IQ_1 - _IQdiv(CONST_IQ_1-Tinput, CONST_IQ_1+Kplus);
    else
        timpuls_corr = -CONST_IQ_1 + _IQdiv(CONST_IQ_1+Tinput, CONST_IQ_1-Kplus);


    return timpuls_corr;
}





#pragma CODE_SECTION(recalc_time_pwm_minimal_2_xilinx_pwm24,".fast_run2");
void recalc_time_pwm_minimal_2_xilinx_pwm24(SVGEN_PWM24 *pwm24,
                                            unsigned int *T0, unsigned int *T1,
                                            int *T_imp,
                                            _iq timpuls_corr )
{
//_iq pwm_t, timpuls_corr;

    volatile unsigned long pwm_t;
    volatile unsigned int minimal_plus, minimal_minus;



    minimal_plus = pwm24->pwm_minimal_impuls_zero_plus;
    minimal_minus = pwm24->pwm_minimal_impuls_zero_minus;

 //   if (pwm24->prev_level == V_PWM24_PREV_PWM_CLOSE || pwm24->prev_level == V_PWM24_PREV_PWM_MIDDLE || pwm24->prev_level == V_PWM24_PREV_PWM_WORK_KM0)
 //   {
 //       minimal_plus *= 2;
//        minimal_minus *= 2;
//    }

    pwm_t = timpuls_corr / pwm24->XilinxFreq;

    *T_imp =  pwm_t;

//    if (pwm_t>(pwm24->Tclosed_high-4*minimal_minus))
//        pwm_t=(pwm24->Tclosed_high-4*minimal_minus);


    if (timpuls_corr >= 0)
    {
        *T0 = pwm_t + minimal_plus;
        *T1 = pwm24->Tclosed_high - minimal_minus;
    }
    else
    {
        *T0 = minimal_plus;
        *T1 = pwm24->Tclosed_high + pwm_t - minimal_minus;
    }


    if (*T0 < minimal_plus)
        *T0 = minimal_plus;

    if (*T0 > (pwm24->Tclosed_high - 2 * minimal_plus))
        *T0 = (pwm24->Tclosed_high - 2 * minimal_plus);

    if (*T1 < (2 * minimal_minus))
        *T1 = 2 * minimal_minus;

    if (*T1 > (pwm24->Tclosed_high - minimal_minus))
        *T1 = (pwm24->Tclosed_high - minimal_minus);

}

#define WRITE_SWGEN_PWM_TIMES_VER   2//1

#if (WRITE_SWGEN_PWM_TIMES_VER==1)
#pragma CODE_SECTION(write_swgen_pwm_times,".fast_run2");
void write_swgen_pwm_times(unsigned int mode_reload)
{

    if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_ABC)
    {
        xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Ta_0;
        xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Ta_1;
        xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tb_0;
        xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tb_1;
        xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tc_0;
        xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tc_1;
    }

    if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_ABC)
    {
        xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Ta_0;
        xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Ta_1;
        xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tb_0;
        xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tb_1;
        xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tc_0;
        xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tc_1;
    }

    if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_BCA)
    {
        xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tb_0;
        xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tb_1;
        xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tc_0;
        xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tc_1;
        xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Ta_0;
        xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Ta_1;
    }

    if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_BCA)
    {
        xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tb_0;
        xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tb_1;
        xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tc_0;
        xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tc_1;
        xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Ta_0;
        xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Ta_1;
    }

    if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_CAB)
    {
        xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tc_0;
        xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tc_1;
        xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Ta_0;
        xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Ta_1;
        xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tb_0;
        xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tb_1;
    }
    if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_NORMAL_CAB)
    {
        xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tc_0;
        xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tc_1;
        xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Ta_0;
        xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Ta_1;
        xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tb_0;
        xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tb_1;
    }

    // fix revers
    if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_BAC)
    {
        xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tb_0;
        xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tb_1;
        xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Ta_0;
        xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Ta_1;
        xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tc_0;
        xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tc_1;
    }
    if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_BAC)
    {
        xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tb_0;
        xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tb_1;
        xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Ta_0;
        xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Ta_1;
        xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tc_0;
        xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tc_1;
    }

    if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_ACB)
    {
        xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Ta_0;
        xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Ta_1;
        xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tc_0;
        xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tc_1;
        xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tb_0;
        xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tb_1;
    }
    if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_ACB)
    {
        xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Ta_0;
        xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Ta_1;
        xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tc_0;
        xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tc_1;
        xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tb_0;
        xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tb_1;
    }

    if (svgen_pwm24_1.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_CBA)
    {
        xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Tc_0;
        xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Tc_1;
        xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tb_0;
        xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tb_1;
        xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Ta_0;
        xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Ta_1;
    }
    if (svgen_pwm24_2.phase_sequence == V_PWM24_PHASE_SEQ_REVERS_CBA)
    {
        xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Tc_0;
        xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Tc_1;
        xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tb_0;
        xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tb_1;
        xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Ta_0;
        xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Ta_1;
    }

    xpwm_time.Tbr0_0 = break_result_1;
    xpwm_time.Tbr0_1 = break_result_2;
    xpwm_time.Tbr1_0 = 0;//break_result_3;
    xpwm_time.Tbr1_1 = 0;//break_result_4;
    xpwm_time.mode_reload = mode_reload;

    xpwm_time.write_1_2_winding_break_times(&xpwm_time);
}
#endif
///////////////////////////////////////////////////////
// ver 2
///////////////////////////////////////////////////////
#if (WRITE_SWGEN_PWM_TIMES_VER==2)

#pragma CODE_SECTION(set_pwm_times,".fast_run2");
void set_pwm_times(unsigned int Ta0, unsigned int Ta1, unsigned int Tb0, unsigned int Tb1, unsigned int Tc0, unsigned int Tc1, unsigned int winding_num)
{
    if (winding_num == 0)
    {
        xpwm_time.Ta0_0 = Ta0;
        xpwm_time.Ta0_1 = Ta1;
        xpwm_time.Tb0_0 = Tb0;
        xpwm_time.Tb0_1 = Tb1;
        xpwm_time.Tc0_0 = Tc0;
        xpwm_time.Tc0_1 = Tc1;
    }
    else
    {
        xpwm_time.Ta1_0 = Ta0;
        xpwm_time.Ta1_1 = Ta1;
        xpwm_time.Tb1_0 = Tb0;
        xpwm_time.Tb1_1 = Tb1;
        xpwm_time.Tc1_0 = Tc0;
        xpwm_time.Tc1_1 = Tc1;
    }
}

#pragma CODE_SECTION(process_phase_sequence,".fast_run2");
void process_phase_sequence(SVGEN_PWM24 svgen_pwm, unsigned int winding_num)
{
    switch (svgen_pwm.phase_sequence)
    {
        case V_PWM24_PHASE_SEQ_NORMAL_ABC:
            set_pwm_times(svgen_pwm.Ta_0, svgen_pwm.Ta_1, svgen_pwm.Tb_0, svgen_pwm.Tb_1, svgen_pwm.Tc_0, svgen_pwm.Tc_1, winding_num);
            break;
        case V_PWM24_PHASE_SEQ_NORMAL_BCA:
            set_pwm_times(svgen_pwm.Tb_0, svgen_pwm.Tb_1, svgen_pwm.Tc_0, svgen_pwm.Tc_1, svgen_pwm.Ta_0, svgen_pwm.Ta_1, winding_num);
            break;
        case V_PWM24_PHASE_SEQ_NORMAL_CAB:
            set_pwm_times(svgen_pwm.Tc_0, svgen_pwm.Tc_1, svgen_pwm.Ta_0, svgen_pwm.Ta_1, svgen_pwm.Tb_0, svgen_pwm.Tb_1, winding_num);
            break;
        case V_PWM24_PHASE_SEQ_REVERS_BAC:
            set_pwm_times(svgen_pwm.Tb_0, svgen_pwm.Tb_1, svgen_pwm.Ta_0, svgen_pwm.Ta_1, svgen_pwm.Tc_0, svgen_pwm.Tc_1, winding_num);
            break;
        case V_PWM24_PHASE_SEQ_REVERS_ACB:
            set_pwm_times(svgen_pwm.Ta_0, svgen_pwm.Ta_1, svgen_pwm.Tc_0, svgen_pwm.Tc_1, svgen_pwm.Tb_0, svgen_pwm.Tb_1, winding_num);
            break;
        case V_PWM24_PHASE_SEQ_REVERS_CBA:
            set_pwm_times(svgen_pwm.Tc_0, svgen_pwm.Tc_1, svgen_pwm.Tb_0, svgen_pwm.Tb_1, svgen_pwm.Ta_0, svgen_pwm.Ta_1, winding_num);
            break;
    }
}
#pragma CODE_SECTION(write_swgen_pwm_times,".fast_run2");
void write_swgen_pwm_times(unsigned int mode_reload)
{
    process_phase_sequence(svgen_pwm24_1, 0);
    process_phase_sequence(svgen_pwm24_2, 1);

    // fix breaks
    xpwm_time.Tbr0_0 = break_result_1;
    xpwm_time.Tbr0_1 = break_result_2;
    xpwm_time.Tbr1_0 = 0; // break_result_3;
    xpwm_time.Tbr1_1 = 0; // break_result_4;
    xpwm_time.mode_reload = mode_reload;

    xpwm_time.write_1_2_winding_break_times(&xpwm_time);
}
#endif








///////////////////////////////////////////////////////

void svgen_set_time_middle_keys_open(SVGEN_PWM24 *vt)
{
    //a
        if (vt->saw_direct.bits.bit0)
            vt->Ta_0 = vt->Tclosed_saw_direct_0;
        else
            vt->Ta_0 = vt->Tclosed_saw_direct_1;

        if (vt->saw_direct.bits.bit1)
            vt->Ta_1 = vt->Tclosed_saw_direct_0;
        else
            vt->Ta_1 = vt->Tclosed_saw_direct_1;
    //b
        if (vt->saw_direct.bits.bit2)
            vt->Tb_0 = vt->Tclosed_saw_direct_0;
        else
            vt->Tb_0 = vt->Tclosed_saw_direct_1;

        if (vt->saw_direct.bits.bit3)
            vt->Tb_1 = vt->Tclosed_saw_direct_0;
        else
            vt->Tb_1 = vt->Tclosed_saw_direct_1;
    //c
        if (vt->saw_direct.bits.bit4)
            vt->Tc_0 = vt->Tclosed_saw_direct_0;
        else
            vt->Tc_0 = vt->Tclosed_saw_direct_1;

        if (vt->saw_direct.bits.bit5)
            vt->Tc_1 = vt->Tclosed_saw_direct_0;
        else
            vt->Tc_1 = vt->Tclosed_saw_direct_1;


        vt->prev_level = V_PWM24_PREV_PWM_MIDDLE;

/*

    vt->Ta_0 = 0;
    vt->Ta_1 = vt->Tclosed;//var_freq_pwm_xtics + 1;

    vt->Tb_0 = 0;
    vt->Tb_1 = vt->Tclosed;// var_freq_pwm_xtics + 1;

    vt->Tc_0 = 0;
    vt->Tc_1 = vt->Tclosed;// var_freq_pwm_xtics + 1;
    */
}

///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////


#pragma CODE_SECTION(detect_level_interrupt,".fast_run");
unsigned int detect_level_interrupt(int flag_second_PCH)
{
   unsigned int curr_period1, curr_period2, curr_period0;
   static unsigned int count_err_read_pwm_xilinx = 0;


   WriteMemory(ADR_SAW_REQUEST, 0x8000);
   curr_period0 = ReadMemory(ADR_SAW_VALUE);
   WriteMemory(ADR_SAW_REQUEST, 0x8000);
   curr_period1 = ReadMemory(ADR_SAW_VALUE);
   WriteMemory(ADR_SAW_REQUEST, 0x8000);
   curr_period2 = ReadMemory(ADR_SAW_VALUE);

   xpwm_time.current_period = curr_period2;


   // мы находимся в нижней части пилы?
   if (xpwm_time.current_period<xpwm_time.half_pwm_tics)
   {
       xpwm_time.where_interrupt = PWM_LOW_LEVEL_INTERRUPT;
       xpwm_time.what_next_interrupt = PWM_HIGH_LEVEL_INTERRUPT;
       xpwm_time.do_sync_out = (flag_second_PCH==0);
   }
   else   // мы находимся в верхней части пилы?
   {
       xpwm_time.where_interrupt = PWM_HIGH_LEVEL_INTERRUPT;
       xpwm_time.what_next_interrupt = PWM_LOW_LEVEL_INTERRUPT;
       xpwm_time.do_sync_out = !(flag_second_PCH==0);
   }

// по идее этот код выполнится при каких то очень больших расхождений в показаниях
   if (curr_period2>curr_period1) // идем вверх по пиле
   {
       if  ((curr_period2-curr_period1)>xpwm_time.half_pwm_tics)// очень большой разброс
       {
//           xpwm_time.what_next_interrupt = 1;
           // возвращаем ошибку
           return 1;
       }
   }
   else// идем вниз по пиле
   {
       if  ((curr_period1-curr_period2)>xpwm_time.half_pwm_tics)// очень большой разброс
       {
           // возвращаем ошибку
           return 1;
       }
   }

   // нет ошибок, ок!
   return 0;


}