/**************************************************************************
	Description: Функция обслуживания прерывания от АЦП.
	Опрашивает АЦП, содержит блок обработки входных величин
	(напряжений, токов, частоты вращения), блок защит,
	блок с управляющей логикой, блок для работы с ПУ и EEPROM.

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


#include "def.h"
#include "isr.h"


#pragma CODE_SECTION(isr, "ramfuncs");
#pragma CODE_SECTION(make_pause_before_rerun, "ramfuncs");
#pragma CODE_SECTION(sens_wm, "ramfuncs2");
#pragma CODE_SECTION(snapshot_emergency, "ramfuncs");
#pragma CODE_SECTION(stop_inu, "ramfuncs");


void make_pause_before_rerun(void);
void sens_wm(void);
void snapshot_emergency(void);
void stop_inu(void);
extern short test_param(void);
extern void upr(void);


#ifndef ML
interrupt void isr(void) {
#else //ML
void isr(void) {
#endif //ML
	// результаты АЦП
	result.udc1 = AdcMirror.ADCRESULT0 - offset.Udc1;
	result.ic1 = AdcMirror.ADCRESULT2 - offset.Ic1;
	result.ia1 = AdcMirror.ADCRESULT4 - offset.Ia1;
	result.ib1 = AdcMirror.ADCRESULT6 - offset.Ib1;

	// reset SEQ1 or the cascaded sequencer immediately to an initial
	// "pretriggered" state, i.e., waiting for a trigger at CONV00
	AdcRegs.ADCTRL2.bit.RST_SEQ1 = 1;


	// Обработка и вычисление всякого
	//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	udc1Nf = (float)result.udc1*GAIN_UDC;//ед. АЦП -> о.е.
	ia1Nf = (float)result.ia1*GAIN_IAC;//ед. АЦП -> о.е.
	ib1Nf = (float)result.ib1*GAIN_IAC;//ед. АЦП -> о.е.
	// Udc1, o.e.
	udc1 += (udc1Nf - udc1)*Kudc;
	// ... (для вывода)
	out.udc1 += (udc1Nf - out.udc1)*out.K;

	// Iac, о.е.
	// ... проекции на оси x-y
	ix1 = ia1Nf;
	iy1 = (ia1Nf + 2.*ib1Nf)*ISQRT3;

	// ... амплитуда
	iac1Nf = sqrt(ix1*ix1 + iy1*iy1);
	// ... (для вывода)
	out.iac1 += (iac1Nf - out.iac1)*out.K;

	// Wm, o.e. (EQep2Regs.QPOSCNT -> wmNf, wm, out.wm)
	sens_wm();

	// Me, o.e.
	kMe = sgmPar.Kl*psi*KmeCorr;
	meNf = iq1*kMe;
	me += (meNf - me)*Kme;//для защит и ограничения
	out.me += (meNf - out.me)*out.K;//для вывода

	// Pm, o.e.
	pm = wm*me;
	out.pm += (pm - out.pm)*out.K;//для вывода
	// ... мощность, которую можно сравнивать с заданной
	if ( wm >= 0 )
		rp.pmEqv = pm;
	else
		rp.pmEqv = -pm;

	// для выдержки паузы перед повторным пуском
	// (state -> stopPause)
	make_pause_before_rerun();




	// Защиты
	//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	// ----------------------------------------------------------------
	if ( state == STATE_WORK ) {
		// повышение тока
		if ( (result.ia1 > protect.IacMax) || (result.ia1 < protect.IacMin) ||
				(result.ib1 > protect.IacMax) || (result.ib1 < protect.IacMin) ||
				(result.ic1 > protect.IacMax) || (result.ic1 < protect.IacMin) ) {
			faultNo = 22;
			state = STATE_SHUTDOWN;
		}
		// понижение выпрямленного напряжения
		if ( udc1Nf < protect.UdcMin ) {
			if ( protect.tUdc1Min < protect.TudcMin ) {
				protect.tUdc1Min++;
			}
			else {
				faultNo = 30;
				state = STATE_SHUTDOWN;
			}
		}
		else {
			if ( protect.tUdc1Min > 0 )
				protect.tUdc1Min--;
		}
	} //if ( state == STATE_WORK )
	// ----------------------------------------------------------------
	if ( state != STATE_SHUTDOWN ) {
		// повышение выпрямленного напряжения
		if ( udc1Nf > protect.UdcMax ) {
			faultNo = 24;
			state = STATE_SHUTDOWN;
		}
		// повышение оборотов
		if ( wm > protect.WmMax ) {
			if ( protect.tWmMax < protect.TwmMax ) {
				protect.tWmMax++;
			}
			else {
				faultNo = 32;
				state = STATE_SHUTDOWN;
			}
		}
		else {
			if ( protect.tWmMax > 0 )
				protect.tWmMax--;
		}
		// неисправность источника питания +24 В
		if ( DI_24V_SOURCE_FAULT == 1 ) {
			if ( protect.tDI24VSource < protect.Tdi24VSource ) {
				protect.tDI24VSource++;
			}
			else {
				faultNo = 7;
				state = STATE_SHUTDOWN;
			}
		}
		else {
			if ( protect.tDI24VSource > 0 ) {
				protect.tDI24VSource--;
			}
		}
	} //if ( state != STATE_SHUTDOWN )




	// Режим работы INU
	//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	switch ( state ) {
	// Аварийная остановка
	//-------------------------------------------------------------
	case STATE_SHUTDOWN:
		stop_inu();
		// в реж. STATE_SHUTDOWN выводимые величины должны оставаться
		// такими какими они были в момент срабат-ия защиты
		if ( onceShutdown == 0 ) {
			onceShutdown = 1;
			snapshot_emergency();
		}
		// переход в режим STATE_STOP
		if ( mst.faultReset == 1 ) {
			// чтобы не пуститься с неправильными параметрами
			testParamFaultNo = test_param();
			if ( testParamFaultNo == 0 )
				onceFaultReset = 1;
			else
				faultNo = 4;
		}
		if ( onceFaultReset == 1 ) {
			onceFaultReset = 0;
			state = STATE_STOP;
			faultNo = 0;
			onceShutdown = 0;
			// для защит
			protect.tWmMax = 0;
			protect.tDI24VSource = 0;
		}
		break;//STATE_SHUTDOWN
	// Штатная остановка
	//-------------------------------------------------------------
	case STATE_STOP:
		stop_inu();
		// переход в режим STATE_WORK
		if ( (mst.start == 1) && (stopPause == 1) ) {
			state = STATE_WORK;
			onceUpr = 0;
			// для защит
			protect.tUdc1Min = 0;
		}
		break;//STATE_STOP
	// Работа
	//-------------------------------------------------------------
	case STATE_WORK:
		// реализует алгоритм управления
		upr();
		// переход в режим STATE_STOP
		if ( (mst.start == 0) && (inuWork == 2) ) {
			state = STATE_STOP;
		}
		break;//STATE_WORK
	} //switch ( state )



	// service watchdog
	EALLOW;
	SysCtrlRegs.WDKEY = 0x55;
	SysCtrlRegs.WDKEY = 0xAA;
	EDIS;


	// clear Interrupt Flag ADC Sequencer 1
	AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1;
	// acknowledge PIE Interrupt
	PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
} //isr()



/* Устанавливает флажок, если пробыли заданное время в выключенном
состоянии (чтобы ГЭД размагнитился перед повторным пуском)
(state -> stopPause) */
void make_pause_before_rerun(void) {
	static unsigned short T = (unsigned short)(5.0/TY);
	static unsigned short t = 0;

	if ( state == STATE_WORK ) {
		stopPause = 0;
		t = 0;
	}
	else {
		if ( t < T )
			t++;
		else
			stopPause = 1;
	}
} //void make_pause_before_rerun(void)



// Вычисляет частоту вращения (EQep2Regs.QPOSCNT -> wmNf, wm, out.wm)
void sens_wm(void) {
	static short once = 0;
	static unsigned short qposmax;
	static unsigned short qepCnt;
	static unsigned short qepCntPrev = 0;
	static unsigned short QepCntDelMax = (unsigned short)(QEP_CNT_DEL_NOM*2.0);
	static short qepCntDel;

	if ( once == 0 ) {
		once = 1;
		qposmax = (unsigned short)EQep2Regs.QPOSMAX;
	}

	// вычисляем скорость по приращению счётчика импульсов от ДС
	qepCnt = (unsigned short)EQep2Regs.QPOSCNT;
	if ( qepCnt + QepCntDelMax < qepCntPrev )
		qepCntDel = (short)(qposmax - qepCntPrev + qepCnt + 1);//overflow
	else if ( qepCntPrev + QepCntDelMax < qepCnt )
		qepCntDel = (short)(qepCnt - qposmax - qepCntPrev - 1);//underflow
	else
		qepCntDel = (short)(qepCnt - qepCntPrev);
	qepCntPrev = qepCnt;
	wmNf = (float)qepCntDel*GAIN_WM;//о.е.
	// фильтруем
	wm += (wmNf - wm)*Kwm;
	out.wm += (wmNf - out.wm)*out.K;//для вывода
	wmAbs = fabs(wm);
} //void sens_wm(void)



// Запоминает величины в момент срабатывания защиты
void snapshot_emergency(void) {
	emerg.udc1 = udc1Nf;
	emerg.iac1 = iac1Nf;
	emerg.me = me;
	emerg.wm = wm;
	emerg.pm = pm;
} //void snapshot_emergency(void)



// Снимает импульсы управления с INU
void stop_inu(void) {
	// forces a one-shot trip event
	EALLOW;
	EPwm1Regs.TZFRC.all = 0x0004;
	EPwm2Regs.TZFRC.all = 0x0004;
	EPwm3Regs.TZFRC.all = 0x0004;
	EPwm4Regs.TZFRC.all = 0x0004;
	EPwm5Regs.TZFRC.all = 0x0004;
	EPwm6Regs.TZFRC.all = 0x0004;
#ifdef ML
	EPwm7Regs.TZFRC.all = 0x0004;
	EPwm8Regs.TZFRC.all = 0x0004;
	EPwm9Regs.TZFRC.all = 0x0004;
	EPwm10Regs.TZFRC.all = 0x0004;
	EPwm11Regs.TZFRC.all = 0x0004;
	EPwm12Regs.TZFRC.all = 0x0004;
#endif
	EDIS;
	// на всякий случай
	EPwm1Regs.CMPA.half.CMPA = 0;
	EPwm2Regs.CMPA.half.CMPA = 0;
	EPwm3Regs.CMPA.half.CMPA = 0;
	EPwm4Regs.CMPA.half.CMPA = 0;
	EPwm5Regs.CMPA.half.CMPA = 0;
	EPwm6Regs.CMPA.half.CMPA = 0;
#ifdef ML
	EPwm7Regs.CMPA.half.CMPA = 0;
	EPwm8Regs.CMPA.half.CMPA = 0;
	EPwm9Regs.CMPA.half.CMPA = 0;
	EPwm10Regs.CMPA.half.CMPA = 0;
	EPwm11Regs.CMPA.half.CMPA = 0;
	EPwm12Regs.CMPA.half.CMPA = 0;
#endif
	// для передачи на ВУ
	inuWork = 0;
	// для вычисления meNf
	psi = 0;
} //void stop_inu(void)
