Diff: PLC_reg.c

Revision:

0:8ad47e2b6f00

Child:

1:f2adcae3d304

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PLC_reg.c	Sat Jan 30 13:00:39 2016 +0000
@@ -0,0 +1,354 @@
+/****************************************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
+
+
+