123
Fork of LG by
Diff: host/Source/App/PLC_reg.c
- Revision:
- 22:12e6183f04d4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/host/Source/App/PLC_reg.c Wed Feb 03 10:44:42 2016 +0300 @@ -0,0 +1,353 @@ +/****************************************Copyright (c)**************************************************** +**--------------File Info--------------------------------------------------------------------------------- +** File name: PLC_reg.c +** Last modified Date: 2011-09-26 +** Last Version: V1.00 +** Descriptions: Routines for system of perimeter regulating unit +** +**-------------------------------------------------------------------------------------------------------- +** Created by: Electrooptica Incorp. +** Created date: 2011-09-26 +** Version: V1.00 +** Descriptions: +** +**-------------------------------------------------------------------------------------------------------- +*********************************************************************************************************/ +#include "mathDSP.h" +#include "CyclesSync.h" +#include "ThermoCalc.h" +#include "CntrlGLD.h" +#include "InputOutput.h" +#include <math.h> + +#define CONFIG_HFO_REG //r. èçìåíÿåì êîýôôèöèåíò ïåðåäà÷è êîíòóðà ÃÂ× îò íîìèíàëüíîãî íà âðåìÿ îáíóëåíèÿ +#define WP_TRANSITION_ENA // + + //e.--- constants for the CPLC regulator ------------------------------------------------------- //r.--- êîíñòàíòû äëÿ êîíòóðà ÑÐÏ ------------------------------------------------------- + +#define PLC_SHIFT (6) +#define PLC_PHASE_DET_SHIFT (18) //e. 18 - for analog output //r. 18 - äëÿ àíàëîãîâîãî + +#define PLC_RESET_THRESHOLD (-3276) //e. correspond to the voltage +1.2 Volts //r. ñîîòâåòñòâóåò íàïðÿæåíèþ +1.2 âîëüòà +#define WP_REG32MAX_SATURATION (32767 << PLC_SHIFT) +#define WP_REG32MIN_NEW_SATURATION (PLC_RESET_THRESHOLD << PLC_SHIFT) +#define WP_TMP_THRESHOLD (7) //e. temperature threshold, defining heats up or cool down the device //r. òåìïåðàòóðíûé ïîðîã, îïðåäåëÿþùèé íàãðåâàåòñÿ èëè îõëàæäàåòñÿ ïðèáîð + + +#define debugPLC + + int WP_reg32; + int WP_Phase_Det; //e. output of the phase detector of the CPLC (in a digital kind)//r. âûõîä ôàçîâîãî äåòåêòîðà ÑÐÏ (â öèôðîâîì âèäå) + int WP_reset_heating; //e. voltage of reset at heating //r. íàïðÿæåíèå ñáðîñà ïðè íàãðåâàíèè + int WP_reset_cooling; //e. voltage of reset at cooling //r. íàïðÿæåíèå ñáðîñà ïðè îõëàæäåíèè + int MaxDelayPLC; + int sin_func[100]; + +int phase_Digital; + +int WP_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime); + +/****************************************************************************** +** Function name: init_PLC +** +** Descriptions: Initialization procedure for PLC regulator +** +** Parameters: None +** Returned value: None +** +******************************************************************************/ +void init_PLC(void) +{ + int i; + //( 1,2 âîëüòà) + if (Device_blk.Str.WP_reset < PLC_RESET_THRESHOLD) //e. íàïðÿæåíèå ïîñëå ñáðîñà íà íàãðåâàòåëå íå äîëæíî ïðåâûøàòü 1,2 âîëüòà. + //(èñõîäíîå çíà÷åíèå ðåãóëÿòîðà ÑÐÏ (ïîñëå ñáðîñà)) < (-3276). + { + Device_blk.Str.WP_reset = PLC_RESET_THRESHOLD + 1;//(-3275) + } +//íàïðÿæåíèå íà ÑÐÏ = (ìèí. çíà÷åíèå íà íàãðåâàòåëå + ìàõ. çíà÷åíèå íà íàãðåâàòåëå)/2 + Output.Str.WP_reg = (Device_blk.Str.WP_rup + Device_blk.Str.WP_rdw) >> 1; //e. WP_reg start voltage is (WP_rup - WP_rdw)/2 + + // íàïðÿæåíèå íà ÑÐÏ << 6 + WP_reg32 = Output.Str.WP_reg<<PLC_SHIFT; + + if ((Device_blk.Str.PI_b3>100)||(Device_blk.Str.PI_b3<10)) //e. Åñëè òðåáóåìàÿ ÷àñòîòà ìîäóëÿòîðà ÑÐÏ áîëüøå 1kHz èëè ìåíüøå 100Hz + Device_blk.Str.PI_b3 = 40; //e. Óñòàíîâèòü ÷àñòîòó â 250Hz (÷àñòîòà äðåáåçäåíèÿ) + + for (i = 0; i<Device_blk.Str.PI_b3; i++) //e. Ñêàíèðîâàíèå ÑÐÏ ñèãíàëà + { + float temp = sin((float)i*2.0*PI/(float)Device_blk.Str.PI_b3); /// âû÷èñëåíèå çíà÷åíèé ñèíóñà + /// äëÿ ÷àñòîòû ìîäóëÿòîðà ñðï (PI_b3), + sin_func[i] = (int)(temp*32767); /// è êàëèáðîâêà ýòèõ çíà÷åíèé äëÿ ÀÖÏ. + if (sin_func[i] < 0) + sin_func[i] += 65536; + } + + //e. calculation of filter coefficients for PLC +// 250 Hz 10 KHz + init_BandPass( 1.0/(float)Device_blk.Str.PI_b3, 10.0/(float)(DEVICE_SAMPLE_RATE_HZ), PLC); //ïîëîñîôîé ôèëüòð äëÿ âûäåëåíèÿ ÷àñòîòû êîëåáàíèÿ ìîäóëÿòîðà + //è îïðåäåëåíèå êîýôèöèåíòîâ(aPLC[0-2] è bPLC[0-2]) + //(äðåáåçäåíèå ñðï äëÿ îïðåäåëåíèÿ ãðåòü èëè îõëîæäàòü îñíîâíîé ýëåìåíò óïðàâëåíèÿ.) + + Device_blk.Str.WP_scl <<= 1; //e. during fist 10 seconds after start we state Device_blk.Str.WP_scl = 2*Device_blk.Str.WP_scl + // ïåðâûå 10 ñåêóíä ðàáîòàòü ñ êîýôèöèåíòîì ïåðåäà÷è * 2 + + MaxDelayPLC = Device_blk.Str.PI_b3>>1; //e. max expected delay for phase detector output +} // init_PLC + +/****************************************************************************** +** Function name: PLC_MeanderDelay +** +** Descriptions: Outgoing of the delayed meander signal for the PLC regulator +** +** parameters: Input value +** Returned value: Delayed value +** +******************************************************************************/ +int PLC_MeanderDelay(int flag) +{ + static int poz_counter = 0, neg_counter = 0, flg_delay; + + if (Device_blk.Str.WP_ref == 0) + { + return (flag); + } + + //e. check whether delay exceeds the greatest possible value //r. ïðîâåðêà íå ïðåâîñõîäèò ëè çàäåðæêà ìàêñèìàëüíî âîçìîæíóþ + if (Device_blk.Str.WP_ref > MaxDelayPLC) { Device_blk.Str.WP_ref = MaxDelayPLC; } + + if (flag) //e. outgoing poz_sin_flag flag, which delayed by the WP_ref //r. ôîðìèðîâàíèå çàäåðæàííîãî íà âåëè÷èíó WP_ref ôëàãà poz_sin_flag + { + neg_counter = 0; + poz_counter++; + } + else + { + poz_counter = 0; + neg_counter++; + } + if (poz_counter == Device_blk.Str.WP_ref) { flg_delay = 0; } + if (neg_counter == Device_blk.Str.WP_ref) { flg_delay = 1; } + return (flg_delay); +} +/****************************************************************************** +** Function name: clc_PLC +** +** Descriptions: Procedure of initial processing for the CPLC regulator +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void clc_PLC(void) +{ + static int is_zeroing = 0; + static int zero_delay = 0; +// static int WP_DelaySin_Array[21] = {0}; +// int phase_Digital; + int poz_sin_flag; + int poz_sin_flag_delayed; + + + static int plc_reset32; + static enum + { //r. ñîñòîÿíèå ëèíåéíîãî ïåðåõîäà ïðè îáíóëåíèè ÑÐÏ + FINISHED, //r. ëèíåéíûé ïåðåõîä çàâåðøåí + TRANS_HEATING, //r. ïåðåõîä âûïîëíÿåòñÿ ïðè íàãðåâàíèè + TRANS_COOLING //r. ïåðåõîä âûïîëíÿåòñÿ ïðè îõëàæäåíèè + } plc_transiton = FINISHED; + +// int i; + + if (Output.Str.WP_sin >= 32768) + { + poz_sin_flag = 0; + } + else + { + poz_sin_flag = 1; + } + + //r. ïîëîñîâîé ôèëüòð äëÿ êîíòóðà ÑÐÏ + WP_Phase_Det = PLC_PhaseDetFilt(/*Output.Str.WP_sin*/Input.StrIn.WP_sel); + + + if (WP_Phase_Det >0) + { //r. WP_sel>0 + phase_Digital = 1; + } + else + { + phase_Digital = -1; + } + // from this WP_Phase_Det - modulated signal like LIM_DIG + + poz_sin_flag_delayed = PLC_MeanderDelay(poz_sin_flag); + + if(poz_sin_flag_delayed) + { + WP_Phase_Det = -WP_Phase_Det; + phase_Digital = -phase_Digital; + } + // from this WP_Phase_Det - demodulated signal like LIDEM_DIG + + if (!is_zeroing) //r. Íå ïîðà âûïîëíÿòü îáíóëåíèå + { //r. íåò îáíóëåíèÿ + if ((WP_reg32 > (Device_blk.Str.WP_rup << PLC_SHIFT)) && IsHeating) //r. ïðîèñõîäèò íàãðåâàíèå + { + is_zeroing = 1; + //r. íàïðÿæåíèå ñáðîñà ïðè íàãðåâàíèè + WP_reset_heating = CPL_reset_calc(Device_blk.Str.WP_reset, Device_blk.Str.K_WP_rst_heating, Temp_Aver, Device_blk.Str.TemperNormal); + plc_transiton = TRANS_HEATING; + plc_reset32 = WP_reset_heating << PLC_SHIFT;; + + Device_blk.Str.HF_scl = Device_blk.Str.HF_scl_2; //r. èçìåíÿåì êîýôôèöèåíò ïåðåäà÷è êîíòóðà ÃÂ× îò íîìèíàëüíîãî íà âðåìÿ îáíóëåíèÿ + } + else if ((WP_reg32 < (Device_blk.Str.WP_rdw << PLC_SHIFT)) && !IsHeating) //r. îõëàæäåíèå + { + is_zeroing = 1; + //r. íàïðÿæåíèå ñáðîñà ïðè îõëàæäåíèè + WP_reset_cooling = CPL_reset_calc(Device_blk.Str.WP_reset2, Device_blk.Str.K_WP_rst_cooling, Temp_Aver, Device_blk.Str.TemperNormal); + + plc_transiton = TRANS_COOLING; + plc_reset32 = WP_reset_cooling << PLC_SHIFT; + + Device_blk.Str.HF_scl = Device_blk.Str.HF_scl_2; //r. èçìåíÿåì êîýôôèöèåíò ïåðåäà÷è êîíòóðà ÃÂ× îò íîìèíàëüíîãî íà âðåìÿ îáíóëåíèÿ + } + else //r. ïîðîãè íå ïðåâûøåíû, îáû÷íàÿ ðàáîòà êîíòóðà + WP_reg32 = L_mac(WP_reg32, phase_Digital, Device_blk.Str.WP_scl ); // WP_reg32 += phase_Digital * Device_blk.Str.WP_scl; + + } + else //r. ôëàã óñòàíîâëåí (1) - ðåæèì îáíóëåíèÿ + { + + if (plc_transiton != FINISHED) + { + if (plc_transiton == TRANS_HEATING) + { + + WP_reg32 = L_sub(WP_reg32, Device_blk.Str.WP_transition_step); // WP_reg32 -= Device_blk.Str.WP_transition_step; + if (WP_reg32 < plc_reset32) + { + zero_delay = 0; + plc_transiton = FINISHED; //r.false; + WP_reg32 = plc_reset32; + } + } + else // plc_transiton == TRANS_COOLING + { + WP_reg32 = L_add(WP_reg32, Device_blk.Str.WP_transition_step); // WP_reg32 += Device_blk.Str.WP_transition_step; + if (WP_reg32 > plc_reset32) + { + zero_delay = 0; + plc_transiton = FINISHED; //r.false; + WP_reg32 = plc_reset32; + } + } + } + else + + if (zero_delay < Device_blk.Str.WP_mdy) + { + zero_delay++; + } + else //e. resetting was completed //r. îáíóëåíèå çàêîí÷èëîñü + { + is_zeroing = 0; + //e. save the temperature for further comparison //r. çàïîìèíàåì òåìïåðàòóðó äëÿ äàëüíåéøåãî ñðàâíåíèÿ + // TempOfReset = Temp_Aver; //r.x. Temp5_Aver; //r. Tmp_Out[TSENS_NUMB]; // T4; + //r.x Zero_Numb_dbg++; // òàê ìîæíî ïîäñ÷èòûâàòü ÷èñëî îáíóëåíèé + + // DithFreqRangeCalc(); //e. calculation of range of the division factor for the dither drive frequency, depending on current temperature //r. ðàñ÷åò ãðàíèö êîýôôèöèåíòà äåëåíèÿ äëÿ ÷àñòîòû âèáðîïðèâîäà, çàâèñÿùèõ îò òåêóùåé òåìïåðàòóðû + } + } + + Saturation(WP_reg32, WP_REG32MAX_SATURATION, WP_REG32MIN_NEW_SATURATION); //e. the minimum corresponds to a small negative number, appropriate to PLC_RESET_THRESHOLD //r. ìèíèìóì ñîîòâåòñòâóåò íåáîëüøîìó îòðèöàòåëüíîìó ÷èñëó, ñîîòâ-ìó PLC_RESET_THRESHOLD + + + if ( loop_is_closed(WP_REG_ON) ) //e. the regulator loop is closed //r. êîíòóð çàìêíóò + { + Output.Str.WP_reg = (int)(WP_reg32 >> PLC_SHIFT); //e. we use as controlling - voltages of the integrator //r. èñïîëüçóåì êàê óïðàâëÿþùåå - íàïðÿæåíèÿ èíòåãðàòîðà + + } + else //e. the regulator loop is open //r. êîíòóð ðàçîìêíóò + { + WP_reg32 = Output.Str.WP_reg << PLC_SHIFT; //e. set the previous value of the WP_reg //r. ïðèñâàèâàåì ïðåäûäóùåå çíà÷åíèå WP_reg + + } + + //e. integartion of output of the PD of the CPLC regulator for the technological output on the Rate command //r. èíòåãðèðîâàíèå âûõîäà ÔÄ êîíòóðà ÑÐÏ äëÿ òåõíîëîãè÷åñêîãî âûâîäà ïî êîìàíäå Rate + + Output.Str.WP_pll = WP_PhaseDetectorRate( WP_Phase_Det, time_1_Sec); + +} // clc_PLC + +/****************************************************************************** +** Function name: Signal_2_Oscill +** +** Descriptions: Procedure of analog worm output +** +** parameters: Type of output +** Returned value: code to DAC +** +******************************************************************************/ +int Signal_2_Oscill() //e. the signal for the control by scope on DAC output (was DS) //r. ñèãíàë äëÿ êîíòðîëÿ îñöèëëîãðàôîì íà âûõîäå ÖÀÏ (áûâøèé ÄÓÏ) +{ + // Scope_Mode var not used now, reserved for future applications + return (-WP_Phase_Det << 2); +} // Signal_2_Oscill + +/****************************************************************************** +** Function name: clc_WP_sin +** +** Descriptions: Procedure of scan signal generating +** +** parameters: None +** Returned value: Current code for scan signal DAC of PLC +** +******************************************************************************/ +int clc_WP_sin(void) +{ + static int index = 0; + index++; + + if (index >= 40/*Device_blk.Str.PI_b3*/) + index = 0; +/* if (index > 20) + LPC_GPIO0->FIOSET = (1<<26); + else + LPC_GPIO0->FIOCLR = (1<<26); */ + DAC_Output(sin_func[index]); //output to DAC + + return (sin_func[index]); +} // clc_WP_sin + +/****************************************************************************** +** Function name: WP_PhaseDetectorRate +** +** Descriptions: Integartion of output of the PD of the CPLC regulator + for the technological output on the Rate command +** +** Parameters: Current PD magnitude, period of integration +** Returned value: Integrated magnitude of PD +** +******************************************************************************/ +int WP_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime) +{ + + static int SampleAndHoldOut = 0; + static int WP_PhasDet_integr = 0;//, WP_PhasDetector = 0; + + if (IntegrateTime == DEVICE_SAMPLE_RATE_uks) + { + SampleAndHoldOut = (int)(WP_PhasDet_integr >> PLC_PHASE_DET_SHIFT); + WP_PhasDet_integr = 0; + } + else + { + WP_PhasDet_integr += PhaseDetInput; + } + return (SampleAndHoldOut); +} // WP_PhaseDetectorRate + +