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

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


#include "def.h"
#include "upr.h"
#include "estimate.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(float ya, float yb, float yc);
void reference_flux(void);
void reference_power(void);
void reference_speed(void);
void select_feedback(void);


void upr(void) {
	static float yAux1;
	static float yAux2;
	static float ya;
	static float yb;
	static float yc;
	static short decim_psi_wm_pm;
	static Estimate_Test_t step;

	if ( onceUpr == 0 ) {
		estimate_init();
		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;
		estimate_start(ESTIMATE_TEST_RR_L);
	}

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

			estimate_process(cc.yd1, cc.yq1, id1, iq1, TY * DECIM_PSI_WM_PM,
			&idZ, &iqZ, NULL);

		//// заданное потокосцепление
		//// (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

	// 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, 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)



	// переводим из с.к. 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;
	pwm(ya, yb, yc);
} //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.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;
	if ( estimate_get_step() == ESTIMATE_TEST_LM ) {
		cc.yd1FF = 0;
		cc.yq1FF = 0;
	}
	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;
	}
}




// Регулирует потокосцепление
// (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.wr = 0;
		theta = 0;
	}

	// частота роторной ЭДС, o.e.
	if ( ivc.im > 4e-3 ) {
		ivc.wr = (ivc.iq1)/ivc.im*sgmPar.TrInv*(1.0/WE_BAZ);
	}
	if ( estimate_get_step() == ESTIMATE_TEST_LM ) {
		ivc.wr = 0;
		ivc.wsNf = 0;
		ivc.ws = 0;
	}
	else {
		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;
	}
	
	theta_out = theta;//Вывод угла тетта в Simulink
	
	// для координатных преобразований
	sincos(theta, &ivc.sinTheta, &ivc.cosTheta);
	// park transformations, о.е.
	ivc.id1 =  ix1*ivc.cosTheta + iy1*ivc.sinTheta;
	ivc.iq1 = -ix1*ivc.sinTheta + iy1*ivc.cosTheta;
	// ток намагничивания, o.e.
	ivc.im += (ivc.id1 - 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.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;
} //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(float ya, float yb, float yc) {
	static float yPredm = 0;
	static float yaPredm;
	static float ybPredm;
	static float ycPredm;

	// предмодулирующее воздействие
	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);
	}



	// разрешаем импульсы
	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;
} //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;
		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) {
		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;
} //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.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;
	psi = ivc.psi;
} //void select_feedback(void)