123
Fork of LG by
Diff: PLC_reg.c
- Revision:
- 21:bc8c1cec3da6
- Parent:
- 1:f2adcae3d304
- Child:
- 112:4a96133a1311
--- a/PLC_reg.c Tue Feb 02 17:14:25 2016 +0000 +++ b/PLC_reg.c Wed Feb 03 07:19:30 2016 +0000 @@ -9,343 +9,312 @@ ** Created by: Electrooptica Incorp. ** Created date: 2011-09-26 ** Version: V1.00 -** Descriptions: +** Descriptions: ** -**-------------------------------------------------------------------------------------------------------- +**-------------------------------------------------------------------------------------------------------- *********************************************************************************************************/ #include "Global.h" - -#define CONFIG_HFO_REG //r. �������� ����������� �������� ������� ��� �� ������������ �� ����� ��������� +#define CONFIG_HFO_REG //r. изменяем коэффициент передачи контура ГВЧ от номинального на время обнуления #define WP_TRANSITION_ENA // - - //e.--- constants for the CPLC regulator ------------------------------------------------------- //r.--- ��������� ��� ������� ��� ------------------------------------------------------- + +//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_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 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 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 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); +int WP_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime); /****************************************************************************** ** Function name: init_PLC ** -** Descriptions: Initialization procedure for PLC regulator +** Descriptions: Initialization procedure for PLC regulator ** -** Parameters: None -** Returned value: None -** +** 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 + 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 (частота дребездения) - // ���������� �� ��� << 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; + 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 + + //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]) - //(����������� ��� ��� ����������� ����� ��� ��������� �������� ������� ����������.) + 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 + 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 +** Descriptions: Outgoing of the delayed meander signal for the PLC regulator ** -** parameters: Input value -** Returned value: Delayed value -** +** parameters: Input value +** Returned value: Delayed value +** ******************************************************************************/ int PLC_MeanderDelay(int flag) { - static int poz_counter = 0, neg_counter = 0, flg_delay; + 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 (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); -} + 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 +** Descriptions: Procedure of initial processing for the CPLC regulator ** -** parameters: None -** Returned value: None -** +** parameters: None +** Returned value: None +** ******************************************************************************/ void clc_PLC(void) { - static int is_zeroing = 0; - static int zero_delay = 0; + 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; + 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; + 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; - } + 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); + //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 (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;; + 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 - 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); + 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;; - plc_transiton = TRANS_COOLING; - plc_reset32 = WP_reset_cooling << 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); - 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; + 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) - ����� ��������� - { + } else { //r. флаг установлен (1) - режим обнуления + + if (plc_transiton != FINISHED) { + if (plc_transiton == TRANS_HEATING) { - 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 + 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++; // ��� ����� ������������ ����� ��������� + 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. ������ ������ ������������ ������� ��� ������� ������������, ��������� �� ������� ����������� - } - } + // 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 + 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. ���������� ��� ����������� - ���������� ����������� + + 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 - } - 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 - //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); - 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 +** Descriptions: Procedure of analog worm output +** +** parameters: Type of output +** Returned value: code to DAC ** -** 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. ������ ��� �������� ������������� �� ������ ��� (������ ���) +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); + // 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 +** Descriptions: Procedure of scan signal generating ** -** parameters: None +** parameters: None ** Returned value: Current code for scan signal DAC of PLC -** +** ******************************************************************************/ int clc_WP_sin(void) { - static int index = 0; - index++; + 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]); + 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 +** 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 -** +** Parameters: Current PD magnitude, period of integration +** Returned value: Integrated magnitude of PD +** ******************************************************************************/ -int WP_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime) +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); + 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 - - -