/**************************************************************************
	Description: Функция реализует алгоритм управления INU
	(отработка N и P).

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


#include "def.h"
#include "upr.h"
#include "pwm_vector_regul.h"
#include "IQmathLib.h"
#include "adc_tools.h"
#include "params.h"
#include "vector.h"
#include "v_pwm24.h"
#include "xp_write_xpwm_time.h"
#include "rotation_speed.h"


#pragma CODE_SECTION(control_current, "ramfuncs");
#pragma CODE_SECTION(control_flux, "ramfuncs");
#pragma CODE_SECTION(control_speed_power, "ramfuncs");
#pragma CODE_SECTION(indirect_vector_control, "ramfuncs");
#pragma CODE_SECTION(ipark, "ramfuncs");
#pragma CODE_SECTION(limit_current, "ramfuncs2");
#pragma CODE_SECTION(pwm, "ramfuncs2");
#pragma CODE_SECTION(reference_flux, "ramfuncs");
#pragma CODE_SECTION(reference_power, "ramfuncs");
#pragma CODE_SECTION(reference_speed, "ramfuncs");
#pragma CODE_SECTION(select_feedback, "ramfuncs");
#pragma CODE_SECTION(upr, "ramfuncs");


void control_current(void);
void control_flux(void);
void control_speed_power(void);
void indirect_vector_control(void);
void ipark(void);
void limit_current(void);
void pwm(void);
void reference_flux(void);
void reference_power(void);
void reference_speed(void);
void select_feedback(void);

void write_swgen_pwm_times_split_eages(void);
void write_CMP_tims(void);

int reset = 1;
extern double wm_ml;

void upr(void) {
	static short decim_psi_wm_pm;
	static int calcPWMtimes = 0;

	if ( onceUpr == 0 ) {
		onceUpr = 1;
		decim_psi_wm_pm = (short)DECIM_PSI_WM_PM;
		psi = 0;
		rf.once = 0;
		rs.once = 0;
		rp.once = 0;
		cf.once = 0;
		csp.once = 0;
		ivc.once = 0;
		cc.once = 0;
		reset = 1;
		init_DQ_pid();
		InitVariablesSvgen(FREQ_PWM);
		pwm_vector_model_titov(0, 0, 0, 0, 0, 1, calcPWMtimes);
		analog.tetta = 0;
	} else {
		reset = 0;
	}

	// внешние контуры регулирования (по потоку, оборотам и мощности)
	// не должны быть такими же быстрыми как внутренние (по току) (?)
	if ( decim_psi_wm_pm < (short)DECIM_PSI_WM_PM ) {
		decim_psi_wm_pm++;
		calcPWMtimes = 0;
	}
	else {
		decim_psi_wm_pm = 1;
		calcPWMtimes = 1;

		// заданное потокосцепление
		// (rf.PsiZ -> rf.psiZ, rf.pPsiZ)
		reference_flux();

		// заданная скорость
		// (mst.wmZz, mst.wmLim -> rs.wmZ, rs.pWmZ)
		reference_speed();

		// заданная мощность
		// (mst.pmZz, mst.pmLim -> rp.pmZ)
		reference_power();

		// заданный ток (idZ, iqZ)
		// ... регулятор потокосцепления
		// (rf.psiZ, psi, rf.pPsiZ, cf.IdLim, cf.IdLimNeg -> idZ)
		control_flux();
		// ... регуляторы скорости и мощности
		// (rs.wmZ, wm, rs.pWmZ, rp.pmZ, mst.wmLim, mst.pmLim, csp.IqLim,
		// csp.IqLimNeg -> iqZ, inuWork)
		control_speed_power();
		// ... ограничение полного тока
		// (idZ, iqZ, IzLim -> idZ, iqZ, csp.iqLimFlag)
		limit_current();
	} //decim_psi_wm_pm


	if ( mst.start == 1 ) {
		inuWork = 1;
	} else {
		inuWork = 0;
	}



	_iq Pzad = _IQ(rp.pmZ * 1.1);
	// _iq Pzad = _IQ(1.1);
	_iq Fzad = _IQ(rs.wmZ / (PI2 * 1.1574233675198942802148869545233));
	// _iq Frot = _IQ(10.0 / 6.0 / NORMA_FROTOR);
	_iq Frot = _IQ(wm_ml / PI2 / NORMA_FROTOR);
	int direction = Frot >= 0 ? 1 : -1;
	rotor.iqFout = Frot;
	int mode = 2;
	// int reset = 0;
	f.Go = 1;
	if (mode != 0) {
		limit_mzz_zad_power(Frot);
		// set_cos_tetta_calc_params();
		pwm_vector_model_titov(Pzad, Fzad, direction, labs(Frot), mode, 0, calcPWMtimes);
	} else {
		// U/f=Const
	#define K_MODUL_MAX 15435038LL
		vect_control.iqUqKm1 = _IQ(0.6);
		vect_control.iqUqKm2 = _IQ(0.6);
		_iq Fconst = IQ_F_STATOR_NOM / 10;
		test_calc_simple_dq_pwm24(vect_control.iqUqKm1, vect_control.iqUqKm2, Fconst, Fconst, K_MODUL_MAX);
	}


	reset = 0;
	float theta = _IQtoF(analog.tetta);
	sincos(theta, &sinTheta, &cosTheta);

	if (calcPWMtimes)
		write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_LEVEL_HIGH);
	else
		write_swgen_pwm_times_split_eages(PWM_MODE_RELOAD_LEVEL_LOW);
	write_CMP_tims(calcPWMtimes);
	// cc.yd1 = _IQtoF(vect_control.iqUdKm1Out) * Y_LIM;
	// cc.yq1 = _IQtoF(vect_control.iqUqKm1Out) * Y_LIM;
	// cc.yd2 = _IQtoF(vect_control.iqUdKm2Out) * Y_LIM;
	// cc.yq2 = _IQtoF(vect_control.iqUqKm2Out) * Y_LIM;



	// indirect vector control
	// (wmNf, wm, ix1, iy1, ix2, iy2 -> ivc.ws, ivc.sinTheta, ivc.cosTheta,
	// ivc.id1, ivc.iq1, ivc.id2, ivc.iq2, ivc.psi)
	// indirect_vector_control();


	// выбор сигналов о.с.
	// (... -> ws, sinTheta, cosTheta, id1, iq1, id2, iq2, psi)
	// select_feedback();


// idZ = 0.1;
// iqZ = 0.2;
	// регуляторы тока
	// (idZ, iqZ, id1, iq1, id2, iq2, ws, wm, psi ->
	// -> cc.yd1, cc.yq1, cc.yd2, cc.yq2)
	// control_current();


	// перевод сигналов управления из с.к. d-q в с.к. x-y
	// (cc.yd1, cc.yq1, cc.yd2, cc.yq2, sinTheta, cosTheta, ws ->
	// -> ip.yx1, ip.yy1, ip.yx2, ip.yy2)
	// ipark();

	// ШИМ
	// (ip.yx1, ip.yy1, ip.yx2, ip.yy2 ->
	// -> EPwm1Regs.CMPA.half.CMPA, EPwm2Regs.CMPA.half.CMPA,
	// EPwm3Regs.CMPA.half.CMPA, EPwm4Regs.CMPA.half.CMPA,
	// EPwm5Regs.CMPA.half.CMPA, EPwm6Regs.CMPA.half.CMPA,
	// EPwm7Regs.CMPA.half.CMPA, EPwm8Regs.CMPA.half.CMPA,
	// EPwm9Regs.CMPA.half.CMPA, EPwm10Regs.CMPA.half.CMPA,
	// EPwm11Regs.CMPA.half.CMPA, EPwm12Regs.CMPA.half.CMPA)
	// pwm();
} //void upr(void)



// Регулирует ток
// (idZ, iqZ, id1, iq1, id2, iq2, ws, wm, psi ->
// -> cc.yd1, cc.yq1, cc.yd2, cc.yq2)
void control_current(void) {
	if ( cc.once == 0 ) {
		cc.once = 1;
		cc.y1LimFlag = 0;
		cc.yd1I = 0;
		cc.yq1I = 0;
		cc.yd1 = 0;
		cc.yq1 = 0;
		cc.y2LimFlag = 0;
		cc.yd2I = 0;
		cc.yq2I = 0;
		cc.yd2 = 0;
		cc.yq2 = 0;
		// для сокращения вычислений
		cc.K1 = sgmPar.SigmaLs*I_BAZ*WE_BAZ*U_2_Y;
		cc.K2 = sgmPar.Rr*sgmPar.Lm/(sgmPar.Lr*sgmPar.Lr)*PSI_BAZ*U_2_Y;
		cc.K3 = sgmPar.Kl*PSI_BAZ*WE_BAZ*U_2_Y;
	}

	// для сокращения вычислений
	cc.Xyff = ws*cc.K1;
	cc.yffAux2 = psi*cc.K2;
	cc.yffAux3 = psi*wm*cc.K3;

	// регулятор Id1
	cc.del = idZ - id1;
	cc.yd1P = cc.del*cc.Kp;
	if ( (cc.y1LimFlag == 0) || (cc.yd1*cc.del < 0) )
		cc.yd1I += cc.del*cc.Ki;
	cc.yd1FF = -iq1*cc.Xyff - cc.yffAux2;
	cc.yd1 = cc.yd1P + cc.yd1I + cc.yd1FF;
	// регулятор Iq1
	cc.del = iqZ - iq1;
	cc.yq1P = cc.del*cc.Kp;
	if ( (cc.y1LimFlag == 0) || (cc.yq1*cc.del < 0) )
		cc.yq1I += cc.del*cc.Ki;
	cc.yq1FF = id1*cc.Xyff + cc.yffAux3;
	cc.yq1 = cc.yq1P + cc.yq1I + cc.yq1FF;
	// ограничение
	cc.y1 = sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1);
	if ( cc.y1 > Y_LIM ) {
		cc.kYlim = Y_LIM/cc.y1;
		cc.yd1 *= cc.kYlim;
		cc.yq1 *= cc.kYlim;
		cc.y1LimFlag = 1;
	}
	else {
		cc.y1LimFlag = 0;
	}

	// регулятор Id2
	cc.del = idZ - id2;
	cc.yd2P = cc.del*cc.Kp;
	if ( (cc.y2LimFlag == 0) || (cc.yd2*cc.del < 0) )
		cc.yd2I += cc.del*cc.Ki;
	cc.yd2FF = -iq2*cc.Xyff - cc.yffAux2;
	cc.yd2 = cc.yd2P + cc.yd2I + cc.yd2FF;
	// регулятор Iq2
	cc.del = iqZ - iq2;
	cc.yq2P = cc.del*cc.Kp;
	if ( (cc.y2LimFlag == 0) || (cc.yq2*cc.del < 0) )
		cc.yq2I += cc.del*cc.Ki;
	cc.yq2FF = id2*cc.Xyff + cc.yffAux3;
	cc.yq2 = cc.yq2P + cc.yq2I + cc.yq2FF;
	// ограничение
	cc.y2 = sqrt(cc.yd2*cc.yd2 + cc.yq2*cc.yq2);
	if ( cc.y2 > Y_LIM ) {
		cc.kYlim = Y_LIM/cc.y2;
		cc.yd2 *= cc.kYlim;
		cc.yq2 *= cc.kYlim;
		cc.y2LimFlag = 1;
	}
	else {
		cc.y2LimFlag = 0;
	}
} //void control_current(void)



// Регулирует потокосцепление
// (rf.psiZ, psi, rf.pPsiZ, cf.IdLim, cf.IdLimNeg -> idZ)
void control_flux(void) {
	if ( cf.once == 0 ) {
		cf.once = 1;
		cf.idLimFlag = 0;
		cf.idI = 0;
		idZ = 0;
	}

	// регулятор Psi
	cf.del = rf.psiZ - psi;
	cf.idP = cf.del*cf.Kp;
	if ( (cf.idLimFlag == 0) || (idZ*cf.del < 0) )
		cf.idI += cf.del*cf.Ki;
	cf.idFF = (rf.psiZ + rf.pPsiZ*sgmPar.Tr)*sgmPar.LmInv*PSI_BAZ/I_BAZ;
	idZ = cf.idP + cf.idI + cf.idFF;
	// ограничение потокообразующего тока
	if ( idZ > cf.IdLim ) {
		idZ = cf.IdLim;
		cf.idLimFlag = 1;
	}
	else if ( idZ < cf.IdLimNeg ) {
		idZ = cf.IdLimNeg;
		cf.idLimFlag = 1;
	}
	else {
		cf.idLimFlag = 0;
	}
} //void control_flux(void)



// Регулирует скорость или мощность
// (rs.wmZ, wm, rs.pWmZ, rp.pmZ, mst.wmLim, mst.pmLim, csp.IqLim,
// csp.IqLimNeg -> iqZ, inuWork)
void control_speed_power(void) {
	if ( csp.once == 0 ) {
		csp.once = 1;
		csp.wmLimZi = mst.wmLim;
		csp.pmLimZi = mst.pmLim;
		csp.iqLimFlag = 0;
		csp.iqI = 0;
		iqZ = 0;
		csp.iqLimZi = csp.IqLim;
		csp.iqLim = csp.IqLim;
		csp.pmZiRampDown = 0;
	}

	// для ограничения скорости
	if ( mst.wmLim - csp.wmLimZi > rs.WlimIncr ) {
		csp.wmLimZi += rs.WlimIncr;
	}
	else if ( csp.wmLimZi - mst.wmLim > rs.WlimIncr ) {
		csp.wmLimZi -= rs.WlimIncr;
	}
	else {
		csp.wmLimZi = mst.wmLim;
	}
	// для ограничения мощности
	if ( mst.pmLim - csp.pmLimZi > rp.PlimIncr ) {
		csp.pmLimZi += rp.PlimIncr;
	}
	else if ( csp.pmLimZi - mst.pmLim > rp.PlimIncr ) {
		csp.pmLimZi -= rp.PlimIncr;
	}
	else {
		csp.pmLimZi = mst.pmLim;
	}

	if ( inuWork == 0 ) {
		if ( mst.start == 1 ) {
			// ГЭД намагничен, можно переходить к отработке N или P
			//if ( (rf.psiZ > rf.PsiZ*0.97) && (psi > rf.psiZ*0.97) )
				inuWork = 1;
		}
		else {
			// всё выключаем
			inuWork = 2;
		}
		// чтобы стартануть без бросков тока
		rs.wmZi = rs.wmZ = wm;
		rp.pmZi = rp.pmZ = 0;
		iqZ = 0;
	}
	else if ( inuWork == 1 ) {
		if ( mst.start == 1 ) {
			// регулятор N --------------
			if ( mst.pzMode == 0 ) {
				csp.del = rs.wmZ - wm;
				csp.iqP = csp.del*csp.Kp;
				if ( (csp.iqLimFlag == 0) || (iqZ*csp.del < 0) )
					csp.iqI += csp.del*csp.Ki;
				csp.iqFF = rs.pWmZ/kMe*(WM_BAZ*J/M_BAZ);
				iqZ = csp.iqP + csp.iqI + csp.iqFF;
				// ограничение тока для ограничения мощности
				if ( wmAbs > WM_MIN ) {
					csp.iqLimAux = csp.pmLimZi/(wmAbs*kMe);
				}
				else {
					csp.iqLimAux = csp.pmLimZi/(WM_MIN*kMe);
				}
				if ( csp.iqLimAux < csp.IqLim ) {
					csp.iqLim = csp.iqLimAux;
				}
				else {
					csp.iqLim = csp.IqLim;
				}
			}
			// регулятор P --------------
			else { //if ( mst.pzMode == 1 )
				if ( wmAbs <= WM_MIN ) {
					iqZ = rp.pmZ/(WM_MIN*kMe);
				}
				else if ( wmAbs <= rf.WmNomPsi ) {
					iqZ = rp.pmZ/(wmAbs*kMe);
					csp.kMeNom = kMe;
				}
				else {
					iqZ = rp.pmZ/(wmAbs*csp.kMeNom);
				}
				// ограничение тока для ограничения оборотов
				if ( wmAbs < csp.wmLimZi*0.98 ) {
					csp.iqLimAux = fabs(iqZ);
				}
				else if ( wmAbs > csp.wmLimZi*1.02 ) {
					csp.iqLimAux = 0;
				}
				else {
					csp.iqLimAux = csp.iqLimZi;
				}
				// ... меняем скорость изменения ограничения тока (?)
				csp.delWmAbs = fabs(wmAbs - csp.wmLimZi);
				if ( csp.delWmAbs > 0.12 )
					csp.KizIncr = 10.0;
				else if ( csp.delWmAbs < 0.02 )
					csp.KizIncr = 0.1;
				else
					csp.KizIncr = 0.1 + (csp.delWmAbs - 0.02)*(10.0 - 0.1)/(0.12 - 0.02);
				// ... ЗИ
				if ( csp.iqLimAux - csp.iqLimZi > csp.IlimIncr*csp.KizIncr )
					csp.iqLimZi += csp.IlimIncr*csp.KizIncr;
				else if ( csp.iqLimZi - csp.iqLimAux > csp.IlimIncr*csp.KizIncr )
					csp.iqLimZi -= csp.IlimIncr*csp.KizIncr;
				else
					csp.iqLimZi = csp.iqLimAux;
				if ( csp.iqLimZi < csp.IqLim ) {
					csp.iqLim = csp.iqLimZi;
				}
				else {
					csp.iqLim = csp.IqLim;
				}
			} //mst.pzMode
			// для плавной остановки
			csp.pmZiRampDown = rp.pmEqv;
		}
		else { //if ( mst.start == 0 )
			// снижаем заданную мощность
			if ( 0 - csp.pmZiRampDown > mst.pDecrMaxTy ) {
				csp.pmZiRampDown += mst.pDecrMaxTy;
			}
			else if ( csp.pmZiRampDown - 0 > mst.pDecrMaxTy ) {
				csp.pmZiRampDown -= mst.pDecrMaxTy;
			}
			else {
				csp.pmZiRampDown = 0;
				// ток снижен - завершаем работу
				inuWork = 2;
			}
			// формируем заданный ток
			if ( wmAbs > WM_MIN ) {
				iqZ = csp.pmZiRampDown/(wmAbs*kMe);
			}
			else {
				iqZ = csp.pmZiRampDown/(WM_MIN*kMe);
			}
			// на случай, если mst.start восстановится раньше
			// завершения работы (inuWork = 2)
			rs.wmZi = rs.wmZ = wm;
			csp.iqI = iqZ;
			rp.pmZi = rp.pmZ = rp.pmEqv;
		} //mst.start

		// уставка ограничения снизу
		if ( -csp.iqLim > csp.IqLimNeg )
			csp.iqLimNeg = -csp.iqLim;
		else
			csp.iqLimNeg = csp.IqLimNeg;
		// ограничение моментообразующего тока
		if ( iqZ > csp.iqLim ) {
			iqZ = csp.iqLim;
			csp.iqLimFlag = 1;
		}
		else if ( iqZ < csp.iqLimNeg ) {
			iqZ = csp.iqLimNeg;
			csp.iqLimFlag = 1;
		}
		else {
			csp.iqLimFlag = 0;
		}
		// для плавного перехода
		if ( mst.pzMode == 0 ) {
			// ... в режим регулирования P
			rp.pmZ = iqZ*kMe*wmAbs;
			rp.pmZi = rp.pmZ;
			csp.iqLimZi = fabs(iqZ);
		}
		else {
			// ... в режим регулирования N
			csp.iqI = iqZ;
			csp.iqFF = 0;
			rs.wmZ = wm;
			rs.wmZi = rs.wmZ + csp.iqFF*0.05;
		}
	} //inuWork
} //void control_speed_power(void)



// Расчёты для indirect vector control
// (wmNf, wm, ix1, iy1, ix2, iy2 -> ivc.ws, ivc.sinTheta, ivc.cosTheta,
// ivc.id1, ivc.iq1, ivc.id2, ivc.iq2, ivc.psi)
void indirect_vector_control(void) {
	static float theta;

	if ( ivc.once == 0 ) {
		ivc.once = 1;
		ivc.im = 0;
		ivc.iq1 = 0;
		ivc.iq2 = 0;
		ivc.wr = 0;
		theta = 0;
	}

	// частота роторной ЭДС, o.e.
	if ( ivc.im > 4e-3 ) {
		ivc.wr = (ivc.iq1 + ivc.iq2)*0.5/ivc.im*sgmPar.TrInv*(1.0/WE_BAZ);
	}
	// частота поля статора, о.е.
	ivc.wsNf = wmNf + ivc.wr;
	ivc.ws = wm + ivc.wr;
	// электрический угол, рад.
	theta += ivc.wsNf*WE_BAZ*TY;
	if ( theta > PI2 )
		theta -= PI2;
	else if ( theta < 0 )
		theta += PI2;

	vect_control.theta = theta;
	// для координатных преобразований
	sincos(theta, &ivc.sinTheta, &ivc.cosTheta);
	// park transformations, о.е.
	ivc.id1 =  ix1*ivc.cosTheta + iy1*ivc.sinTheta;
	ivc.iq1 = -ix1*ivc.sinTheta + iy1*ivc.cosTheta;
	ivc.id2 =  ix2*ivc.cosTheta + iy2*ivc.sinTheta;
	ivc.iq2 = -ix2*ivc.sinTheta + iy2*ivc.cosTheta;
	// ток намагничивания, o.e.
	ivc.im += ((ivc.id1 + ivc.id2)*0.5 - ivc.im)*sgmPar.TrInv*TY;
	// амплитуда потока, o.e.
	ivc.psi = ivc.im*sgmPar.Lm*(1.0/L_BAZ);
} //void indirect_vector_control(void)



// Переводит сигналы управления из с.к. d-q в с.к. x-y
// (cc.yd1, cc.yq1, cc.yd2, cc.yq2, sinTheta, cosTheta, ws ->
// -> ip.yx1, ip.yy1, ip.yx2, ip.yy2)
void ipark(void) {
	ip.yx1Aux = cc.yd1*cosTheta - cc.yq1*sinTheta;
	ip.yy1Aux = cc.yd1*sinTheta + cc.yq1*cosTheta;
	ip.yx2Aux = cc.yd2*cosTheta - cc.yq2*sinTheta;
	ip.yy2Aux = cc.yd2*sinTheta + cc.yq2*cosTheta;
	// коррекция, связанная с дискретностью СУ
	ip.theta = ws*WE_BAZ*TY*1.5;//рад.
	sincos(ip.theta, &ip.sinTheta, &ip.cosTheta);
	ip.yx1 = ip.yx1Aux*ip.cosTheta - ip.yy1Aux*ip.sinTheta;
	ip.yy1 = ip.yx1Aux*ip.sinTheta + ip.yy1Aux*ip.cosTheta;
	ip.yx2 = ip.yx2Aux*ip.cosTheta - ip.yy2Aux*ip.sinTheta;
	ip.yy2 = ip.yx2Aux*ip.sinTheta + ip.yy2Aux*ip.cosTheta;
} //void ipark(void)



// Ограничивает полный ток
// (idZ, iqZ, IzLim -> idZ, iqZ, csp.iqLimFlag)
void limit_current(void) {
	iZ = sqrt(idZ*idZ + iqZ*iqZ);
	if ( iZ > IzLim ) {
		if ( iqZ >= 0 ) {
			iqZ = sqrt(IzLim*IzLim - idZ*idZ);
		}
		else {
			iqZ = -sqrt(IzLim*IzLim - idZ*idZ);
		}
		csp.iqLimFlag = 1;
	}
} //void limit_current(void)



// ШИМ
// (ip.yx1, ip.yy1, ip.yx2, ip.yy2 ->
// -> EPwm1Regs.CMPA.half.CMPA, EPwm2Regs.CMPA.half.CMPA,
// EPwm3Regs.CMPA.half.CMPA, EPwm4Regs.CMPA.half.CMPA,
// EPwm5Regs.CMPA.half.CMPA, EPwm6Regs.CMPA.half.CMPA,
// EPwm7Regs.CMPA.half.CMPA, EPwm8Regs.CMPA.half.CMPA,
// EPwm9Regs.CMPA.half.CMPA, EPwm10Regs.CMPA.half.CMPA,
// EPwm11Regs.CMPA.half.CMPA, EPwm12Regs.CMPA.half.CMPA)
void pwm(void) {
	static float yAux1;
	static float yAux2;
	static float ya;
	static float yb;
	static float yc;
	static float yPredm = 0;
	static float yaPredm;
	static float ybPredm;
	static float ycPredm;

	// переводим из с.к. x-y в с.к. a-b-c
	yAux1 = ip.yx1*(-0.5*ISQRT3);
	yAux2 = ip.yy1*0.5;
	ya = ip.yx1*ISQRT3;
	yb = yAux1 + yAux2;
	yc = yAux1 - yAux2;
	// предмодулирующее воздействие
	if ((ya >= yb) && (ya <= yc)) {
		yPredm = ya*0.5;
	}
	else if ((yc >= yb) && (yc <= ya)) {
		yPredm = yc*0.5;
	}
	else if ((yb >= yc) && (yb <= ya)) {
		yPredm = yb*0.5;
	}
	else if ((ya >= yc) && (ya <= yb)) {
		yPredm = ya*0.5;
	}
	else if ((yc >= ya) && (yc <= yb)) {
		yPredm = yc*0.5;
	}
	else if ((yb >= ya) && (yb <= yc)) {
		yPredm = yb*0.5;
	}
	yaPredm = (ya + yPredm)*2.;
	ybPredm = (yb + yPredm)*2.;
	ycPredm = (yc + yPredm)*2.;
	// full compare unit compare registers
	if (yaPredm >= 0) {
		EPwm1Regs.CMPA.half.CMPA = (unsigned short)yaPredm;
		EPwm2Regs.CMPA.half.CMPA = 0;
	}
	else {
		EPwm1Regs.CMPA.half.CMPA = 0;
		EPwm2Regs.CMPA.half.CMPA = (unsigned short)(-yaPredm);
	}
	if (ybPredm >= 0) {
		EPwm3Regs.CMPA.half.CMPA = (unsigned short)ybPredm;
		EPwm4Regs.CMPA.half.CMPA = 0;
	}
	else {
		EPwm3Regs.CMPA.half.CMPA = 0;
		EPwm4Regs.CMPA.half.CMPA = (unsigned short)(-ybPredm);
	}
	if (ycPredm >= 0) {
		EPwm5Regs.CMPA.half.CMPA = (unsigned short)ycPredm;
		EPwm6Regs.CMPA.half.CMPA = 0;
	}
	else {
		EPwm5Regs.CMPA.half.CMPA = 0;
		EPwm6Regs.CMPA.half.CMPA = (unsigned short)(-ycPredm);
	}

	// переводим из с.к. x-y в с.к. a-b-c
#ifndef SHIFT
	yAux1 = ip.yx2*(-0.5*ISQRT3);
	yAux2 = ip.yy2*0.5;
	ya = ip.yx2*ISQRT3;
	yb = yAux1 + yAux2;
	yc = yAux1 - yAux2;
#else //SHIFT
	yAux1 = ip.yx2*0.5;
	yAux2 = ip.yy2*0.5*ISQRT3;
	ya =  yAux1 + yAux2;
	yb = -yAux1 + yAux2;
	yc = ip.yy2*(-ISQRT3);
#endif //SHIFT
	// предмодулирующее воздействие
	if ((ya >= yb) && (ya <= yc)) {
		yPredm = ya*0.5;
	}
	else if ((yc >= yb) && (yc <= ya)) {
		yPredm = yc*0.5;
	}
	else if ((yb >= yc) && (yb <= ya)) {
		yPredm = yb*0.5;
	}
	else if ((ya >= yc) && (ya <= yb)) {
		yPredm = ya*0.5;
	}
	else if ((yc >= ya) && (yc <= yb)) {
		yPredm = yc*0.5;
	}
	else if ((yb >= ya) && (yb <= yc)) {
		yPredm = yb*0.5;
	}
	yaPredm = (ya + yPredm)*2.;
	ybPredm = (yb + yPredm)*2.;
	ycPredm = (yc + yPredm)*2.;
#ifdef ML
	// full compare unit compare registers
	if (yaPredm >= 0) {
		EPwm7Regs.CMPA.half.CMPA = (unsigned short)yaPredm;
		EPwm8Regs.CMPA.half.CMPA = 0;
	}
	else {
		EPwm7Regs.CMPA.half.CMPA = 0;
		EPwm8Regs.CMPA.half.CMPA = (unsigned short)(-yaPredm);
	}
	if (ybPredm >= 0) {
		EPwm9Regs.CMPA.half.CMPA = (unsigned short)ybPredm;
		EPwm10Regs.CMPA.half.CMPA = 0;
	}
	else {
		EPwm9Regs.CMPA.half.CMPA = 0;
		EPwm10Regs.CMPA.half.CMPA = (unsigned short)(-ybPredm);
	}
	if (ycPredm >= 0) {
		EPwm11Regs.CMPA.half.CMPA = (unsigned short)ycPredm;
		EPwm12Regs.CMPA.half.CMPA = 0;
	}
	else {
		EPwm11Regs.CMPA.half.CMPA = 0;
		EPwm12Regs.CMPA.half.CMPA = (unsigned short)(-ycPredm);
	}
#endif //ML

	// разрешаем импульсы
	EALLOW;
	EPwm1Regs.TZCLR.all = 0x0004;
	EPwm2Regs.TZCLR.all = 0x0004;
	EPwm3Regs.TZCLR.all = 0x0004;
	EPwm4Regs.TZCLR.all = 0x0004;
	EPwm5Regs.TZCLR.all = 0x0004;
	EPwm6Regs.TZCLR.all = 0x0004;
#ifdef ML
	EPwm7Regs.TZCLR.all = 0x0004;
	EPwm8Regs.TZCLR.all = 0x0004;
	EPwm9Regs.TZCLR.all = 0x0004;
	EPwm10Regs.TZCLR.all = 0x0004;
	EPwm11Regs.TZCLR.all = 0x0004;
	EPwm12Regs.TZCLR.all = 0x0004;
#endif //ML
	EDIS;
} //void pwm(void)



// Формирует заданный поток
// (rf.PsiZ -> rf.psiZ, rf.pPsiZ)
void reference_flux(void) {
	if ( rf.once == 0 ) {
		rf.once = 1;
		rf.KpsiSub = TY*DECIM_PSI_WM_PM/6.0;
		rf.psiZi = 0;
		cc.y1 = 0;
		cc.y2 = 0;
		rf.psiSub = 0;
		rf.psiZ = 0;
		rf.psiZPrev1 = 0;
		rf.psiZPrev2 = 0;
		rf.psiZPrev3 = 0;
	}

	// ЗИ
	if ( rf.PsiZ - rf.psiZi > rf.PsizIncr ) {
		rf.psiZi += rf.PsizIncr;
	}
	else if ( rf.psiZi - rf.PsiZ > rf.PsizIncr ) {
		rf.psiZi -= rf.PsizIncr;
	}
	else {
		rf.psiZi = rf.PsiZ;
	}
	// коррекция в соответствии со скоростью
	if ( wmAbs <= rf.WmNomPsi )
		rf.psiZCorr = rf.psiZi;
	else
		rf.psiZCorr = rf.psiZi*rf.WmNomPsi/wmAbs;
	// коррекция в соответствии с противоЭДС
	if ( (cc.y1 > rf.YlimPsi) || (cc.y2 > rf.YlimPsi) ) {
		rf.psiSub += (rf.psiZCorr - rf.psiSub)*rf.KpsiSub;
	}
	else {
		rf.psiSub += (0 - rf.psiSub)*rf.KpsiSub;
	}
	rf.psiZCorr2 = rf.psiZCorr - rf.psiSub;
	// чтобы задание менялось чуть плавнее
	rf.psiZ += (rf.psiZCorr2 - rf.psiZ)*rf.Kpsiz;

	// производная заданного потокосцепления
	rf.pPsiZ = (rf.psiZ - rf.psiZPrev3)/(TY*DECIM_PSI_WM_PM*3.);
	rf.psiZPrev3 = rf.psiZPrev2;
	rf.psiZPrev2 = rf.psiZPrev1;
	rf.psiZPrev1 = rf.psiZ;
} //void reference_flux(void)



// Формирует заданную мощность
// (mst.pmZz, mst.pmLim -> rp.pmZ)
void reference_power(void) {
	if ( rp.once == 0 ) {
		rp.once = 1;
		rp.pmZi = 0;
		rp.pmZ = 0;
	}

	// ограничение
	if ( fabs(mst.pmZz) > mst.pmLim ) {
		if ( mst.pmZz >= 0 )
			rp.pmZz = mst.pmLim;
		else
			rp.pmZz = -mst.pmLim;
	}
	else {
		rp.pmZz = mst.pmZz;
	}
	// для ограничения приращения мощности (?)
	if ( fabs(rp.pmZi - rp.pmEqv) > 0.02 )
		rp.KpIncrDecr = 0.10;
	else
		rp.KpIncrDecr = 1.00;
	// ЗИ
	if ( rp.pmZz - rp.pmZi > mst.pIncrMaxTy*rp.KpIncrDecr ) {
		rp.pmZi += mst.pIncrMaxTy*rp.KpIncrDecr;
	}
	else if ( rp.pmZi - rp.pmZz > mst.pDecrMaxTy*rp.KpIncrDecr ) {
		rp.pmZi -= mst.pDecrMaxTy*rp.KpIncrDecr;
	}
	else {
		rp.pmZi = rp.pmZz;
	}
	// чтобы задание менялось чуть плавнее
	// rp.pmZ += (rp.pmZi - rp.pmZ)*rp.Kpmz;
	rp.pmZ = rp.pmZz;
} //void reference_power(void)



// Формирует заданную скорость
// (mst.wmZz, mst.wmLim -> rs.wmZ, rs.pWmZ)
void reference_speed(void) {
	if ( rs.once == 0 ) {
		rs.once = 1;
		rs.wmZi = rs.wmZ = wm;
		rs.wzIncr = rs.WlimIncr;
		rs.wmZPrev1 = rs.wmZ;
		rs.wmZPrev2 = rs.wmZ;
		rs.wmZPrev3 = rs.wmZ;
		rs.tPwmZ = 0;
	}

	// ограничение
	if ( fabs(mst.wmZz) > mst.wmLim ) {
		if ( mst.wmZz >= 0 )
			rs.wmZz = mst.wmLim;
		else
			rs.wmZz = -mst.wmLim;
	}
	else {
		rs.wmZz = mst.wmZz;
	}
	// для ограничения приращения мощности (?)
	if ( fabs(rs.wmZi) < 0.5 )
		rs.wzIncrNf = rs.WlimIncr*3.5;
	else if ( fabs(rs.wmZi) < 0.8 )
		rs.wzIncrNf = rs.WlimIncr*2.0;
	else
		rs.wzIncrNf = rs.WlimIncr;
	rs.wzIncr += (rs.wzIncrNf - rs.wzIncr)*(TY*DECIM_PSI_WM_PM)/0.25;
	// ЗИ
	if ( rs.wmZz - rs.wmZi > rs.wzIncr ) {
		rs.wmZi += rs.wzIncr;
	}
	else if ( rs.wmZi - rs.wmZz > rs.wzIncr ) {
		rs.wmZi -= rs.wzIncr;
	}
	else {
		rs.wmZi = rs.wmZz;
	}
	// чтобы задание менялось чуть плавнее
	// rs.wmZ += (rs.wmZi - rs.wmZ)*rs.Kwmz;
	rs.wmZ = rs.wmZz;

	// производная заданной скорости
	rs.pWmZ = (rs.wmZ - rs.wmZPrev3)/(TY*DECIM_PSI_WM_PM*3.);
	rs.wmZPrev3 = rs.wmZPrev2;
	rs.wmZPrev2 = rs.wmZPrev1;
	rs.wmZPrev1 = rs.wmZ;
	// ... чтобы избежать бросков при входе в рабочий режим
	// if ( (inuWork == 0) || (mst.start == 0) || (mst.pzMode == 1) )
	// 	rs.tPwmZ = 0;
	// if ( rs.tPwmZ <= 3 ) {
	// 	rs.tPwmZ++;
	// 	rs.pWmZ = 0;
	// }
} //void reference_speed(void)



// Выбирает о.с.
// (... -> ws, sinTheta, cosTheta, id1, iq1, id2, iq2, psi)
void select_feedback(void) {
	ws = ivc.ws;
	sinTheta = ivc.sinTheta;
	cosTheta = ivc.cosTheta;
	id1 = ivc.id1;
	iq1 = ivc.iq1;
	id2 = ivc.id2;
	iq2 = ivc.iq2;
	psi = ivc.psi;
} //void select_feedback(void)

void write_swgen_pwm_times_split_eages(unsigned int mode_reload) {

	xpwm_time.Ta0_0 = (unsigned int) svgen_pwm24_1.Ta_0.Ti;
	xpwm_time.Ta0_1 = (unsigned int) svgen_pwm24_1.Ta_1.Ti;
	xpwm_time.Tb0_0 = (unsigned int) svgen_pwm24_1.Tb_0.Ti;
	xpwm_time.Tb0_1 = (unsigned int) svgen_pwm24_1.Tb_1.Ti;
	xpwm_time.Tc0_0 = (unsigned int) svgen_pwm24_1.Tc_0.Ti;
	xpwm_time.Tc0_1 = (unsigned int) svgen_pwm24_1.Tc_1.Ti;

	xpwm_time.Ta1_0 = (unsigned int) svgen_pwm24_2.Ta_0.Ti;
	xpwm_time.Ta1_1 = (unsigned int) svgen_pwm24_2.Ta_1.Ti;
	xpwm_time.Tb1_0 = (unsigned int) svgen_pwm24_2.Tb_0.Ti;
	xpwm_time.Tb1_1 = (unsigned int) svgen_pwm24_2.Tb_1.Ti;
	xpwm_time.Tc1_0 = (unsigned int) svgen_pwm24_2.Tc_0.Ti;
	xpwm_time.Tc1_1 = (unsigned int) svgen_pwm24_2.Tc_1.Ti;

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

	xpwm_time.mode_reload = PWM_MODE_RELOAD_FORCE;// mode_reload;

   xpwm_time.write_1_2_winding_break_times_split(&xpwm_time);
}

void write_CMP_tims(int calcPWMtimes) {

	// if (calcPWMtimes == 0) {
	// 	return;
	// }

	// EPwm1Regs.CMPA.half.CMPA = xpwm_time.Ta0_1;
	// EPwm2Regs.CMPA.half.CMPA = xpwm_time.Ta0_0;
	
	// EPwm3Regs.CMPA.half.CMPA = xpwm_time.Tb0_1;
	// EPwm4Regs.CMPA.half.CMPA = xpwm_time.Tb0_0;

	// EPwm5Regs.CMPA.half.CMPA = xpwm_time.Tc0_1;
	// EPwm6Regs.CMPA.half.CMPA = xpwm_time.Tc0_0;


	// EPwm7Regs.CMPA.half.CMPA = xpwm_time.Ta1_1;
	// EPwm8Regs.CMPA.half.CMPA = xpwm_time.Ta1_0;

	// EPwm9Regs.CMPA.half.CMPA = xpwm_time.Tb1_1;
	// EPwm10Regs.CMPA.half.CMPA = xpwm_time.Tb1_0;

	// EPwm11Regs.CMPA.half.CMPA = xpwm_time.Tc1_1;
	// EPwm12Regs.CMPA.half.CMPA = xpwm_time.Tc1_0;

		// разрешаем импульсы
	EALLOW;
	EPwm1Regs.TZCLR.all = 0x0004;
	EPwm2Regs.TZCLR.all = 0x0004;
	EPwm3Regs.TZCLR.all = 0x0004;
	EPwm4Regs.TZCLR.all = 0x0004;
	EPwm5Regs.TZCLR.all = 0x0004;
	EPwm6Regs.TZCLR.all = 0x0004;
#ifdef ML
	EPwm7Regs.TZCLR.all = 0x0004;
	EPwm8Regs.TZCLR.all = 0x0004;
	EPwm9Regs.TZCLR.all = 0x0004;
	EPwm10Regs.TZCLR.all = 0x0004;
	EPwm11Regs.TZCLR.all = 0x0004;
	EPwm12Regs.TZCLR.all = 0x0004;
#endif //ML
	EDIS;
}