123
Fork of LG by
Diff: CntrlGLD.c
- 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 + +