401 lines
12 KiB
C
401 lines
12 KiB
C
/**************************************************************************
|
||
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
|
||
// <20> òàòíàÿ îñòàíîâêà
|
||
//-------------------------------------------------------------
|
||
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)
|