Dmitry Kovalev / Mbed 2 deprecated LGfiltr

forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

PLC_reg.c@0:8ad47e2b6f00, 2016-01-30 (annotated)

Committer:
igor_v
Date:
Sat Jan 30 13:00:39 2016 +0000
Revision:
0:8ad47e2b6f00
Child:
1:f2adcae3d304
2016_01_30;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
igor_v 0:8ad47e2b6f00 1 /****************************************Copyright (c)****************************************************
igor_v 0:8ad47e2b6f00 2 **--------------File Info---------------------------------------------------------------------------------
igor_v 0:8ad47e2b6f00 3 ** File name: PLC_reg.c
igor_v 0:8ad47e2b6f00 4 ** Last modified Date: 2011-09-26
igor_v 0:8ad47e2b6f00 5 ** Last Version: V1.00
igor_v 0:8ad47e2b6f00 6 ** Descriptions: Routines for system of perimeter regulating unit
igor_v 0:8ad47e2b6f00 7 **
igor_v 0:8ad47e2b6f00 8 **--------------------------------------------------------------------------------------------------------
igor_v 0:8ad47e2b6f00 9 ** Created by: Electrooptica Incorp.
igor_v 0:8ad47e2b6f00 10 ** Created date: 2011-09-26
igor_v 0:8ad47e2b6f00 11 ** Version: V1.00
igor_v 0:8ad47e2b6f00 12 ** Descriptions:
igor_v 0:8ad47e2b6f00 13 **
igor_v 0:8ad47e2b6f00 14 **--------------------------------------------------------------------------------------------------------
igor_v 0:8ad47e2b6f00 15 *********************************************************************************************************/
igor_v 0:8ad47e2b6f00 16 #include "mathDSP.h"
igor_v 0:8ad47e2b6f00 17 #include "CyclesSync.h"
igor_v 0:8ad47e2b6f00 18 #include "ThermoCalc.h"
igor_v 0:8ad47e2b6f00 19 #include "CntrlGLD.h"
igor_v 0:8ad47e2b6f00 20 #include "InputOutput.h"
igor_v 0:8ad47e2b6f00 21 #include <math.h>
igor_v 0:8ad47e2b6f00 22
igor_v 0:8ad47e2b6f00 23 #define CONFIG_HFO_REG //r. èçìåíÿåì êîýôôèöèåíò ïåðåäà÷è êîíòóðà ÃÂ× îò íîìèíàëüíîãî íà âðåìÿ îáíóëåíèÿ
igor_v 0:8ad47e2b6f00 24 #define WP_TRANSITION_ENA //
igor_v 0:8ad47e2b6f00 25
igor_v 0:8ad47e2b6f00 26 //e.--- constants for the CPLC regulator ------------------------------------------------------- //r.--- êîíñòàíòû äëÿ êîíòóðà ÑÐÏ -------------------------------------------------------
igor_v 0:8ad47e2b6f00 27
igor_v 0:8ad47e2b6f00 28 #define PLC_SHIFT (6)
igor_v 0:8ad47e2b6f00 29 #define PLC_PHASE_DET_SHIFT (18) //e. 18 - for analog output //r. 18 - äëÿ àíàëîãîâîãî
igor_v 0:8ad47e2b6f00 30
igor_v 0:8ad47e2b6f00 31 #define PLC_RESET_THRESHOLD (-3276) //e. correspond to the voltage +1.2 Volts //r. ñîîòâåòñòâóåò íàïðÿæåíèþ +1.2 âîëüòà
igor_v 0:8ad47e2b6f00 32 #define WP_REG32MAX_SATURATION (32767 << PLC_SHIFT)
igor_v 0:8ad47e2b6f00 33 #define WP_REG32MIN_NEW_SATURATION (PLC_RESET_THRESHOLD << PLC_SHIFT)
igor_v 0:8ad47e2b6f00 34 #define WP_TMP_THRESHOLD (7) //e. temperature threshold, defining heats up or cool down the device //r. òåìïåðàòóðíûé ïîðîã, îïðåäåëÿþùèé íàãðåâàåòñÿ èëè îõëàæäàåòñÿ ïðèáîð
igor_v 0:8ad47e2b6f00 35
igor_v 0:8ad47e2b6f00 36
igor_v 0:8ad47e2b6f00 37 #define debugPLC
igor_v 0:8ad47e2b6f00 38
igor_v 0:8ad47e2b6f00 39 int WP_reg32;
igor_v 0:8ad47e2b6f00 40 int WP_Phase_Det; //e. output of the phase detector of the CPLC (in a digital kind)//r. âûõîä ôàçîâîãî äåòåêòîðà ÑÐÏ (â öèôðîâîì âèäå)
igor_v 0:8ad47e2b6f00 41 int WP_reset_heating; //e. voltage of reset at heating //r. íàïðÿæåíèå ñáðîñà ïðè íàãðåâàíèè
igor_v 0:8ad47e2b6f00 42 int WP_reset_cooling; //e. voltage of reset at cooling //r. íàïðÿæåíèå ñáðîñà ïðè îõëàæäåíèè
igor_v 0:8ad47e2b6f00 43 int MaxDelayPLC;
igor_v 0:8ad47e2b6f00 44 int sin_func[100];
igor_v 0:8ad47e2b6f00 45
igor_v 0:8ad47e2b6f00 46 int phase_Digital;
igor_v 0:8ad47e2b6f00 47
igor_v 0:8ad47e2b6f00 48 int WP_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime);
igor_v 0:8ad47e2b6f00 49
igor_v 0:8ad47e2b6f00 50 /******************************************************************************
igor_v 0:8ad47e2b6f00 51 ** Function name: init_PLC
igor_v 0:8ad47e2b6f00 52 **
igor_v 0:8ad47e2b6f00 53 ** Descriptions: Initialization procedure for PLC regulator
igor_v 0:8ad47e2b6f00 54 **
igor_v 0:8ad47e2b6f00 55 ** Parameters: None
igor_v 0:8ad47e2b6f00 56 ** Returned value: None
igor_v 0:8ad47e2b6f00 57 **
igor_v 0:8ad47e2b6f00 58 ******************************************************************************/
igor_v 0:8ad47e2b6f00 59 void init_PLC(void)
igor_v 0:8ad47e2b6f00 60 {
igor_v 0:8ad47e2b6f00 61 int i;
igor_v 0:8ad47e2b6f00 62 //( 1,2 âîëüòà)
igor_v 0:8ad47e2b6f00 63 if (Device_blk.Str.WP_reset < PLC_RESET_THRESHOLD) //e. íàïðÿæåíèå ïîñëå ñáðîñà íà íàãðåâàòåëå íå äîëæíî ïðåâûøàòü 1,2 âîëüòà.
igor_v 0:8ad47e2b6f00 64 //(èñõîäíîå çíà÷åíèå ðåãóëÿòîðà ÑÐÏ (ïîñëå ñáðîñà)) < (-3276).
igor_v 0:8ad47e2b6f00 65 {
igor_v 0:8ad47e2b6f00 66 Device_blk.Str.WP_reset = PLC_RESET_THRESHOLD + 1;//(-3275)
igor_v 0:8ad47e2b6f00 67 }
igor_v 0:8ad47e2b6f00 68 //íàïðÿæåíèå íà ÑÐÏ = (ìèí. çíà÷åíèå íà íàãðåâàòåëå + ìàõ. çíà÷åíèå íà íàãðåâàòåëå)/2
igor_v 0:8ad47e2b6f00 69 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
igor_v 0:8ad47e2b6f00 70
igor_v 0:8ad47e2b6f00 71 // íàïðÿæåíèå íà ÑÐÏ << 6
igor_v 0:8ad47e2b6f00 72 WP_reg32 = Output.Str.WP_reg<<PLC_SHIFT;
igor_v 0:8ad47e2b6f00 73
igor_v 0:8ad47e2b6f00 74 if ((Device_blk.Str.PI_b3>100)||(Device_blk.Str.PI_b3<10)) //e. Åñëè òðåáóåìàÿ ÷àñòîòà ìîäóëÿòîðà ÑÐÏ áîëüøå 1kHz èëè ìåíüøå 100Hz
igor_v 0:8ad47e2b6f00 75 Device_blk.Str.PI_b3 = 40; //e. Óñòàíîâèòü ÷àñòîòó â 250Hz (÷àñòîòà äðåáåçäåíèÿ)
igor_v 0:8ad47e2b6f00 76
igor_v 0:8ad47e2b6f00 77 for (i = 0; i<Device_blk.Str.PI_b3; i++) //e. Ñêàíèðîâàíèå ÑÐÏ ñèãíàëà
igor_v 0:8ad47e2b6f00 78 {
igor_v 0:8ad47e2b6f00 79 float temp = sin((float)i*2.0*PI/(float)Device_blk.Str.PI_b3); /// âû÷èñëåíèå çíà÷åíèé ñèíóñà
igor_v 0:8ad47e2b6f00 80 /// äëÿ ÷àñòîòû ìîäóëÿòîðà ñðï (PI_b3),
igor_v 0:8ad47e2b6f00 81 sin_func[i] = (int)(temp*32767); /// è êàëèáðîâêà ýòèõ çíà÷åíèé äëÿ ÀÖÏ.
igor_v 0:8ad47e2b6f00 82 if (sin_func[i] < 0)
igor_v 0:8ad47e2b6f00 83 sin_func[i] += 65536;
igor_v 0:8ad47e2b6f00 84 }
igor_v 0:8ad47e2b6f00 85
igor_v 0:8ad47e2b6f00 86 //e. calculation of filter coefficients for PLC
igor_v 0:8ad47e2b6f00 87 // 250 Hz 10 KHz
igor_v 0:8ad47e2b6f00 88 init_BandPass( 1.0/(float)Device_blk.Str.PI_b3, 10.0/(float)(DEVICE_SAMPLE_RATE_HZ), PLC); //ïîëîñîôîé ôèëüòð äëÿ âûäåëåíèÿ ÷àñòîòû êîëåáàíèÿ ìîäóëÿòîðà
igor_v 0:8ad47e2b6f00 89 //è îïðåäåëåíèå êîýôèöèåíòîâ(aPLC[0-2] è bPLC[0-2])
igor_v 0:8ad47e2b6f00 90 //(äðåáåçäåíèå ñðï äëÿ îïðåäåëåíèÿ ãðåòü èëè îõëîæäàòü îñíîâíîé ýëåìåíò óïðàâëåíèÿ.)
igor_v 0:8ad47e2b6f00 91
igor_v 0:8ad47e2b6f00 92 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
igor_v 0:8ad47e2b6f00 93 // ïåðâûå 10 ñåêóíä ðàáîòàòü ñ êîýôèöèåíòîì ïåðåäà÷è * 2
igor_v 0:8ad47e2b6f00 94
igor_v 0:8ad47e2b6f00 95 MaxDelayPLC = Device_blk.Str.PI_b3>>1; //e. max expected delay for phase detector output
igor_v 0:8ad47e2b6f00 96 } // init_PLC
igor_v 0:8ad47e2b6f00 97
igor_v 0:8ad47e2b6f00 98 /******************************************************************************
igor_v 0:8ad47e2b6f00 99 ** Function name: PLC_MeanderDelay
igor_v 0:8ad47e2b6f00 100 **
igor_v 0:8ad47e2b6f00 101 ** Descriptions: Outgoing of the delayed meander signal for the PLC regulator
igor_v 0:8ad47e2b6f00 102 **
igor_v 0:8ad47e2b6f00 103 ** parameters: Input value
igor_v 0:8ad47e2b6f00 104 ** Returned value: Delayed value
igor_v 0:8ad47e2b6f00 105 **
igor_v 0:8ad47e2b6f00 106 ******************************************************************************/
igor_v 0:8ad47e2b6f00 107 int PLC_MeanderDelay(int flag)
igor_v 0:8ad47e2b6f00 108 {
igor_v 0:8ad47e2b6f00 109 static int poz_counter = 0, neg_counter = 0, flg_delay;
igor_v 0:8ad47e2b6f00 110
igor_v 0:8ad47e2b6f00 111 if (Device_blk.Str.WP_ref == 0)
igor_v 0:8ad47e2b6f00 112 {
igor_v 0:8ad47e2b6f00 113 return (flag);
igor_v 0:8ad47e2b6f00 114 }
igor_v 0:8ad47e2b6f00 115
igor_v 0:8ad47e2b6f00 116 //e. check whether delay exceeds the greatest possible value //r. ïðîâåðêà íå ïðåâîñõîäèò ëè çàäåðæêà ìàêñèìàëüíî âîçìîæíóþ
igor_v 0:8ad47e2b6f00 117 if (Device_blk.Str.WP_ref > MaxDelayPLC) { Device_blk.Str.WP_ref = MaxDelayPLC; }
igor_v 0:8ad47e2b6f00 118
igor_v 0:8ad47e2b6f00 119 if (flag) //e. outgoing poz_sin_flag flag, which delayed by the WP_ref //r. ôîðìèðîâàíèå çàäåðæàííîãî íà âåëè÷èíó WP_ref ôëàãà poz_sin_flag
igor_v 0:8ad47e2b6f00 120 {
igor_v 0:8ad47e2b6f00 121 neg_counter = 0;
igor_v 0:8ad47e2b6f00 122 poz_counter++;
igor_v 0:8ad47e2b6f00 123 }
igor_v 0:8ad47e2b6f00 124 else
igor_v 0:8ad47e2b6f00 125 {
igor_v 0:8ad47e2b6f00 126 poz_counter = 0;
igor_v 0:8ad47e2b6f00 127 neg_counter++;
igor_v 0:8ad47e2b6f00 128 }
igor_v 0:8ad47e2b6f00 129 if (poz_counter == Device_blk.Str.WP_ref) { flg_delay = 0; }
igor_v 0:8ad47e2b6f00 130 if (neg_counter == Device_blk.Str.WP_ref) { flg_delay = 1; }
igor_v 0:8ad47e2b6f00 131 return (flg_delay);
igor_v 0:8ad47e2b6f00 132 }
igor_v 0:8ad47e2b6f00 133 /******************************************************************************
igor_v 0:8ad47e2b6f00 134 ** Function name: clc_PLC
igor_v 0:8ad47e2b6f00 135 **
igor_v 0:8ad47e2b6f00 136 ** Descriptions: Procedure of initial processing for the CPLC regulator
igor_v 0:8ad47e2b6f00 137 **
igor_v 0:8ad47e2b6f00 138 ** parameters: None
igor_v 0:8ad47e2b6f00 139 ** Returned value: None
igor_v 0:8ad47e2b6f00 140 **
igor_v 0:8ad47e2b6f00 141 ******************************************************************************/
igor_v 0:8ad47e2b6f00 142 void clc_PLC(void)
igor_v 0:8ad47e2b6f00 143 {
igor_v 0:8ad47e2b6f00 144 static int is_zeroing = 0;
igor_v 0:8ad47e2b6f00 145 static int zero_delay = 0;
igor_v 0:8ad47e2b6f00 146 // static int WP_DelaySin_Array[21] = {0};
igor_v 0:8ad47e2b6f00 147 // int phase_Digital;
igor_v 0:8ad47e2b6f00 148 int poz_sin_flag;
igor_v 0:8ad47e2b6f00 149 int poz_sin_flag_delayed;
igor_v 0:8ad47e2b6f00 150
igor_v 0:8ad47e2b6f00 151
igor_v 0:8ad47e2b6f00 152 static int plc_reset32;
igor_v 0:8ad47e2b6f00 153 static enum
igor_v 0:8ad47e2b6f00 154 { //r. ñîñòîÿíèå ëèíåéíîãî ïåðåõîäà ïðè îáíóëåíèè ÑÐÏ
igor_v 0:8ad47e2b6f00 155 FINISHED, //r. ëèíåéíûé ïåðåõîä çàâåðøåí
igor_v 0:8ad47e2b6f00 156 TRANS_HEATING, //r. ïåðåõîä âûïîëíÿåòñÿ ïðè íàãðåâàíèè
igor_v 0:8ad47e2b6f00 157 TRANS_COOLING //r. ïåðåõîä âûïîëíÿåòñÿ ïðè îõëàæäåíèè
igor_v 0:8ad47e2b6f00 158 } plc_transiton = FINISHED;
igor_v 0:8ad47e2b6f00 159
igor_v 0:8ad47e2b6f00 160 // int i;
igor_v 0:8ad47e2b6f00 161
igor_v 0:8ad47e2b6f00 162 if (Output.Str.WP_sin >= 32768)
igor_v 0:8ad47e2b6f00 163 {
igor_v 0:8ad47e2b6f00 164 poz_sin_flag = 0;
igor_v 0:8ad47e2b6f00 165 }
igor_v 0:8ad47e2b6f00 166 else
igor_v 0:8ad47e2b6f00 167 {
igor_v 0:8ad47e2b6f00 168 poz_sin_flag = 1;
igor_v 0:8ad47e2b6f00 169 }
igor_v 0:8ad47e2b6f00 170
igor_v 0:8ad47e2b6f00 171 //r. ïîëîñîâîé ôèëüòð äëÿ êîíòóðà ÑÐÏ
igor_v 0:8ad47e2b6f00 172 WP_Phase_Det = PLC_PhaseDetFilt(/*Output.Str.WP_sin*/Input.StrIn.WP_sel);
igor_v 0:8ad47e2b6f00 173
igor_v 0:8ad47e2b6f00 174
igor_v 0:8ad47e2b6f00 175 if (WP_Phase_Det >0)
igor_v 0:8ad47e2b6f00 176 { //r. WP_sel>0
igor_v 0:8ad47e2b6f00 177 phase_Digital = 1;
igor_v 0:8ad47e2b6f00 178 }
igor_v 0:8ad47e2b6f00 179 else
igor_v 0:8ad47e2b6f00 180 {
igor_v 0:8ad47e2b6f00 181 phase_Digital = -1;
igor_v 0:8ad47e2b6f00 182 }
igor_v 0:8ad47e2b6f00 183 // from this WP_Phase_Det - modulated signal like LIM_DIG
igor_v 0:8ad47e2b6f00 184
igor_v 0:8ad47e2b6f00 185 poz_sin_flag_delayed = PLC_MeanderDelay(poz_sin_flag);
igor_v 0:8ad47e2b6f00 186
igor_v 0:8ad47e2b6f00 187 if(poz_sin_flag_delayed)
igor_v 0:8ad47e2b6f00 188 {
igor_v 0:8ad47e2b6f00 189 WP_Phase_Det = -WP_Phase_Det;
igor_v 0:8ad47e2b6f00 190 phase_Digital = -phase_Digital;
igor_v 0:8ad47e2b6f00 191 }
igor_v 0:8ad47e2b6f00 192 // from this WP_Phase_Det - demodulated signal like LIDEM_DIG
igor_v 0:8ad47e2b6f00 193
igor_v 0:8ad47e2b6f00 194 if (!is_zeroing) //r. Íå ïîðà âûïîëíÿòü îáíóëåíèå
igor_v 0:8ad47e2b6f00 195 { //r. íåò îáíóëåíèÿ
igor_v 0:8ad47e2b6f00 196 if ((WP_reg32 > (Device_blk.Str.WP_rup << PLC_SHIFT)) && IsHeating) //r. ïðîèñõîäèò íàãðåâàíèå
igor_v 0:8ad47e2b6f00 197 {
igor_v 0:8ad47e2b6f00 198 is_zeroing = 1;
igor_v 0:8ad47e2b6f00 199 //r. íàïðÿæåíèå ñáðîñà ïðè íàãðåâàíèè
igor_v 0:8ad47e2b6f00 200 WP_reset_heating = CPL_reset_calc(Device_blk.Str.WP_reset, Device_blk.Str.K_WP_rst_heating, Temp_Aver, Device_blk.Str.TemperNormal);
igor_v 0:8ad47e2b6f00 201 plc_transiton = TRANS_HEATING;
igor_v 0:8ad47e2b6f00 202 plc_reset32 = WP_reset_heating << PLC_SHIFT;;
igor_v 0:8ad47e2b6f00 203
igor_v 0:8ad47e2b6f00 204 Device_blk.Str.HF_scl = Device_blk.Str.HF_scl_2; //r. èçìåíÿåì êîýôôèöèåíò ïåðåäà÷è êîíòóðà ÃÂ× îò íîìèíàëüíîãî íà âðåìÿ îáíóëåíèÿ
igor_v 0:8ad47e2b6f00 205 }
igor_v 0:8ad47e2b6f00 206 else if ((WP_reg32 < (Device_blk.Str.WP_rdw << PLC_SHIFT)) && !IsHeating) //r. îõëàæäåíèå
igor_v 0:8ad47e2b6f00 207 {
igor_v 0:8ad47e2b6f00 208 is_zeroing = 1;
igor_v 0:8ad47e2b6f00 209 //r. íàïðÿæåíèå ñáðîñà ïðè îõëàæäåíèè
igor_v 0:8ad47e2b6f00 210 WP_reset_cooling = CPL_reset_calc(Device_blk.Str.WP_reset2, Device_blk.Str.K_WP_rst_cooling, Temp_Aver, Device_blk.Str.TemperNormal);
igor_v 0:8ad47e2b6f00 211
igor_v 0:8ad47e2b6f00 212 plc_transiton = TRANS_COOLING;
igor_v 0:8ad47e2b6f00 213 plc_reset32 = WP_reset_cooling << PLC_SHIFT;
igor_v 0:8ad47e2b6f00 214
igor_v 0:8ad47e2b6f00 215 Device_blk.Str.HF_scl = Device_blk.Str.HF_scl_2; //r. èçìåíÿåì êîýôôèöèåíò ïåðåäà÷è êîíòóðà ÃÂ× îò íîìèíàëüíîãî íà âðåìÿ îáíóëåíèÿ
igor_v 0:8ad47e2b6f00 216 }
igor_v 0:8ad47e2b6f00 217 else //r. ïîðîãè íå ïðåâûøåíû, îáû÷íàÿ ðàáîòà êîíòóðà
igor_v 0:8ad47e2b6f00 218 WP_reg32 = L_mac(WP_reg32, phase_Digital, Device_blk.Str.WP_scl ); // WP_reg32 += phase_Digital * Device_blk.Str.WP_scl;
igor_v 0:8ad47e2b6f00 219
igor_v 0:8ad47e2b6f00 220 }
igor_v 0:8ad47e2b6f00 221 else //r. ôëàã óñòàíîâëåí (1) - ðåæèì îáíóëåíèÿ
igor_v 0:8ad47e2b6f00 222 {
igor_v 0:8ad47e2b6f00 223
igor_v 0:8ad47e2b6f00 224 if (plc_transiton != FINISHED)
igor_v 0:8ad47e2b6f00 225 {
igor_v 0:8ad47e2b6f00 226 if (plc_transiton == TRANS_HEATING)
igor_v 0:8ad47e2b6f00 227 {
igor_v 0:8ad47e2b6f00 228
igor_v 0:8ad47e2b6f00 229 WP_reg32 = L_sub(WP_reg32, Device_blk.Str.WP_transition_step); // WP_reg32 -= Device_blk.Str.WP_transition_step;
igor_v 0:8ad47e2b6f00 230 if (WP_reg32 < plc_reset32)
igor_v 0:8ad47e2b6f00 231 {
igor_v 0:8ad47e2b6f00 232 zero_delay = 0;
igor_v 0:8ad47e2b6f00 233 plc_transiton = FINISHED; //r.false;
igor_v 0:8ad47e2b6f00 234 WP_reg32 = plc_reset32;
igor_v 0:8ad47e2b6f00 235 }
igor_v 0:8ad47e2b6f00 236 }
igor_v 0:8ad47e2b6f00 237 else // plc_transiton == TRANS_COOLING
igor_v 0:8ad47e2b6f00 238 {
igor_v 0:8ad47e2b6f00 239 WP_reg32 = L_add(WP_reg32, Device_blk.Str.WP_transition_step); // WP_reg32 += Device_blk.Str.WP_transition_step;
igor_v 0:8ad47e2b6f00 240 if (WP_reg32 > plc_reset32)
igor_v 0:8ad47e2b6f00 241 {
igor_v 0:8ad47e2b6f00 242 zero_delay = 0;
igor_v 0:8ad47e2b6f00 243 plc_transiton = FINISHED; //r.false;
igor_v 0:8ad47e2b6f00 244 WP_reg32 = plc_reset32;
igor_v 0:8ad47e2b6f00 245 }
igor_v 0:8ad47e2b6f00 246 }
igor_v 0:8ad47e2b6f00 247 }
igor_v 0:8ad47e2b6f00 248 else
igor_v 0:8ad47e2b6f00 249
igor_v 0:8ad47e2b6f00 250 if (zero_delay < Device_blk.Str.WP_mdy)
igor_v 0:8ad47e2b6f00 251 {
igor_v 0:8ad47e2b6f00 252 zero_delay++;
igor_v 0:8ad47e2b6f00 253 }
igor_v 0:8ad47e2b6f00 254 else //e. resetting was completed //r. îáíóëåíèå çàêîí÷èëîñü
igor_v 0:8ad47e2b6f00 255 {
igor_v 0:8ad47e2b6f00 256 is_zeroing = 0;
igor_v 0:8ad47e2b6f00 257 //e. save the temperature for further comparison //r. çàïîìèíàåì òåìïåðàòóðó äëÿ äàëüíåéøåãî ñðàâíåíèÿ
igor_v 0:8ad47e2b6f00 258 // TempOfReset = Temp_Aver; //r.x. Temp5_Aver; //r. Tmp_Out[TSENS_NUMB]; // T4;
igor_v 0:8ad47e2b6f00 259 //r.x Zero_Numb_dbg++; // òàê ìîæíî ïîäñ÷èòûâàòü ÷èñëî îáíóëåíèé
igor_v 0:8ad47e2b6f00 260
igor_v 0:8ad47e2b6f00 261 // DithFreqRangeCalc(); //e. calculation of range of the division factor for the dither drive frequency, depending on current temperature //r. ðàñ÷åò ãðàíèö êîýôôèöèåíòà äåëåíèÿ äëÿ ÷àñòîòû âèáðîïðèâîäà, çàâèñÿùèõ îò òåêóùåé òåìïåðàòóðû
igor_v 0:8ad47e2b6f00 262 }
igor_v 0:8ad47e2b6f00 263 }
igor_v 0:8ad47e2b6f00 264
igor_v 0:8ad47e2b6f00 265 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
igor_v 0:8ad47e2b6f00 266
igor_v 0:8ad47e2b6f00 267
igor_v 0:8ad47e2b6f00 268 if ( loop_is_closed(WP_REG_ON) ) //e. the regulator loop is closed //r. êîíòóð çàìêíóò
igor_v 0:8ad47e2b6f00 269 {
igor_v 0:8ad47e2b6f00 270 Output.Str.WP_reg = (int)(WP_reg32 >> PLC_SHIFT); //e. we use as controlling - voltages of the integrator //r. èñïîëüçóåì êàê óïðàâëÿþùåå - íàïðÿæåíèÿ èíòåãðàòîðà
igor_v 0:8ad47e2b6f00 271
igor_v 0:8ad47e2b6f00 272 }
igor_v 0:8ad47e2b6f00 273 else //e. the regulator loop is open //r. êîíòóð ðàçîìêíóò
igor_v 0:8ad47e2b6f00 274 {
igor_v 0:8ad47e2b6f00 275 WP_reg32 = Output.Str.WP_reg << PLC_SHIFT; //e. set the previous value of the WP_reg //r. ïðèñâàèâàåì ïðåäûäóùåå çíà÷åíèå WP_reg
igor_v 0:8ad47e2b6f00 276
igor_v 0:8ad47e2b6f00 277 }
igor_v 0:8ad47e2b6f00 278
igor_v 0:8ad47e2b6f00 279 //e. integartion of output of the PD of the CPLC regulator for the technological output on the Rate command //r. èíòåãðèðîâàíèå âûõîäà ÔÄ êîíòóðà ÑÐÏ äëÿ òåõíîëîãè÷åñêîãî âûâîäà ïî êîìàíäå Rate
igor_v 0:8ad47e2b6f00 280
igor_v 0:8ad47e2b6f00 281 Output.Str.WP_pll = WP_PhaseDetectorRate( WP_Phase_Det, time_1_Sec);
igor_v 0:8ad47e2b6f00 282
igor_v 0:8ad47e2b6f00 283 } // clc_PLC
igor_v 0:8ad47e2b6f00 284
igor_v 0:8ad47e2b6f00 285 /******************************************************************************
igor_v 0:8ad47e2b6f00 286 ** Function name: Signal_2_Oscill
igor_v 0:8ad47e2b6f00 287 **
igor_v 0:8ad47e2b6f00 288 ** Descriptions: Procedure of analog worm output
igor_v 0:8ad47e2b6f00 289 **
igor_v 0:8ad47e2b6f00 290 ** parameters: Type of output
igor_v 0:8ad47e2b6f00 291 ** Returned value: code to DAC
igor_v 0:8ad47e2b6f00 292 **
igor_v 0:8ad47e2b6f00 293 ******************************************************************************/
igor_v 0:8ad47e2b6f00 294 int Signal_2_Oscill() //e. the signal for the control by scope on DAC output (was DS) //r. ñèãíàë äëÿ êîíòðîëÿ îñöèëëîãðàôîì íà âûõîäå ÖÀÏ (áûâøèé ÄÓÏ)
igor_v 0:8ad47e2b6f00 295 {
igor_v 0:8ad47e2b6f00 296 // Scope_Mode var not used now, reserved for future applications
igor_v 0:8ad47e2b6f00 297 return (-WP_Phase_Det << 2);
igor_v 0:8ad47e2b6f00 298 } // Signal_2_Oscill
igor_v 0:8ad47e2b6f00 299
igor_v 0:8ad47e2b6f00 300 /******************************************************************************
igor_v 0:8ad47e2b6f00 301 ** Function name: clc_WP_sin
igor_v 0:8ad47e2b6f00 302 **
igor_v 0:8ad47e2b6f00 303 ** Descriptions: Procedure of scan signal generating
igor_v 0:8ad47e2b6f00 304 **
igor_v 0:8ad47e2b6f00 305 ** parameters: None
igor_v 0:8ad47e2b6f00 306 ** Returned value: Current code for scan signal DAC of PLC
igor_v 0:8ad47e2b6f00 307 **
igor_v 0:8ad47e2b6f00 308 ******************************************************************************/
igor_v 0:8ad47e2b6f00 309 int clc_WP_sin(void)
igor_v 0:8ad47e2b6f00 310 {
igor_v 0:8ad47e2b6f00 311 static int index = 0;
igor_v 0:8ad47e2b6f00 312 index++;
igor_v 0:8ad47e2b6f00 313
igor_v 0:8ad47e2b6f00 314 if (index >= 40/*Device_blk.Str.PI_b3*/)
igor_v 0:8ad47e2b6f00 315 index = 0;
igor_v 0:8ad47e2b6f00 316 /* if (index > 20)
igor_v 0:8ad47e2b6f00 317 LPC_GPIO0->FIOSET = (1<<26);
igor_v 0:8ad47e2b6f00 318 else
igor_v 0:8ad47e2b6f00 319 LPC_GPIO0->FIOCLR = (1<<26); */
igor_v 0:8ad47e2b6f00 320 DAC_Output(sin_func[index]); //output to DAC
igor_v 0:8ad47e2b6f00 321
igor_v 0:8ad47e2b6f00 322 return (sin_func[index]);
igor_v 0:8ad47e2b6f00 323 } // clc_WP_sin
igor_v 0:8ad47e2b6f00 324
igor_v 0:8ad47e2b6f00 325 /******************************************************************************
igor_v 0:8ad47e2b6f00 326 ** Function name: WP_PhaseDetectorRate
igor_v 0:8ad47e2b6f00 327 **
igor_v 0:8ad47e2b6f00 328 ** Descriptions: Integartion of output of the PD of the CPLC regulator
igor_v 0:8ad47e2b6f00 329 for the technological output on the Rate command
igor_v 0:8ad47e2b6f00 330 **
igor_v 0:8ad47e2b6f00 331 ** Parameters: Current PD magnitude, period of integration
igor_v 0:8ad47e2b6f00 332 ** Returned value: Integrated magnitude of PD
igor_v 0:8ad47e2b6f00 333 **
igor_v 0:8ad47e2b6f00 334 ******************************************************************************/
igor_v 0:8ad47e2b6f00 335 int WP_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime)
igor_v 0:8ad47e2b6f00 336 {
igor_v 0:8ad47e2b6f00 337
igor_v 0:8ad47e2b6f00 338 static int SampleAndHoldOut = 0;
igor_v 0:8ad47e2b6f00 339 static int WP_PhasDet_integr = 0;//, WP_PhasDetector = 0;
igor_v 0:8ad47e2b6f00 340
igor_v 0:8ad47e2b6f00 341 if (IntegrateTime == DEVICE_SAMPLE_RATE_uks)
igor_v 0:8ad47e2b6f00 342 {
igor_v 0:8ad47e2b6f00 343 SampleAndHoldOut = (int)(WP_PhasDet_integr >> PLC_PHASE_DET_SHIFT);
igor_v 0:8ad47e2b6f00 344 WP_PhasDet_integr = 0;
igor_v 0:8ad47e2b6f00 345 }
igor_v 0:8ad47e2b6f00 346 else
igor_v 0:8ad47e2b6f00 347 {
igor_v 0:8ad47e2b6f00 348 WP_PhasDet_integr += PhaseDetInput;
igor_v 0:8ad47e2b6f00 349 }
igor_v 0:8ad47e2b6f00 350 return (SampleAndHoldOut);
igor_v 0:8ad47e2b6f00 351 } // WP_PhaseDetectorRate
igor_v 0:8ad47e2b6f00 352
igor_v 0:8ad47e2b6f00 353
igor_v 0:8ad47e2b6f00 354