/**************************************************************************
	Description: Программа моделирует работу процессора - осуществляет
	вызов функций init28335, detcoeff, isr.
	Также моделирует различные периферийные устройства процессора
	TMS320F28335/TMS320F28379D (ADC, PWM, QEP и т.д.).

	Автор: Улитовский Д.И.
	Дата последнего обновления: 2021.11.08
**************************************************************************/


#include "simstruc.h"
#include "controller.h"
#include "init28335.h"


extern UMotorMeasure motor;

extern TimerSimHandle t1sim;
extern TimerSimHandle t2sim;
extern TimerSimHandle t3sim;
extern TimerSimHandle t4sim;
extern TimerSimHandle t5sim;
extern TimerSimHandle t6sim;
extern TimerSimHandle t7sim;
extern TimerSimHandle t8sim;
extern TimerSimHandle t9sim;
extern TimerSimHandle t10sim;
extern TimerSimHandle t11sim;
extern TimerSimHandle t12sim;

extern DeadBandSimHandle dt1sim;
extern DeadBandSimHandle dt2sim;
extern DeadBandSimHandle dt3sim;
extern DeadBandSimHandle dt4sim;
extern DeadBandSimHandle dt5sim;
extern DeadBandSimHandle dt6sim;
extern DeadBandSimHandle dt7sim;
extern DeadBandSimHandle dt8sim;
extern DeadBandSimHandle dt9sim;
extern DeadBandSimHandle dt10sim;
extern DeadBandSimHandle dt11sim;
extern DeadBandSimHandle dt12sim;

extern void init28335(void);
extern void detcoeff(void);
extern void isr(void);
extern void input_param(unsigned short num, unsigned short val);
// extern void init_DQ_pid();
int calcAlgUpr = 0;

int timers_adc = 0;
int timers_pwm = 0;




void readInputParameters(const real_T *u) {
 // int t;
	//// ВХОДЫ (begin)
	//nn = 0;
	//// аналоговые величины
	//motor.udc1_ml = u[nn++];//В
	//motor.udc2_ml = u[nn++];//В

	//motor.ia1_ml = u[nn++];//А
	//motor.ib1_ml = u[nn++];//А
	//motor.ic1_ml = u[nn++];//А
	//motor.ia2_ml = u[nn++];//А
	//motor.ib2_ml = u[nn++];//А
	//motor.ic2_ml = u[nn++];//А
	//motor.wm_ml = u[nn++];//рад/с

	//// управление (например, с ВУ)
	//mst.faultReset = (unsigned short)u[nn++];
	//mst.start = (unsigned short)u[nn++];
	//mst.pzMode = (unsigned short)u[nn++];
	//mst.wmZz = u[nn++];//o.e. (от N_BAZ)
	//mst.pmZz = u[nn++]*(P_NOM/S_BAZ);//o.e. (от S_BAZ)
	//mst.wmLim = u[nn++];//o.e. (от N_BAZ)
	//mst.pmLim = u[nn++]*(P_NOM/S_BAZ);//o.e. (от S_BAZ)
	//mst.pIncrMaxTy = u[nn++]*TY*DECIM_PSI_WM_PM*(P_NOM/S_BAZ);//o.e. (от S_BAZ)
	//mst.pDecrMaxTy = u[nn++]*TY*DECIM_PSI_WM_PM*(P_NOM/S_BAZ);//o.e. (от S_BAZ)

	//// 
	//mst.off_curr_pi = (unsigned short)u[nn++];
	//mst.only_one_km = (unsigned short)u[nn++];
	//mst.enable_compens_iq1_iq2 = (unsigned short)u[nn++];
	//mst.pi_iq_ki = u[nn++];
	//mst.pi_id_ki = u[nn++];
	//t = (unsigned short)u[nn++];
	//t = (unsigned short)u[nn++];
	//t = (unsigned short)u[nn++];
	//t = (unsigned short)u[nn++];
	//t = (unsigned short)u[nn++];


	//// параметры (например, с ПУ)
	//paramNo = FIRST_WRITE_PAR_NUM;
	//paramNew[paramNo++] = (unsigned short)u[nn++];
	//paramNew[paramNo++] = (unsigned short)u[nn++];


	//// ВХОДЫ (end)


}


void processSFunctionIfChanged(SimStruct *S, int_T *iW) {

	// обрабатываем параметры S-Function каждый раз, когда они изменились
	if ( iW[0] == 1 ) {
		iW[0] = 0;
		kkk = 0;
		for ( lll = 0; lll < NPARAMS; lll++ ) {
			// определяем кол-во элементов в параметре
			dimen = mxGetNumberOfElements(ssGetSFcnParam(S,lll));
			// обрабатываем параметр в зависимости от его размера
			if ( dimen > LEN_PARAM_MATR*2 ) {
				ssSetErrorStatus(S,"В параметре-массиве слишком много элементов");
				return;
			}
			else if ( dimen > 1 ) {
				// запоминаем кол-во элементов параметра-матрицы
				paramMatrDimen = dimen;
				// запоминаем значения элементов параметра-матрицы
				for ( mmm = 0; mmm < dimen; mmm++ )
					paramMatr[mmm] = mxGetPr(ssGetSFcnParam(S,lll))[mmm];
			}
			else {
				// запоминаем значения параметров-скаляров
				paramScal[kkk++] = mxGetPr(ssGetSFcnParam(S,lll))[0];
			}
		}
		// ПАРАМЕТРЫ (begin)
		nn = 0;
		dt = paramScal[nn++];//шаг дискретизации (всегда должен передаваться в S-function последним!)
		// ПАРАМЕТРЫ (end)
	} //if ( iW[0] == 1 )

}

void initialisationOnStart(int_T *iW) {

	// кое-что выполняем один раз при запуске модели
	if ( iW[1] == 1 ) {
		iW[1] = 0;

		init28335();
	} //if ( iW[1] == 1 )


}

void simulatePWMcounterAndReadComarators(void) {

	//// Моделируем Time-Base Submodule, Counter-Compare Submodule и
	//// Event-Trigger Submodule
	//// ePWM1 (up-down-count mode)
	//// -------------------------
	//t1cntAuxPrev = t1cntAux;
	//t1cntAux += TxCntPlus;
	//if ( t1cntAux > T1Pr ) {
	//	t1cntAux -= T1Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp1A = (double)EPwm1Regs.CMPA.half.CMPA;
	//	// запуск АЦП
	//	// tAdc = Tadc;
	//	// nAdc = 0;
	//	calcAlgUpr = 1; // ШИМ прерывание
	//}
	//if ( (t1cntAuxPrev < 0) && (t1cntAux >= 0) ) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp1A = (double)EPwm1Regs.CMPA.half.CMPA;
	//	// запуск АЦП
	//	// tAdc = Tadc;
	//	// nAdc = 0;
	//	calcAlgUpr = 1; // ШИМ прерывание
	//}
	//t1cnt = fabs(t1cntAux);

	//// ePWM2 (up-down-count mode)
	//// -------------------------
	//t2cntAuxPrev = t2cntAux;
	//t2cntAux += TxCntPlus;
	//if ( t2cntAux > T2Pr ) {
	//	t2cntAux -= T2Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp2A = (double)EPwm2Regs.CMPA.half.CMPA;
	//}
	//if ( (t2cntAuxPrev < 0) && (t2cntAux >= 0) ) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp2A = (double)EPwm2Regs.CMPA.half.CMPA;
	//}
	//t2cnt = fabs(t2cntAux);

	//// ePWM3 (up-down-count mode)
	//// -------------------------
	//t3cntAuxPrev = t3cntAux;
	//t3cntAux += TxCntPlus;
	//if ( t3cntAux > T3Pr ) {
	//	t3cntAux -= T3Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp3A = (double)EPwm3Regs.CMPA.half.CMPA;
	//}
	//if ( (t3cntAuxPrev < 0) && (t3cntAux >= 0) ) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp3A = (double)EPwm3Regs.CMPA.half.CMPA;
	//}
	//t3cnt = fabs(t3cntAux);

	//// ePWM4 (up-down-count mode)
	//// -------------------------
	//t4cntAuxPrev = t4cntAux;
	//t4cntAux += TxCntPlus;
	//if ( t4cntAux > T4Pr ) {
	//	t4cntAux -= T4Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp4A = (double)EPwm4Regs.CMPA.half.CMPA;
	//}
	//if ( (t4cntAuxPrev < 0) && (t4cntAux >= 0) ) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp4A = (double)EPwm4Regs.CMPA.half.CMPA;
	//}
	//t4cnt = fabs(t4cntAux);

	//// ePWM5 (up-down-count mode)
	//// -------------------------
	//t5cntAuxPrev = t5cntAux;
	//t5cntAux += TxCntPlus;
	//if ( t5cntAux > T5Pr ) {
	//	t5cntAux -= T5Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp5A = (double)EPwm5Regs.CMPA.half.CMPA;
	//}
	//if ( (t5cntAuxPrev < 0) && (t5cntAux >= 0) ) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp5A = (double)EPwm5Regs.CMPA.half.CMPA;
	//}
	//t5cnt = fabs(t5cntAux);

	//// ePWM6 (up-down-count mode)
	//// -------------------------
	//t6cntAuxPrev = t6cntAux;
	//t6cntAux += TxCntPlus;
	//if ( t6cntAux > T6Pr ) {
	//	t6cntAux -= T6Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp6A = (double)EPwm6Regs.CMPA.half.CMPA;
	//}
	//if ( (t6cntAuxPrev < 0) && (t6cntAux >= 0) ) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp6A = (double)EPwm6Regs.CMPA.half.CMPA;
	//}
	//t6cnt = fabs(t6cntAux);

	//// ePWM7 (up-down-count mode)
	//// -------------------------
	//t7cntAuxPrev = t7cntAux;
	//t7cntAux += TxCntPlus;
	//if (t7cntAux > T7Pr) {
	//	t7cntAux -= T7Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp7A = (double)EPwm7Regs.CMPA.half.CMPA;
	//}
	//if ( (t7cntAuxPrev < 0) && (t7cntAux >= 0) ) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp7A = (double)EPwm7Regs.CMPA.half.CMPA;
	//}
	//t7cnt = fabs(t7cntAux);

	//// ePWM8 (up-down-count mode)
	//// -------------------------
	//t8cntAuxPrev = t8cntAux;
	//t8cntAux += TxCntPlus;
	//if(t8cntAux > T8Pr) {
	//	t8cntAux -= T8Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp8A = (double)EPwm8Regs.CMPA.half.CMPA;
	//}
	//if((t8cntAuxPrev < 0) && (t8cntAux >= 0)) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp8A = (double)EPwm8Regs.CMPA.half.CMPA;
	//}
	//t8cnt = fabs(t8cntAux);

	//// ePWM9 (up-down-count mode)
	//// -------------------------
	//t9cntAuxPrev = t9cntAux;
	//t9cntAux += TxCntPlus;
	//if(t9cntAux > T9Pr) {
	//	t9cntAux -= T9Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp9A = (double)EPwm9Regs.CMPA.half.CMPA;
	//}
	//if((t9cntAuxPrev < 0) && (t9cntAux >= 0)) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp9A = (double)EPwm9Regs.CMPA.half.CMPA;
	//}
	//t9cnt = fabs(t9cntAux);

	//// ePWM10 (up-down-count mode)
	//// -------------------------
	//t10cntAuxPrev = t10cntAux;
	//t10cntAux += TxCntPlus;
	//if(t10cntAux > T10Pr) {
	//	t10cntAux -= T10Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp10A = (double)EPwm10Regs.CMPA.half.CMPA;
	//}
	//if((t10cntAuxPrev < 0) && (t10cntAux >= 0)) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp10A = (double)EPwm10Regs.CMPA.half.CMPA;
	//}
	//t10cnt = fabs(t10cntAux);

	//// ePWM11 (up-down-count mode)
	//// -------------------------
	//t11cntAuxPrev = t11cntAux;
	//t11cntAux += TxCntPlus;
	//if(t11cntAux > T11Pr) {
	//	t11cntAux -= T11Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp11A = (double)EPwm11Regs.CMPA.half.CMPA;
	//}
	//if((t11cntAuxPrev < 0) && (t11cntAux >= 0)) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp11A = (double)EPwm11Regs.CMPA.half.CMPA;
	//}
	//t11cnt = fabs(t11cntAux);

	//// ePWM12 (up-down-count mode)
	//// -------------------------
	//t12cntAuxPrev = t12cntAux;
	//t12cntAux += TxCntPlus;
	//if(t12cntAux > T12Pr) {
	//	t12cntAux -= T12Pr*2.;
	//	// active CMPA load from shadow when TBCTR == TBPRD
	//	cmp12A = (double)EPwm12Regs.CMPA.half.CMPA;
	//}
	//if((t12cntAuxPrev < 0) && (t12cntAux >= 0)) {
	//	// active CMPA load from shadow when TBCTR == 0
	//	cmp12A = (double)EPwm12Regs.CMPA.half.CMPA;
	//}
	//t12cnt = fabs(t12cntAux);


	//// Моделируем работу счётчика в eQEP
	//qposcnt += wm_ml/PI2*NOP*4.*dt;
	//if ( qposcnt >= (Qposmax + 1.) )
	//	qposcnt -= (Qposmax + 1.);
	//else if ( qposcnt < 0 )
	//	qposcnt += (Qposmax + 1.);
	//EQep2Regs.QPOSCNT = (short)qposcnt;

}


void update_norm_ADC_array()
{
			//// Udc1
			//if ( udc1_ml > UDC_SENS_MAX )
			//	udc1_ml = UDC_SENS_MAX;
			//else if ( udc1_ml < -UDC_SENS_MAX )
			//	udc1_ml = -UDC_SENS_MAX;
			//iq_norm_ADC[0] = _IQ(udc1_ml/NORMA_ACP);
			//// Udc2
			//if ( udc2_ml > UDC_SENS_MAX )
			//	udc2_ml = UDC_SENS_MAX;
			//else if ( udc2_ml < -UDC_SENS_MAX )
			//	udc2_ml = -UDC_SENS_MAX;
			//iq_norm_ADC[1] = _IQ(udc2_ml/NORMA_ACP);
			//// Udc3
			//if ( udc3_ml > UDC_SENS_MAX )
			//	udc3_ml = UDC_SENS_MAX;
			//else if ( udc3_ml < -UDC_SENS_MAX )
			//	udc3_ml = -UDC_SENS_MAX;
			//iq_norm_ADC[7] = _IQ(udc3_ml/NORMA_ACP);
			//// Udc4
			//if ( udc4_ml > UDC_SENS_MAX )
			//	udc4_ml = UDC_SENS_MAX;
			//else if ( udc4_ml < -UDC_SENS_MAX )
			//	udc4_ml = -UDC_SENS_MAX;
			//iq_norm_ADC[8] = _IQ(udc4_ml/NORMA_ACP);
			//// Idc1
			//if ( idc1_ml > IDC_SENS_MAX )
			//	idc1_ml = IDC_SENS_MAX;
			//else if ( idc1_ml < -IDC_SENS_MAX )
			//	idc1_ml = -IDC_SENS_MAX;
			//iq_norm_ADC[2] = _IQ(idc1_ml/NORMA_ACP);
			//// Idc2
			//if ( idc2_ml > IDC_SENS_MAX )
			//	idc2_ml = IDC_SENS_MAX;
			//else if ( idc2_ml < -IDC_SENS_MAX )
			//	idc2_ml = -IDC_SENS_MAX;
			//iq_norm_ADC[3] = _IQ(idc2_ml/NORMA_ACP);
			//// Idc3
			//if ( idc3_ml > IDC_SENS_MAX )
			//	idc3_ml = IDC_SENS_MAX;
			//else if ( idc3_ml < -IDC_SENS_MAX )
			//	idc3_ml = -IDC_SENS_MAX;
			//iq_norm_ADC[9] = _IQ(idc3_ml/NORMA_ACP);
			//// Idc4
			//if ( idc4_ml > IDC_SENS_MAX )
			//	idc4_ml = IDC_SENS_MAX;
			//else if ( idc4_ml < -IDC_SENS_MAX )
			//	idc4_ml = -IDC_SENS_MAX;
			//iq_norm_ADC[10] = _IQ(idc4_ml/NORMA_ACP);

			//// Ia1
			//if ( ia1_ml > IAC_SENS_MAX )
			//	ia1_ml = IAC_SENS_MAX;
			//else if ( ia1_ml < -IAC_SENS_MAX )
			//	ia1_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[4] = _IQ(ia1_ml/NORMA_ACP);

			//// Ib1
			//if ( ib1_ml > IAC_SENS_MAX )
			//	ib1_ml = IAC_SENS_MAX;
			//else if ( ib1_ml < -IAC_SENS_MAX )
			//	ib1_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[5] = _IQ(ib1_ml/NORMA_ACP);

			//// Ic1
			//if ( ic1_ml > IAC_SENS_MAX )
			//	ic1_ml = IAC_SENS_MAX;
			//else if ( ic1_ml < -IAC_SENS_MAX )
			//	ic1_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[6] = _IQ(ic1_ml/NORMA_ACP);


			//// Ia2
			//if ( ia2_ml > IAC_SENS_MAX )
			//	ia2_ml = IAC_SENS_MAX;
			//else if ( ia2_ml < -IAC_SENS_MAX )
			//	ia2_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[11] = _IQ(ia2_ml/NORMA_ACP);

			//// Ib2
			//if ( ib2_ml > IAC_SENS_MAX )
			//	ib2_ml = IAC_SENS_MAX;
			//else if ( ib2_ml < -IAC_SENS_MAX )
			//	ib2_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[12] = _IQ(ib2_ml/NORMA_ACP);

			//// Ic2
			//if ( ic2_ml > IAC_SENS_MAX )
			//	ic2_ml = IAC_SENS_MAX;
			//else if ( ic2_ml < -IAC_SENS_MAX )
			//	ic2_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[13] = _IQ(ic2_ml/NORMA_ACP);



			//vect_control.off_curr_pi = mst.off_curr_pi;
			//vect_control.only_one_km = mst.only_one_km;
			//vect_control.enable_compens_iq1_iq2 = mst.enable_compens_iq1_iq2;
}

void simulateAdcAndCallIsr() {

	///* Моделируем преобразования измеряемых величин датчиками,
	//операционниками и АЦП (с помощью nAdc учитываем сдвиг по времени
	//между АЦП разных сигналов) */
	//if ( tAdc < Tadc ) {
	//	tAdc++;
	//}
	//else {

	//	tAdc = 1;
	//	timers_adc++;
	//	if (timers_adc>=FREQ_ADC_TIMER)
	//	   timers_adc = 0;

	//	update_norm_ADC_array();
	//	// после завершения серии АЦП вызываем isr()
	//	acp_Handler();
	//	//isr();


	//	// nAdc++;
	//	// switch ( nAdc ) {
	//	// case 5:
	//
	//	// 	break;
	//	// case 6:

	//	// 	break;

	//	// case 7:
	//	// 	// как бы с ПУ
	//	// 	for ( j = FIRST_WRITE_PAR_NUM; j < paramNo; j++ ) {
	//	// 		if ( paramNew[j] != param[j] ) {
	//	// 			input_param((short)j, paramNew[j]);
	//	// 			break;
	//	// 		}
	//	// 	}
	//	// 	// после завершения серии АЦП вызываем isr()
	//	// 	isr();
	//	// 	break;
	//	// } //switch ( nAdc )

	//} //tAdc

	//		if (calcAlgUpr) {
	//			// реализует алгоритм управления
	//			upr();
	//			calcAlgUpr = 0;
	//			timers_pwm++;
	//			if (timers_pwm>=FREQ_PWM_TIMER)
	//			   timers_pwm = 0;
	//		}


}

void simulateActionActionQualifierSubmodule(void) {
	//// Моделируем Action-Qualifier Submodule
	//// ... ePWM1
	//if ( cmp1A > t1cnt ) {
	//	ci1A = 0;
	//	ci1B = 1; 
	//}
	//else if ( cmp1A < t1cnt ) {
	//	ci1A = 1;
	//	ci1B = 0;
	//}
	//// ... ePWM2
	//if ( cmp2A > t2cnt ) {
	//	ci2A = 0;
	//	ci2B = 1;
	//}
	//else if ( cmp2A < t2cnt ) {
	//	ci2A = 1;
	//	ci2B = 0;
	//}
	//// ... ePWM3
	//if ( cmp3A > t3cnt ) {
	//	ci3A = 0;
	//	ci3B = 1;
	//}
	//else if ( cmp3A < t3cnt ) {
	//	ci3A = 1;
	//	ci3B = 0;
	//}
	//// ... ePWM4
	//if ( cmp4A > t4cnt ) {
	//	ci4A = 0;
	//	ci4B = 1;
	//}
	//else if ( cmp4A < t4cnt ) {
	//	ci4A = 1;
	//	ci4B = 0;
	//}
	//// ... ePWM5
	//if ( cmp5A > t5cnt ) {
	//	ci5A = 0;
	//	ci5B = 1;
	//}
	//else if ( cmp5A < t5cnt ) {
	//	ci5A = 1;
	//	ci5B = 0;
	//}
	//// ... ePWM6
	//if ( cmp6A > t6cnt ) {
	//	ci6A = 0;
	//	ci6B = 1;
	//}
	//else if ( cmp6A < t6cnt ) {
	//	ci6A = 1;
	//	ci6B = 0;
	//}
	//// ... ePWM7
	//if ( cmp7A > t7cnt ) {
	//	ci7A = 0;
	//	ci7B = 1;
	//}
	//else if ( cmp7A < t7cnt ) {
	//	ci7A = 1;
	//	ci7B = 0;
	//}
	//// ... ePWM8
	//if ( cmp8A > t8cnt ) {
	//	ci8A = 0;
	//	ci8B = 1;
	//}
	//else if ( cmp8A < t8cnt ) {
	//	ci8A = 1;
	//	ci8B = 0;
	//}
	//// ... ePWM9
	//if ( cmp9A > t9cnt ) {
	//	ci9A = 0;
	//	ci9B = 1;
	//}
	//else if ( cmp9A < t9cnt ) {
	//	ci9A = 1;
	//	ci9B = 0;
	//}
	//// ... ePWM10
	//if ( cmp10A > t10cnt ) {
	//	ci10A = 0;
	//	ci10B = 1;
	//}
	//else if ( cmp10A < t10cnt ) {
	//	ci10A = 1;
	//	ci10B = 0;
	//}
	//// ... ePWM11
	//if ( cmp11A > t11cnt ) {
	//	ci11A = 0;
	//	ci11B = 1;
	//}
	//else if ( cmp11A < t11cnt ) {
	//	ci11A = 1;
	//	ci11B = 0;
	//}
	//// ... ePWM12
	//if ( cmp12A > t12cnt ) {
	//	ci12A = 0;
	//	ci12B = 1;
	//}
	//else if ( cmp12A < t12cnt ) {
	//	ci12A = 1;
	//	ci12B = 0;
	//}


}

void simulateDeadBendSubmodule(void) {

	//// Моделируем Dead-Band Submodule
	//// ... ePWM1
	//if ( stateDt1 == 1 ) {
	//	ci1A_DT = ci1A;
	//	ci1B_DT = 0;
	//	if ( ci1A == 1 )
	//		cntDt1 = CntDt;
	//	if ( cntDt1 > 0 )
	//		cntDt1--;
	//	else
	//		stateDt1 = 2;
	//}
	//else if ( stateDt1 == 2 ) {
	//	ci1A_DT = 0;
	//	ci1B_DT = ci1B;
	//	if ( ci1B == 1 )
	//		cntDt1 = CntDt;
	//	if ( cntDt1 > 0 )
	//		cntDt1--;
	//	else
	//		stateDt1 = 1;
	//}
	//// ... ePWM2
	//if ( stateDt2 == 1 ) {
	//	ci2A_DT = ci2A;
	//	ci2B_DT = 0;
	//	if ( ci2A == 1 )
	//		cntDt2 = CntDt;
	//	if ( cntDt2 > 0 )
	//		cntDt2--;
	//	else
	//		stateDt2 = 2;
	//}
	//else if ( stateDt2 == 2 ) {
	//	ci2A_DT = 0;
	//	ci2B_DT = ci2B;
	//	if ( ci2B == 1 )
	//		cntDt2 = CntDt;
	//	if ( cntDt2 > 0 )
	//		cntDt2--;
	//	else
	//		stateDt2 = 1;
	//}
	//// ... ePWM3
	//if ( stateDt3 == 1 ) {
	//	ci3A_DT = ci3A;
	//	ci3B_DT = 0;
	//	if ( ci3A == 1 )
	//		cntDt3 = CntDt;
	//	if ( cntDt3 > 0 )
	//		cntDt3--;
	//	else
	//		stateDt3 = 2;
	//}
	//else if ( stateDt3 == 2 ) {
	//	ci3A_DT = 0;
	//	ci3B_DT = ci3B;
	//	if ( ci3B == 1 )
	//		cntDt3 = CntDt;
	//	if ( cntDt3 > 0 )
	//		cntDt3--;
	//	else
	//		stateDt3 = 1;
	//}
	//// ... ePWM4
	//if ( stateDt4 == 1 ) {
	//	ci4A_DT = ci4A;
	//	ci4B_DT = 0;
	//	if ( ci4A == 1 )
	//		cntDt4 = CntDt;
	//	if ( cntDt4 > 0 )
	//		cntDt4--;
	//	else
	//		stateDt4 = 2;
	//}
	//else if ( stateDt4 == 2 ) {
	//	ci4A_DT = 0;
	//	ci4B_DT = ci4B;
	//	if ( ci4B == 1 )
	//		cntDt4 = CntDt;
	//	if ( cntDt4 > 0 )
	//		cntDt4--;
	//	else
	//		stateDt4 = 1;
	//}
	//// ... ePWM5
	//if ( stateDt5 == 1 ) {
	//	ci5A_DT = ci5A;
	//	ci5B_DT = 0;
	//	if ( ci5A == 1 )
	//		cntDt5 = CntDt;
	//	if ( cntDt5 > 0 )
	//		cntDt5--;
	//	else
	//		stateDt5 = 2;
	//}
	//else if ( stateDt5 == 2 ) {
	//	ci5A_DT = 0;
	//	ci5B_DT = ci5B;
	//	if ( ci5B == 1 )
	//		cntDt5 = CntDt;
	//	if ( cntDt5 > 0 )
	//		cntDt5--;
	//	else
	//		stateDt5 = 1;
	//}
	//// ... ePWM6
	//if ( stateDt6 == 1 ) {
	//	ci6A_DT = ci6A;
	//	ci6B_DT = 0;
	//	if ( ci6A == 1 )
	//		cntDt6 = CntDt;
	//	if ( cntDt6 > 0 )
	//		cntDt6--;
	//	else
	//		stateDt6 = 2;
	//}
	//else if ( stateDt6 == 2 ) {
	//	ci6A_DT = 0;
	//	ci6B_DT = ci6B;
	//	if ( ci6B == 1 )
	//		cntDt6 = CntDt;
	//	if ( cntDt6 > 0 )
	//		cntDt6--;
	//	else
	//		stateDt6 = 1;
	//}
	//// ... ePWM7
	//if ( stateDt7 == 1 ) {
	//	ci7A_DT = ci7A;
	//	ci7B_DT = 0;
	//	if ( ci7A == 1 )
	//		cntDt7 = CntDt;
	//	if ( cntDt7 > 0 )
	//		cntDt7--;
	//	else
	//		stateDt7 = 2;
	//}
	//else if ( stateDt7 == 2 ) {
	//	ci7A_DT = 0;
	//	ci7B_DT = ci7B;
	//	if ( ci7B == 1 )
	//		cntDt7 = CntDt;
	//	if ( cntDt7 > 0 )
	//		cntDt7--;
	//	else
	//		stateDt7 = 1;
	//}
	//// ... ePWM8
	//if ( stateDt8 == 1 ) {
	//	ci8A_DT = ci8A;
	//	ci8B_DT = 0;
	//	if ( ci8A == 1 )
	//		cntDt8 = CntDt;
	//	if ( cntDt8 > 0 )
	//		cntDt8--;
	//	else
	//		stateDt8 = 2;
	//}
	//else if ( stateDt8 == 2 ) {
	//	ci8A_DT = 0;
	//	ci8B_DT = ci8B;
	//	if ( ci8B == 1 )
	//		cntDt8 = CntDt;
	//	if ( cntDt8 > 0 )
	//		cntDt8--;
	//	else
	//		stateDt8 = 1;
	//}
	//// ... ePWM9
	//if ( stateDt9 == 1 ) {
	//	ci9A_DT = ci9A;
	//	ci9B_DT = 0;
	//	if ( ci9A == 1 )
	//		cntDt9 = CntDt;
	//	if ( cntDt9 > 0 )
	//		cntDt9--;
	//	else
	//		stateDt9 = 2;
	//}
	//else if ( stateDt9 == 2 ) {
	//	ci9A_DT = 0;
	//	ci9B_DT = ci9B;
	//	if ( ci9B == 1 )
	//		cntDt9 = CntDt;
	//	if ( cntDt9 > 0 )
	//		cntDt9--;
	//	else
	//		stateDt9 = 1;
	//}
	//// ... ePWM10
	//if ( stateDt10 == 1 ) {
	//	ci10A_DT = ci10A;
	//	ci10B_DT = 0;
	//	if ( ci10A == 1 )
	//		cntDt10 = CntDt;
	//	if ( cntDt10 > 0 )
	//		cntDt10--;
	//	else
	//		stateDt10 = 2;
	//}
	//else if ( stateDt10 == 2 ) {
	//	ci10A_DT = 0;
	//	ci10B_DT = ci10B;
	//	if ( ci10B == 1 )
	//		cntDt10 = CntDt;
	//	if ( cntDt10 > 0 )
	//		cntDt10--;
	//	else
	//		stateDt10 = 1;
	//}
	//// ... ePWM11
	//if ( stateDt11 == 1 ) {
	//	ci11A_DT = ci11A;
	//	ci11B_DT = 0;
	//	if ( ci11A == 1 )
	//		cntDt11 = CntDt;
	//	if ( cntDt11 > 0 )
	//		cntDt11--;
	//	else
	//		stateDt11 = 2;
	//}
	//else if ( stateDt11 == 2 ) {
	//	ci11A_DT = 0;
	//	ci11B_DT = ci11B;
	//	if ( ci11B == 1 )
	//		cntDt11 = CntDt;
	//	if ( cntDt11 > 0 )
	//		cntDt11--;
	//	else
	//		stateDt11 = 1;
	//}
	//// ... ePWM12
	//if ( stateDt12 == 1 ) {
	//	ci12A_DT = ci12A;
	//	ci12B_DT = 0;
	//	if ( ci12A == 1 )
	//		cntDt12 = CntDt;
	//	if ( cntDt12 > 0 )
	//		cntDt12--;
	//	else
	//		stateDt12 = 2;
	//}
	//else if ( stateDt12 == 2 ) {
	//	ci12A_DT = 0;
	//	ci12B_DT = ci12B;
	//	if ( ci12B == 1 )
	//		cntDt12 = CntDt;
	//	if ( cntDt12 > 0 )
	//		cntDt12--;
	//	else
	//		stateDt12 = 1;
	//}

}


void simulateTripZoneSubmodule(void) {

//// Моделируем Trip-Zone Submodule
//	// ... clear flag for one-shot trip latch
//	if ( EPwm1Regs.TZCLR.all == 0x0004 ) {
//		EPwm1Regs.TZCLR.all = 0x0000;
//		EPwm1Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm2Regs.TZCLR.all == 0x0004 ) {
//		EPwm2Regs.TZCLR.all = 0x0000;
//		EPwm2Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm3Regs.TZCLR.all == 0x0004 ) {
//		EPwm3Regs.TZCLR.all = 0x0000;
//		EPwm3Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm4Regs.TZCLR.all == 0x0004 ) {
//		EPwm4Regs.TZCLR.all = 0x0000;
//		EPwm4Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm5Regs.TZCLR.all == 0x0004 ) {
//		EPwm5Regs.TZCLR.all = 0x0000;
//		EPwm5Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm6Regs.TZCLR.all == 0x0004 ) {
//		EPwm6Regs.TZCLR.all = 0x0000;
//		EPwm6Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm7Regs.TZCLR.all == 0x0004 ) {
//		EPwm7Regs.TZCLR.all = 0x0000;
//		EPwm7Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm8Regs.TZCLR.all == 0x0004 ) {
//		EPwm8Regs.TZCLR.all = 0x0000;
//		EPwm8Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm9Regs.TZCLR.all == 0x0004 ) {
//		EPwm9Regs.TZCLR.all = 0x0000;
//		EPwm9Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm10Regs.TZCLR.all == 0x0004 ) {
//		EPwm10Regs.TZCLR.all = 0x0000;
//		EPwm10Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm11Regs.TZCLR.all == 0x0004 ) {
//		EPwm11Regs.TZCLR.all = 0x0000;
//		EPwm11Regs.TZFRC.all = 0x0000;
//	}
//	if ( EPwm12Regs.TZCLR.all == 0x0004 ) {
//		EPwm12Regs.TZCLR.all = 0x0000;
//		EPwm12Regs.TZFRC.all = 0x0000;
//	}
//	// ... forces a one-shot trip event
//	if ( EPwm1Regs.TZFRC.all == 0x0004 )
//		ci1A_DT = ci1B_DT = 0;
//	if ( EPwm2Regs.TZFRC.all == 0x0004 )
//		ci2A_DT = ci2B_DT = 0;
//	if ( EPwm3Regs.TZFRC.all == 0x0004 )
//		ci3A_DT = ci3B_DT = 0;
//	if ( EPwm4Regs.TZFRC.all == 0x0004 )
//		ci4A_DT = ci4B_DT = 0;
//	if ( EPwm5Regs.TZFRC.all == 0x0004 )
//		ci5A_DT = ci5B_DT = 0;
//	if ( EPwm6Regs.TZFRC.all == 0x0004 )
//		ci6A_DT = ci6B_DT = 0;
//	if ( EPwm7Regs.TZFRC.all == 0x0004 )
//		ci7A_DT = ci7B_DT = 0;
//	if ( EPwm8Regs.TZFRC.all == 0x0004 )
//		ci8A_DT = ci8B_DT = 0;
//	if ( EPwm9Regs.TZFRC.all == 0x0004 )
//		ci9A_DT = ci9B_DT = 0;
//	if ( EPwm10Regs.TZFRC.all == 0x0004 )
//		ci10A_DT = ci10B_DT = 0;
//	if ( EPwm11Regs.TZFRC.all == 0x0004 )
//		ci11A_DT = ci11B_DT = 0;
//	if ( EPwm12Regs.TZFRC.all == 0x0004 )
//		ci12A_DT = ci12B_DT = 0;


}


void writeOutputParameters(real_T *xD) {

//	// ВЫХОДЫ (begin)
//	nn = 0;
//	// Управление
//	// ... INU1
//	xD[nn++] = ci1A_DT;
//	xD[nn++] = ci2A_DT;
//	xD[nn++] = ci1B_DT;
//	xD[nn++] = ci2B_DT;
//
//	xD[nn++] = ci3A_DT;
//	xD[nn++] = ci4A_DT;
//	xD[nn++] = ci3B_DT;
//	xD[nn++] = ci4B_DT;
//
//	xD[nn++] = ci5A_DT;
//	xD[nn++] = ci6A_DT;
//	xD[nn++] = ci5B_DT;
//	xD[nn++] = ci6B_DT;
//	// ... INU2
//	xD[nn++] = ci7A_DT;
//	xD[nn++] = ci8A_DT;
//	xD[nn++] = ci7B_DT;
//	xD[nn++] = ci8B_DT;
//
//	xD[nn++] = ci9A_DT;
//	xD[nn++] = ci10A_DT;
//	xD[nn++] = ci9B_DT;
//	xD[nn++] = ci10B_DT;
//
//	xD[nn++] = ci11A_DT;
//	xD[nn++] = ci12A_DT;
//	xD[nn++] = ci11B_DT;
//	xD[nn++] = ci12B_DT;
//
//
//// выходы ацп для контроля
//	xD[nn++] = udc1_ml;
//	xD[nn++] = udc2_ml;
//	xD[nn++] = udc3_ml;
//	xD[nn++] = udc4_ml;
//
//	xD[nn++] = idc1_ml;
//	xD[nn++] = idc2_ml;
//	xD[nn++] = idc3_ml;
//	xD[nn++] = idc4_ml;
//
//	xD[nn++] = _IQtoF(analog.iqIa1_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(analog.iqIb1_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(analog.iqIc1_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(analog.iqIa2_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(analog.iqIb2_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(analog.iqIc2_1) * NORMA_ACP;
//
//// timers out
//
//	xD[nn++] = timers_adc;
//	xD[nn++] = timers_pwm;
//	xD[nn++] = Tadc;
//	xD[nn++] = dt;
//
//	// Только для просмотра
//	xD[nn++] = _IQtoF(rp.pmZ);
//	xD[nn++] = _IQtoF(rs.wmZ);
//
//	xD[nn++] = mst.start;
//	xD[nn++] = inuWork;
//	xD[nn++] = mst.pzMode;
//
//	xD[nn++] = psi;
//	xD[nn++] = rf.psiZ;
//
//	xD[nn++] = wm;
//	xD[nn++] = _IQtoF(vect_control.koeff_correct_Id);//rs.wmZ;
//	xD[nn++] = _IQtoF(vect_control.iqFrot) * NORMA_FROTOR * 60.0 / N_BAZ;//csp.wmLimZi;
//
//	xD[nn++] = pm*S_BAZ;///P_NOM;
//	xD[nn++] = rp.pmZ*S_BAZ;///P_NOM;
//	xD[nn++] = csp.pmLimZi*S_BAZ;///P_NOM;
//
//	xD[nn++] = _IQtoF(analog.iqId1)* NORMA_ACP;//_IQtoF(vect_control.iqPzad);
//	xD[nn++] = _IQtoF(analog.iqIq1)* NORMA_ACP;// * NORMA_ACP;
//	xD[nn++] = _IQtoF(analog.iqId2)* NORMA_ACP;//_IQtoF(vect_control.iqUqCompensation1);//
//	xD[nn++] = _IQtoF(analog.iqIq2)* NORMA_ACP;
//	xD[nn++] = _IQtoF(vect_control.iqId_zad)* NORMA_ACP;
//	xD[nn++] = _IQtoF(vect_control.iqIq_zad)* NORMA_ACP;//iqZ;
//
//	xD[nn++] = me*M_BAZ/M_NOM;
//
//	xD[nn++] = _IQtoF(vect_control.iqPzad) * NORMA_ACP * NORMA_ACP / 1000.0;;
//	xD[nn++] = _IQtoF(vect_control.iqPizm) * NORMA_ACP * NORMA_ACP / 1000.0;;
//	// xD[nn++] = sqrt(idZ*idZ + iqZ*iqZ);
//	// xD[nn++] = IzLim;
//
//	// xD[nn++] = EPwm2Regs.CMPA.half.CMPA;//xpwm_time.Ta0_0;//cc.yd1;
//	// xD[nn++] = xpwm_time.Ta0_0;
//	// xD[nn++] = xpwm_time.Ta0_1;
//	// xD[nn++] = _IQtoF(cos_fi.cos_fi_nom);//cc.yd1;
//	// xD[nn++] = _IQtoF(cos_fi.cos_fi_nom_squared);//cc.yq1;
//	// xD[nn++] = _IQtoF(tetta_calc.k_t);
//	xD[nn++] = _IQtoF(vect_control.iqUzad1);//cc.yd1;
//	xD[nn++] = _IQtoF(vect_control.iqUzad2);//cc.yd1;
////	xD[nn++] = _IQtoF(vect_control.iqUdKm1Out);//cc.yq1;
////	xD[nn++] = _IQtoF(vect_control.iqUqKm1Out);
//
//	xD[nn++] = _IQtoF(vect_control.iqUdKm1);//sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1);
//	xD[nn++] = _IQtoF(vect_control.iqUqKm1);//sqrt(cc.yd2*cc.yd2 + cc.yq2*cc.yq2);
//	xD[nn++] = _IQtoF(vect_control.iqUdKm2);//Y_LIM;
//	xD[nn++] = _IQtoF(vect_control.iqUqKm2);//Y_LIM;
//
//
//	xD[nn++] = _IQtoF(vect_control.iqUdKm1Out);//sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1);
//	xD[nn++] = _IQtoF(vect_control.iqUqKm1Out);//sqrt(cc.yd2*cc.yd2 + cc.yq2*cc.yq2);
//	xD[nn++] = _IQtoF(vect_control.iqUdKm2Out);//Y_LIM;
//	xD[nn++] = _IQtoF(vect_control.iqUqKm2Out);//Y_LIM;
//
//	xD[nn++] = _IQtoF(vect_control.k_modul_max);//Y_LIM;
//
//    xD[nn++] = _IQtoF(vect_control.k_modul_max_def);
//   xD[nn++] = _IQtoF(vect_control.maxUq1);
//   xD[nn++] = _IQtoF(vect_control.maxUq2);
//   xD[nn++] = _IQtoF(vect_control.Uq1Out);
//   xD[nn++] = _IQtoF(vect_control.Uq2Out);
////    xD[nn++] = _IQtoF(vect_control.K_MODUL_MAX);
////    xD[nn++] = _IQtoF(vect_control.K_MODUL_MAX);
////    xD[nn++] = _IQtoF(vect_control.K_MODUL_MAX);
//
//	
//	// xD[nn++] = sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1);
//	// xD[nn++] = sqrt(cc.yd2*cc.yd2 + cc.yq2*cc.yq2);
//	// xD[nn++] = Y_LIM;
//	// ВЫХОДЫ (end)
//
//
/////////////////  new
//	xD[nn++] = _IQtoF(f.iq_mzz_zad) * NORMA_ACP;//
//	xD[nn++] = _IQtoF(pidPvect.OutMax) * NORMA_ACP;
//	xD[nn++] = _IQtoF(pidPvect.OutMin) * NORMA_ACP;
//	xD[nn++] = _IQtoF(pidPvect.Out) * NORMA_ACP;
//	xD[nn++] = _IQtoF(vect_control.mzz_zad_int) * NORMA_ACP ;
//	xD[nn++] = _IQtoF(filter.iqIm_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(filter.iqIm_2) * NORMA_ACP;
//
//	xD[nn++] = _IQtoF(vect_control.Pzad_rmp) * NORMA_ACP * NORMA_ACP / 1000.0;
//	xD[nn++] = _IQtoF(f.iq_p_rampa) * NORMA_ACP * NORMA_ACP / 1000.0;
//	xD[nn++] = _IQtoF(analog.iqPvsi1) * NORMA_ACP * NORMA_ACP / 1000.0;
//	xD[nn++] = _IQtoF(analog.iqPvsi1) * NORMA_ACP * NORMA_ACP / 1000.0;
//
//	xD[nn++] = _IQtoF(analog.iqW1) * NORMA_ACP * NORMA_ACP / 1000.0;
//	xD[nn++] = _IQtoF(analog.iqW2) * NORMA_ACP * NORMA_ACP / 1000.0;	
//	xD[nn++] = _IQtoF(analog.iqW) * NORMA_ACP * NORMA_ACP / 1000.0;
//	xD[nn++] = _IQtoF(filter.iqW1) * NORMA_ACP * NORMA_ACP / 1000.0;
//	xD[nn++] = _IQtoF(filter.iqW2) * NORMA_ACP * NORMA_ACP / 1000.0;	
//	xD[nn++] = _IQtoF(filter.iqW) * NORMA_ACP * NORMA_ACP / 1000.0;
//
//	
//
//	xD[nn++] = _IQtoF(vect_control.iqFrot) * NORMA_FROTOR * 60.0;
//	xD[nn++] = vect_control.flag_reverse;//_IQtoF(0);
//
//	xD[nn++] = _IQtoF(vect_control.koeff_correct_Id); 
//	xD[nn++] = _IQtoF(cos_fi.cos_fi_nom);
//
//	xD[nn++] = _IQtoF(filter.Fsl) * NORMA_FROTOR;
//
//	xD[nn++] = _IQtoF(filter.iqIa1_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(filter.iqIb1_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(filter.iqIc1_1) * NORMA_ACP;
//
//	xD[nn++] = _IQtoF(filter.iqIa2_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(filter.iqIb2_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(filter.iqIc2_1) * NORMA_ACP;
//
//	xD[nn++] = _IQtoF(tetta_calc.Imds);
//	xD[nn++] = _IQtoF(tetta_calc.tetta);
//
//	xD[nn++] = _IQtoF(vect_control.Is) * NORMA_ACP;
//	xD[nn++] = _IQtoF(vect_control.Ids) * NORMA_ACP;
//	xD[nn++] = _IQtoF(vect_control.Id_ref_fw) * NORMA_ACP;	
//	xD[nn++] = _IQtoF(vect_control.Id_ref_1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(vect_control.Is_max) * NORMA_ACP;
//
//	xD[nn++] = vect_control.flag_fw;
//
//	xD[nn++] = _IQtoF(vect_control.ws_Iq1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(vect_control.ws_Id_filter1) * NORMA_ACP;
//	xD[nn++] = _IQtoF(vect_control.ws_Id_filter2) * NORMA_ACP;
//
//	xD[nn++] = _IQtoF(vect_control.ws_ws_t1) * NORMA_FROTOR;
//
//	xD[nn++] = _IQtoF(analog.Fsl) * NORMA_FROTOR;
//	xD[nn++] = _IQtoF(filter.Fsl) * NORMA_FROTOR;
//
//	xD[nn++] = vect_control.tmp1;
//	xD[nn++] = vect_control.tmp2;
//	xD[nn++] = vect_control.tmp3;
//	//xD[nn++] = _IQtoF(0) * NORMA_ACP;
//	//xD[nn++] = _IQtoF(0) * NORMA_ACP;
//	xD[nn++] = _IQtoF(0) * NORMA_ACP;
//
//
//
//	xD[nn++] = _IQtoF(svgen_dq_1.Ta)*1000.0;
//	xD[nn++] = _IQtoF(svgen_dq_1.Tb)*1000.0;
//	xD[nn++] = _IQtoF(svgen_dq_1.Tc)*1000.0;
//
//	xD[nn++] = _IQtoF(svgen_dq_2.Ta)*1000.0;
//	xD[nn++] = _IQtoF(svgen_dq_2.Tb)*1000.0;
//	xD[nn++] = _IQtoF(svgen_dq_2.Tc)*1000.0;
//




}



void controller(SimStruct *S, const real_T *u, real_T *xD, real_T *rW, int_T *iW) {

	static _iq Uzad1 = 0, Fzad = 0, Uzad2 = 0, Izad_out = 0, Uzad_from_master = 0;
	_iq wd;

	readInputParameters(u);
	processSFunctionIfChanged(S, iW);
	initialisationOnStart(iW);

	simulatePWMcounterAndReadComarators();

	
	simulateAdcAndCallIsr();
	simulateActionActionQualifierSubmodule();

	// convertSVGenTimesToTkLines();
	simulateDeadBendSubmodule();

	// xilinxPwm3LevelSimulation();
	simulateTripZoneSubmodule();


	writeOutputParameters(xD);

	if (edrk.flag_second_PCH == 0) {
		wd = uf_alg.winding_displacement_bs1;
	}
	else {
		wd = uf_alg.winding_displacement_bs2;
	}

	vectorControlConstId(edrk.zadanie.iq_power_zad_rmp, edrk.zadanie.iq_oborots_zad_hz_rmp,
		WRotor.RotorDirectionSlow, WRotor.iqWRotorSumFilter,
		edrk.Mode_ScalarVectorUFConst,
		edrk.MasterSlave, edrk.zadanie.iq_Izad, wd,
		edrk.master_theta, edrk.master_Iq, edrk.iq_power_kw_another_bs,
		&edrk.tetta_to_slave, &edrk.Iq_to_slave, &edrk.P_to_master,
		0, 1);

	test_calc_vect_dq_pwm24_Ing(vect_control.iqTheta, vect_control.iqUdKm, vect_control.iqUqKm,
		edrk.disable_alg_u_disbalance,
		edrk.zadanie.iq_kplus_u_disbalance_rmp, edrk.zadanie.iq_k_u_disbalance_rmp,
		filter.iqU_1_fast, filter.iqU_2_fast,
		0,
		edrk.Uzad_max,
		edrk.MasterSlave,
		edrk.flag_second_PCH,
		&edrk.Kplus, &edrk.Uzad_to_slave);
	analog.PowerFOC = edrk.P_to_master;
	Fzad = vect_control.iqFstator;
	Izad_out = edrk.Iq_to_slave;


} //void controller(SimStruct ...
