/**************************************************************************
	Description: Ôóíêöèÿ îáñëóæèâàíèÿ ïðåðûâàíèÿ îò ÀÖÏ.
	Îïðàøèâàåò ÀÖÏ, ñîäåðæèò áëîê îáðàáîòêè âõîäíûõ âåëè÷èí
	(íàïðÿæåíèé, òîêîâ, ÷àñòîòû âðàùåíèÿ), áëîê çàùèò,
	áëîê ñ óïðàâëÿþùåé ëîãèêîé, áëîê äëÿ ðàáîòû ñ ÏÓ è EEPROM.

	Àâòîð: Óëèòîâñêèé Ä.È.
	Äàòà ïîñëåäíåãî îáíîâëåíèÿ: 2021.11.08
**************************************************************************/


#include "def.h"
#include "isr.h"
// #include "controller.h"
#include "adc_tools.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.udc2 = AdcMirror.ADCRESULT1 - offset.Udc2;
	result.ic1 = AdcMirror.ADCRESULT2 - offset.Ic1;
	result.ic2 = AdcMirror.ADCRESULT3 - offset.Ic2;
	result.ia1 = AdcMirror.ADCRESULT4 - offset.Ia1;
	result.ia2 = AdcMirror.ADCRESULT5 - offset.Ia2;
	result.ib1 = AdcMirror.ADCRESULT6 - offset.Ib1;
	result.ib2 = AdcMirror.ADCRESULT7 - offset.Ib2;

	rawData.U1_1 = result.udc1 / 2;
	rawData.U1_2 = result.udc1 / 2;
	rawData.Izpt1_1 = 0;
	rawData.Izpt1_2 = 0;
	rawData.Ia1 = result.ia1;
	rawData.Ib1 = result.ib1;
	rawData.Ic1 = result.ic1;
	rawData.U2_1 = result.udc2 / 2;
	rawData.U2_2 = result.udc2 / 2;
	rawData.Izpt2_1 = 0;
	rawData.Izpt2_2 = 0;
	rawData.Ia2 = result.ia2;
	rawData.Ib2 = result.ib2;
	rawData.Ic2 = result.ic2;

	// 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;//åä. ÀÖÏ -> î.å.
	udc2Nf = (float)result.udc2*GAIN_UDC;//åä. ÀÖÏ -> î.å.
	ia1Nf = (float)result.ia1*GAIN_IAC;//åä. ÀÖÏ -> î.å.
	ib1Nf = (float)result.ib1*GAIN_IAC;//åä. ÀÖÏ -> î.å.
	ia2Nf = (float)result.ia2*GAIN_IAC;//åä. ÀÖÏ -> î.å.
	ib2Nf = (float)result.ib2*GAIN_IAC;//åä. ÀÖÏ -> î.å.

	// Udc1, o.e.
	udc1 += (udc1Nf - udc1)*Kudc;
	udc2 += (udc2Nf - udc2)*Kudc;
	// ... (äëÿ âûâîäà)
	out.udc1 += (udc1Nf - out.udc1)*out.K;
	out.udc2 += (udc2Nf - out.udc2)*out.K;

	// Iac, î.å.
	// ... ïðîåêöèè íà îñè x-y
	ix1 = ia1Nf;
	iy1 = (ia1Nf + 2.*ib1Nf)*ISQRT3;
#ifndef SHIFT
	ix2 = ia2Nf;
	iy2 = (ia2Nf + 2.*ib2Nf)*ISQRT3;
#else //SHIFT
	ix2 = (ia2Nf - ib2Nf)*ISQRT3;
	iy2 = ia2Nf + ib2Nf;
#endif //SHIFT
	// ... àìïëèòóäà
	iac1Nf = sqrt(ix1*ix1 + iy1*iy1);
	iac2Nf = sqrt(ix2*ix2 + iy2*iy2);
	// ... (äëÿ âûâîäà)
	out.iac1 += (iac1Nf - out.iac1)*out.K;
	out.iac2 += (iac2Nf - out.iac2)*out.K;

	// Wm, o.e. (EQep2Regs.QPOSCNT -> wmNf, wm, out.wm)
	sens_wm();

	// Me, o.e.
	kMe = sgmPar.Kl*psi*KmeCorr;
	meNf = (iq1 + iq2)*0.5*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;

	//Ôóíêöèè ÷òåíèÿ ÀÖÏ è ðàñ÷¸òà àíàëîãîâûõ çíà÷åíèé
	//èç ïðîåêòà 22220
	read_adc(&rawData);
	acp_Handler();

	// äëÿ âûäåðæêè ïàóçû ïåðåä ïîâòîðíûì ïóñêîì
	// (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 ( (result.ia2 > protect.IacMax) || (result.ia2 < protect.IacMin) ||
				(result.ib2 > protect.IacMax) || (result.ib2 < protect.IacMin) ||
				(result.ic2 > protect.IacMax) || (result.ic2 < protect.IacMin) ) {
			faultNo = 23;
			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 ( udc2Nf < protect.UdcMin ) {
			if ( protect.tUdc2Min < protect.TudcMin ) {
				protect.tUdc2Min++;
			}
			else {
				faultNo = 31;
				state = STATE_SHUTDOWN;
			}
		}
		else {
			if ( protect.tUdc2Min > 0 )
				protect.tUdc2Min--;
		}*/
	} //if ( state == STATE_WORK )
	// ----------------------------------------------------------------
	if ( state != STATE_SHUTDOWN ) {
		// ïîâûøåíèå âûïðÿìëåííîãî íàïðÿæåíèÿ
		/*if ( udc1Nf > protect.UdcMax ) {
			faultNo = 24;
			state = STATE_SHUTDOWN;
		}
		if ( udc2Nf > protect.UdcMax ) {
			faultNo = 25;
			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;
			protect.tUdc2Min = 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.udc2 = udc2Nf;
	emerg.iac1 = iac1Nf;
	emerg.iac2 = iac2Nf;
	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)