fork

Dependencies:   mbed

Fork of LG by igor Apu

Revision:
0:8ad47e2b6f00
Child:
1:f2adcae3d304
diff -r 000000000000 -r 8ad47e2b6f00 CntrlGLD.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CntrlGLD.c	Sat Jan 30 13:00:39 2016 +0000
@@ -0,0 +1,262 @@
+
+
+#include "Global.h"
+#define COMMAND_DEBUG  
+
+uint32_t 	SRgR;      		     //r. ��������� ������� ������ ����� ADSP
+
+uint32_t 	RgConA;            //r. �������� ������� ���������� ����������
+uint32_t 	RgConB;            //r. �������������� ������� ����������
+//uint32_t	Copy_e5_RgR;     //r. ����� �������� ������ ����� E5
+//uint32_t	Copy_e5_RgRA;    //r. ����� ��������������� �������� ������ E5
+
+uint32_t	Valid_Data;   //r. ����-������� ����������� ������, ������������� ������ �� ������� �� �������
+
+TDEVICE_BLK Device_blk;   //e. union for device constants
+uint32_t	ser_num;	 //e. device serial number
+
+int 	Device_Mode = DM_INT_10KHZ_LATCH;  //r. ����� ������ ������� (�����.�������, �������� ������ ��� ������� �������)
+
+void Sts_Pulse(void);
+void Sts_Pause(void);
+void Sts_Polling(void);
+
+ //r. ������� ��������� ��������� ������� �������
+void 	(* Tab_Gld_Start[3])(void) = {Sts_Pulse, Sts_Pause, Sts_Polling};
+ //r. ������� ��������� ��������� ������� ������
+void	(* Tab_Gld_Pulse[1])(void) = {Sts_Pulse};
+
+	//e. stack of states of the procedure of starting //r. ���� ���������� ��������� ��������� �������
+uint32_t		Stk_Gld_Start[3] = {LIGHT_UP_PULSE_WDTH, LIGHT_UP_PAUSE, LIGHT_UP_POLLING};
+
+uint32_t		gyro_Sts = 0;       //r. ������� ��������� �������
+uint32_t		start_Rq = 1;       //r. ������ �� ������ �������
+uint32_t		start_Go = 0;       //r. ��������� ������� �������
+uint32_t		stop_Rq = 0;        //r. ������ �� ������� �������
+uint32_t		pulse_Rq = 0;       //r. ������ �� ������ ������
+uint32_t		pulse_Go = 0;       //r. ������� ������� �������
+
+uint32_t     	state_index;    //r. ������ ��������� ��������� (������)
+uint32_t     	sts_work;       //r. 1 - ������� ��������� - � ������� ����, 0 - �������������
+uint32_t     	sts_time;       //r. ������� ������� �������� ���������
+uint32_t     	attempt_Str;    //r. ������� ������� ������� �������
+
+ 
+uint32_t 		BIT_number;
+uint32_t		Is_BIT = 0;
+
+void GLD_Stop(void)	//e.---------- procedure of GLD stopping -------------------------------------- //r.---------- ��������� �������� ��� --------------------------------------
+{
+	if (!stop_Rq)	 //r. ��������� ������ �� ���������� �������
+	{
+		return;		//r. ���� ���, �������
+	}
+	
+     //r. �������� �����
+	start_Rq = 0;       //r. ������� ���������,
+	start_Go = 0;       //r. ���������� �������� ������� �������,
+	stop_Rq  = 0;       //r. ������� ��������
+	open_all_loops();   //r. ��������� ��� ������� �������������
+
+	gyro_Sts = 0;       //r. ����� �������� ��������� ���������� ���
+
+	 //r. ��������� �������� ����� �������� ���������� �����������
+#if !defined COMMAND_DEBUG
+	disable_DACs();
+#endif
+} // GLD_Stop
+
+void GLD_Status(void)     //r.---------- ���������� �������� ���������� ��� --------------------------
+{
+  static uint32_t Device_Mode_Old = DM_INT_10KHZ_LATCH/*DM_EXT_LATCH_DELTA_SF_PULSE*/;
+
+	if ( loop_is_closed(GLD_ON) == 0 )
+	{
+		open_all_loops();
+		GLD_Stop();
+	}
+	if (Device_Mode_Old != Device_Mode)		   //devi�e mode has been changed
+	 {
+	  if (SwitchMode())						   //e. mode is changed successful
+	  {
+	   Device_Mode_Old = Device_Mode;		   //e. save current mode name
+	   if (Device_Mode_Old == DM_INT_10KHZ_LATCH)
+	   trm_ena = 1;							   //e. enable answer
+	   }	
+	 }	 
+} // GLD_Status
+
+void Sts_Pause(void)      //r.------- ��������� ��������� ����� --------------------------------------
+{
+	if (sts_work)      //r. ��������� � �������� ����?
+    {
+		sts_time--;      //r. ��������� �������� ������� ���������
+	    if (sts_time > 0)
+	    {
+	    	return;  //r. ���������� ������, ���� ������� �����������
+	    }
+	     //r. ����� ��������� ���������
+	    sts_work = 0;        //r. �������� ���� ����������
+	    state_index++;      //r. ����������� ��������� �� ��������� ���������
+	}
+    else
+    {
+		sts_time = Stk_Gld_Start[state_index];	 //r. ����� ������� ���������      
+		sts_work = 1;         //r. ���������� ���� �������� ����
+	}       
+} // Sts_Pause
+
+void Sts_Pulse()      //r. ------- ��������� ��������� �������� ������� --------------------------
+{
+    if (sts_work)     //r. ��������� � �������� ����?
+    {
+		sts_time--;       //r. ��������� �������� ������� ���������
+	    if (sts_time > 0)
+	    {
+	    	return; //r. ���������� ������, ���� ������� �����������
+	    }
+        //r. ����� ��������� ���������
+	    sts_work = 0;       //r. �������� ���� ����������
+	    state_index++;     //r. ����������� ��������� �� ��������� ���������
+	    
+        Set_LightUp;		 //r. ����� ������ ������� ������        
+
+	}
+    else
+    {
+		sts_time = Stk_Gld_Start[state_index];	//r. ����� ������� ���������      
+		sts_work = 1;        //r. ���������� ���� �������� ����
+
+        Reset_LightUp;		 //r. ������������ ������ ������� ������
+
+	}
+} // Sts_Pulse
+
+void Sts_Polling(void)        //r.========== ��������� ��������� ��������� � �������� ������� � �������
+{
+	static int32_t Sum_F_ras = 0;      //r. ������������ ����� ����������� ��� ��������� ������
+
+    if (sts_work)      //r. ��������� � �������� ����?
+    {
+			if 	(Dif_Curr_Vib >0)
+        		Sum_F_ras = L_add(Sum_F_ras, Dif_Curr_Vib);
+			else
+				Sum_F_ras = L_sub(Sum_F_ras, Dif_Curr_Vib);
+
+//r. ���, ���������� ���������� ����� �����������
+		sts_time--;      //r. ��������� �������� ������� ���������
+	    if (sts_time > 0)
+	    {
+	    	return; //r. ���������� ������, ���� ������� �����������
+	    }
+	        //r. ����� ��������� ���������
+	    sts_work = 0;       //r. �������� ���� ����������   
+	    state_index++;     //r. ����������� ��������� �� ��������� ���������
+        //r. ��������� ������� ���������
+        if ( Sum_F_ras >= F_RAS_MIN) //e. //r. F_RAS_MIN = 10000 ??? ��� ��� �� �������???
+        {
+	        gyro_Sts |= LASER_ON;      //r. ����� ����� ���������, �������� � �������� ���������
+    	    return;                    //r. ������� � ��������� �������
+        }
+         //r. ���� ��� ������, ��������� ���
+        attempt_Str--;           //r. ��������� �������� ������� ������� ������
+        if (attempt_Str <= 0)
+        {
+        	return;       //r. ���� ������� ���������, ��������� �������
+        }
+        //r. ����� ���������� ��������� ��������� ��
+        state_index = 0;       //r. ������ �������� ������� ��� ��������� �������    
+	}
+    else
+    {
+		sts_time = Stk_Gld_Start[state_index];	//r. ����� ������� ���������      
+		sts_work = 1;         //r. ���������� ���� �������� ����
+        Sum_F_ras = 0;               //r. ��������� ������������ ����� �����������
+	}
+	
+} // Sts_Polling
+
+
+//r.---------- ������� ������� ������� -------------------------------------
+void GLD_Start(void)
+{
+	if (!start_Rq)	//r. ��������� ��������� ������� �� ������ �������
+	{
+		return;		 //r. ������� ���, �������
+	}
+
+	if (!start_Go)	 //r. ������� ������� ��� �� ��� �������?
+	{	
+		 //r. ��� ������ �������
+        attempt_Str = N_START_MAX;   //r. ���������� ������������ ����� ������� �������
+		//r. ���������� ������� �����������
+		//open_loop(VB_FREQ_ON | VB_TAU_ON | WP_REG_ON | HF_REG_ON); // 0xff95
+		//close_all_loops();
+	
+        sts_work = 0;       //r. �������� ���� �������� ���� ���������
+        state_index= 0;      //r. ���������� ��������� ��������� � ��������� ���������
+        start_Go = 1;       //r. ������� ������ �������
+	}
+
+	( *Tab_Gld_Start[state_index] )();		 //r. ����� ��������� ��������� �� ������ � �������
+    	
+    if (sts_work)      	 //r. ������� ��������� ��������?
+    {
+    	return;			//r. ���������� ��������
+    }
+   //r. ��������� ����������� ��������� ��������� ���������
+    if ( state_index >= 3 )
+    {
+	//r. ��������� ��������
+	     //r. �������� �����
+	    start_Rq = 0;       //r. �������
+	    start_Go = 0;     //r. � ���������� ��������
+#if !defined COMMAND_DEBUG
+	    Out_G_photo(Device_blk.Str.Gain_Ph_A, Device_blk.Str.Gain_Ph_B); 		 //r. �������� ���������������� ����-�� �������� ��������������
+#endif
+	  //  close_all_loops();
+
+	}
+        
+} // GLD_Start
+
+void GLD_Pulse(void)      //r.---------- ������� ������� ������ --------------------------------------
+{
+	if (!pulse_Rq)     //r. ��������� ��������� ������� �� ������ ������
+    {
+    	return; 		//r. ������� ���, �������
+    }
+                 
+    if (!pulse_Go)      //r. ������� ������� ��� �� ��� �������?
+    {
+        sts_work = 0;      //r. �������� ���� �������� ���� ���������
+        state_index = 0;     //r. ���������� ��������� ��������� � ��������� ���������
+        pulse_Go = 1;       //r. ������� ������ �������
+    }
+    
+    ( *Tab_Gld_Pulse[state_index] )();		 //r. ����� ��������� ��������� �� ������ � �������
+	
+	if (sts_work)      	 //r. ������� ��������� ��������?
+    {
+    	return;			//r. ���������� ��������
+    }
+    
+	if ( state_index >= 1 )
+    {
+	    //r. �������� �����
+	    pulse_Rq = 0;       //r. �������
+	    pulse_Go = 0;      //r. � ���������� ��������
+    }
+        
+} // GLD_Pulse
+
+void contrl_GLD(void)     //r.========== ��������� ���������� �������� ===============================
+{
+	GLD_Start();   //r. ��������� ������, ���� ���� ������
+	GLD_Status();  //r. �������� ��������� �������
+	GLD_Stop();    //r. ���������� ������, ���� ����������
+	GLD_Pulse();   //r. ��������� �������� ������� ��� ������� �������
+	GLD_Output();  //r. ������ ������ ������ �� ���������
+} // contrl_GLD
+
+