335 lines
8.2 KiB
C
335 lines
8.2 KiB
C
/**************************************************************************
|
|
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)
|