Dmitry Kovalev / Mbed 2 deprecated LGstaandart

forkd

Dependencies:   mbed

Fork of LG2 by Dmitry Kovalev

host/Source/App/PLC_reg.c@23:12e6183f04d4, 2016-02-03 (annotated)

Committer:
Kovalev_D
Date:
Wed Feb 03 10:44:42 2016 +0300
Revision:
23:12e6183f04d4
[thyz

Who changed what in which revision?

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