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

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


#include "simstruc.h"
#include "wrapper_inu.h"
#include "def.h"
#include "controller.h"


extern void init28335(void);
extern void detcoeff(void);
extern void isr(void);
extern void input_param(unsigned short num, unsigned short val);


void controller(SimStruct *S, const real_T *u, real_T *xD, real_T *rW, int_T *iW) {
	// ВХОДЫ (begin)
	nn = 0;
	// аналоговые величины
	udc1_ml = u[nn++];//В
	ia1_ml = u[nn++];//А
	ib1_ml = u[nn++];//А
	ic1_ml = u[nn++];//А
	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)
	iref = u[nn++];//о.е.
	// параметры (например, с ПУ)
	paramNo = FIRST_WRITE_PAR_NUM;
	paramNew[paramNo++] = (unsigned short)u[nn++];
	paramNew[paramNo++] = (unsigned short)u[nn++];
	// ВХОДЫ (end)

	//wm_ml = wm_ml_0*wm_err;//угловая скорость с учетом погрешности


	// обрабатываем параметры 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 )




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

		// инициализация процессора
		init28335();

		// имитация считывания параметров из EEPROM
		// ... параметры из модели (см. блок "Parameters")
		for ( j = FIRST_WRITE_PAR_NUM; j < paramNo; j++ ) {
			param[j] = paramNew[j];
		}
		// ... параметры из файла
		param[180] = 930;//rf.PsiZ, %*10 от PSI_BAZ

		param[200] = 2048;//offset.Ia1, ед. АЦП
		param[201] = 2048;//offset.Ib1, ед. АЦП
		param[202] = 2048;//offset.Ic1, ед. АЦП
		param[203] = 2048;//offset.Udc1, ед. АЦП
		param[206] = 2048;//offset.Ia2, ед. АЦП
		param[207] = 2048;//offset.Ib2, ед. АЦП
		param[208] = 2048;//offset.Ic2, ед. АЦП
		param[209] = 2048;//offset.Udc2, ед. АЦП

		param[210] = 100;//cc.Kp, %
		param[211] = 100;//cc.Ki, %
		param[212] = 100;//cf.Kp, %
		param[213] = 100;//cf.Ki, %
		param[214] = 100;//csp.Kp, %
		param[215] = 100;//csp.Ki, %

		param[220] = 99;//protect.IacMax, % от IAC_SENS_MAX
		param[221] = 130;//protect.UdcMax, % от U_NOM
		param[222] = 110;//IzLim, % от I_BAZ (д.б. больше cf.IdLim)
		param[223] = 105;//cf.IdLim, % от I_BAZ (д.б. меньше IzLim)
		param[224] = 105;//csp.IqLim, % от I_BAZ
		param[225] = 97;//protect.UdcMin, % от U_NOM
		param[226] = 115;//protect.WmMax, % от N_NOM
		param[228] = 103;//rf.WmNomPsi, % от N_NOM
		param[229] = 97;//rf.YlimPsi, % от Y_LIM
		param[231] = 300;//protect.TudcMin, мс
		param[233] = 1000;//protect.TwmMax, мс

		param[244] = 26000;//rs.WlimIncr, мс
		param[245] = 2000;//csp.IlimIncr, мс
		param[248] = 6000;//rp.PlimIncr, мс

		param[269] = 9964;//9700;//KmeCorr, %*100

		param[285] = 10;//Kudc, мс*10
		param[286] = 700;//Kwm, мс*10
		param[288] = 250;//rs.Kwmz, мс
		param[289] = 50;//rf.Kpsiz, мс
		param[290] = 40;//Kme, мс
		param[292] = 80;//rp.Kpmz, мс

		param[303] = (unsigned short)(19200.);//sgmPar.Rs, мкОм
		param[304] = (unsigned short)(19364.);//sgmPar.Lls, мкГн*10
		param[305] = (unsigned short)(8500.);//sgmPar.Rr, мкОм
		param[306] = (unsigned short)(10212.);//sgmPar.Llr, мкГн*10
		param[307] = (unsigned short)(35810.);//sgmPar.Lm, мкГн

		// инициализация программы
		detcoeff();

		// для моделирования таймеров
		T1Pr = (double)EPwm1Regs.TBPRD;
		T2Pr = (double)EPwm2Regs.TBPRD;
		T3Pr = (double)EPwm3Regs.TBPRD;
		T4Pr = (double)EPwm4Regs.TBPRD;
		T5Pr = (double)EPwm5Regs.TBPRD;
		T6Pr = (double)EPwm6Regs.TBPRD;
		t1cntAux = (double)EPwm1Regs.TBCTR;
		t2cntAux = (double)EPwm2Regs.TBCTR;
		t3cntAux = (double)EPwm3Regs.TBCTR;
		t4cntAux = (double)EPwm4Regs.TBCTR;
		t5cntAux = (double)EPwm5Regs.TBCTR;
		t6cntAux = (double)EPwm6Regs.TBCTR;
		// ... приращение счётчиков таймеров за шаг дискретизации
		TxCntPlus = FTBCLK*dt;

		// для моделирования eQEP
		Qposmax = (double)EQep2Regs.QPOSMAX;
		qposcnt = 1.;//(double)EQep2Regs.QPOSCNT;

		// для моделирования АЦП
		// (на счёт 1e-6 см. SetupAdc(), хотя там скорее не 1.0 мкс, а 0.8 мкс)
		Tadc = (int)(1e-6/dt);
		// ... на всякий случай
		if ( Tadc < 1 )
			Tadc = 1;
		tAdc = 0;
		// ... чтобы АЦП ждал запуска
		nAdc = 10;

		// для моделирования Dead-Band Unit
		CntDt = (int)(DT/dt);
		stateDt1 = stateDt2 = stateDt3 = stateDt4 = stateDt5 = stateDt6 = 1;
		cntDt1 = cntDt2 = cntDt3 = cntDt4 = cntDt5 = cntDt6 = 0;

		// для защит
		DI_24V_SOURCE_FAULT = 0;

		// для вывода
		inuWork = 0;
		ivc.psi = 0;
		rf.psiZ = 0;
		rs.wmZ = 0;
		csp.wmLimZi = 0;
		pm = 0;
		rp.pmZ = 0;
		csp.pmLimZi = 0;
		id1 = 0;
		iq1 = 0;
		idZ = 0;
		iqZ = 0;
		cf.idP = 0;
		cf.idFF = 0;
		cf.idI = 0;
		csp.iqP = 0;
		csp.iqFF = 0;
		csp.iqI = 0;
		cc.yd1 = 0;
		cc.yq1 = 0;
		cc.y1 = 0;
	} //if ( iW[1] == 1 )




	// Моделируем 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;
	}
	if ( (t1cntAuxPrev < 0) && (t1cntAux >= 0) ) {
		// active CMPA load from shadow when TBCTR == 0
		cmp1A = (double)EPwm1Regs.CMPA.half.CMPA;
		// запуск АЦП
		tAdc = Tadc;
		nAdc = 0;
	}
	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);


	// Моделируем работу счётчика в 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;


	/* Моделируем преобразования измеряемых величин датчиками,
	операционниками и АЦП (с помощью nAdc учитываем сдвиг по времени
	между АЦП разных сигналов) */
	if ( tAdc < Tadc ) {
		tAdc++;
	}
	else {
		tAdc = 1;
		nAdc++;
		switch ( nAdc ) {
		case 5:
			// Udc1
			if ( udc1_ml > UDC_SENS_MAX )
				udc1_ml = UDC_SENS_MAX;
			else if ( udc1_ml < -UDC_SENS_MAX )
				udc1_ml = -UDC_SENS_MAX;
			AdcMirror.ADCRESULT0 =
				(unsigned short)(udc1_ml/UDC_SENS_MAX*2048. + (float)offset.Udc1);
			// Ic1
			if ( ic1_ml > IAC_SENS_MAX )
				ic1_ml = IAC_SENS_MAX;
			else if ( ic1_ml < -IAC_SENS_MAX )
				ic1_ml = -IAC_SENS_MAX;
			AdcMirror.ADCRESULT2 =
				(unsigned short)(ic1_ml/IAC_SENS_MAX*2048. + (float)offset.Ic1);
			break;
		case 6:
			// Ia1
			if ( ia1_ml > IAC_SENS_MAX )
				ia1_ml = IAC_SENS_MAX;
			else if ( ia1_ml < -IAC_SENS_MAX )
				ia1_ml = -IAC_SENS_MAX;
			AdcMirror.ADCRESULT4 =
				(unsigned short)(ia1_ml/IAC_SENS_MAX*2048. + (float)offset.Ia1);
			// Ib1
			if ( ib1_ml > IAC_SENS_MAX )
				ib1_ml = IAC_SENS_MAX;
			else if ( ib1_ml < -IAC_SENS_MAX )
				ib1_ml = -IAC_SENS_MAX;
			AdcMirror.ADCRESULT6 =
				(unsigned short)(ib1_ml/IAC_SENS_MAX*2048. + (float)offset.Ib1);
			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


	// Моделируем Action-Qualifier Submodule
	// ... ePWM1
	if ( cmp1A > t1cnt ) {
		ci1A = 1;
		ci1B = 0; 
	}
	else if ( cmp1A < t1cnt ) {
		ci1A = 0;
		ci1B = 1;
	}
	// ... ePWM2
	if ( cmp2A < t2cnt ) {
		ci2A = 1;
		ci2B = 0;
	}
	else if ( cmp2A > t2cnt ) {
		ci2A = 0;
		ci2B = 1;
	}
	// ... ePWM3
	if ( cmp3A > t3cnt ) {
		ci3A = 1;
		ci3B = 0;
	}
	else if ( cmp3A < t3cnt ) {
		ci3A = 0;
		ci3B = 1;
	}
	// ... ePWM4
	if ( cmp4A < t4cnt ) {
		ci4A = 1;
		ci4B = 0;
	}
	else if ( cmp4A > t4cnt ) {
		ci4A = 0;
		ci4B = 1;
	}
	// ... ePWM5
	if ( cmp5A > t5cnt ) {
		ci5A = 1;
		ci5B = 0;
	}
	else if ( cmp5A < t5cnt ) {
		ci5A = 0;
		ci5B = 1;
	}
	// ... ePWM6
	if ( cmp6A < t6cnt ) {
		ci6A = 1;
		ci6B = 0;
	}
	else if ( cmp6A > t6cnt ) {
		ci6A = 0;
		ci6B = 1;
	}


	// Моделируем 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;
	}


	// Моделируем 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;
	}

	// ... 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;




	// ВЫХОДЫ (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;

	// Только для просмотра
	xD[nn++] = state;
	xD[nn++] = faultNo;

	xD[nn++] = mst.start;
	xD[nn++] = inuWork;
	xD[nn++] = mst.pzMode;

	xD[nn++] = psi;
	xD[nn++] = rf.psiZ;

	xD[nn++] = wm;
	xD[nn++] = rs.wmZ;
	xD[nn++] = 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++] = id1;
	xD[nn++] = iq1;
	xD[nn++] = idZ;
	xD[nn++] = iqZ;

	xD[nn++] = me*M_BAZ/M_NOM;

	xD[nn++] = sqrt(idZ*idZ + iqZ*iqZ);
	xD[nn++] = IzLim;

	xD[nn++] = sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1);
	xD[nn++] = Y_LIM;
	
	xD[nn++] = theta_out;
	// ВЫХОДЫ (end)

} //void controller(SimStruct ...