fork

Dependencies:   mbed

Fork of LG by igor Apu

CntrlGLD.c

Committer:
Kovalev_D
Date:
2016-02-01
Revision:
7:078abbed4565
Parent:
1:f2adcae3d304
Child:
9:30702450ad00

File content as of revision 7:078abbed4565:

#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