Dmitry Kovalev / Mbed 2 deprecated LGstaandart

forkd

Dependencies:   mbed

Fork of LG2 by Dmitry Kovalev

host/Source/App/Dither_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 #include "lpc17xx.h"
Kovalev_D 23:12e6183f04d4 2 #include "mathDSP.h"
Kovalev_D 23:12e6183f04d4 3 #include "InputOutput.h"
Kovalev_D 23:12e6183f04d4 4 #include "CyclesSync.h"
Kovalev_D 23:12e6183f04d4 5 #include "CntrlGLD.h"
Kovalev_D 23:12e6183f04d4 6 #include "stdlib.h"
Kovalev_D 23:12e6183f04d4 7 #include "sip.h"
Kovalev_D 23:12e6183f04d4 8 #include "console.h"
Kovalev_D 23:12e6183f04d4 9
Kovalev_D 23:12e6183f04d4 10
Kovalev_D 23:12e6183f04d4 11 #define SHIFT_7680_12500 15 //e. 14 digits for 7680 to 12500 clock converting and 1 division digit
Kovalev_D 23:12e6183f04d4 12 #define SHIFT_C_7680_12500 11 //
Kovalev_D 23:12e6183f04d4 13 #define DITH_VBN_SHIFT 2 //e. //r. îïðåäåëÿåò ñäâèã (äåëåíèå íà 4) êîýôôèöèåíòà äåëåíèÿ âèáðîïðèâîäà, ÷òîáû èìåòü çàïàñ íà ðåãóëèðîâàíèå
Kovalev_D 23:12e6183f04d4 14 #define DITH_VB_TAU_SHIFT 2
Kovalev_D 23:12e6183f04d4 15
Kovalev_D 23:12e6183f04d4 16
Kovalev_D 23:12e6183f04d4 17
Kovalev_D 23:12e6183f04d4 18 int32_t RI_diff; //e.input signal of "recovery" APS //r. âõîäíîé ñèãíàë "âîññòàíîâëåííîãî" ÄÓÏ
Kovalev_D 23:12e6183f04d4 19 int32_t MaxDelay;
Kovalev_D 23:12e6183f04d4 20 int32_t VB_tau_Ins; //r. âíóòðåííåå çíà÷åíèå êîíòóðà ðåãóëèðîâàíèÿ Òàó
Kovalev_D 23:12e6183f04d4 21 int32_t VB_Nmin0; //r. ìèíèìóì âûõîäíîãî çíà÷åíèÿ ðåãóëÿòîðà ïåðèîäà äëÿ òåìïåðàòóðû Device_blk.Str.TemperNormal
Kovalev_D 23:12e6183f04d4 22 int32_t VB_Nmax0; //r. ìàêñèìóì âûõîäíîãî çíà÷åíèÿ ðåãóëÿòîðà ïåðèîäà äëÿ òåìïåðàòóðû Device_blk.Str.TemperNormal
Kovalev_D 23:12e6183f04d4 23
Kovalev_D 23:12e6183f04d4 24 uint32_t In_Flag = 0;
Kovalev_D 23:12e6183f04d4 25 uint32_t SwitchCntInq = 0;
Kovalev_D 23:12e6183f04d4 26
Kovalev_D 23:12e6183f04d4 27 int32_t accum_error = 0;
Kovalev_D 23:12e6183f04d4 28 int32_t ph_error = 0;
Kovalev_D 23:12e6183f04d4 29 int32_t accum_error_old = 0;
Kovalev_D 23:12e6183f04d4 30 int32_t PhaseShift;
Kovalev_D 23:12e6183f04d4 31 int32_t temp2;
Kovalev_D 23:12e6183f04d4 32 int32_t temp3;
Kovalev_D 23:12e6183f04d4 33
Kovalev_D 23:12e6183f04d4 34 #if defined DITHERSIM
Kovalev_D 23:12e6183f04d4 35 int32_t timeDither = 0;
Kovalev_D 23:12e6183f04d4 36 int32_t LIM0;
Kovalev_D 23:12e6183f04d4 37 #endif
Kovalev_D 23:12e6183f04d4 38 extern uint32_t Vibro_2_CountIn;
Kovalev_D 23:12e6183f04d4 39 void clc_Noise_regulator(void);
Kovalev_D 23:12e6183f04d4 40
Kovalev_D 23:12e6183f04d4 41 /******************************************************************************
Kovalev_D 23:12e6183f04d4 42 ** Function name: VibroDither_Set
Kovalev_D 23:12e6183f04d4 43 **
Kovalev_D 23:12e6183f04d4 44 ** Descriptions: Set period and pulse width for dither.
Kovalev_D 23:12e6183f04d4 45 **
Kovalev_D 23:12e6183f04d4 46 ** parameters: duration of vibro pulses, period of dither
Kovalev_D 23:12e6183f04d4 47 ** Returned value: None
Kovalev_D 23:12e6183f04d4 48 **
Kovalev_D 23:12e6183f04d4 49 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 50 void VibroDither_Set()
Kovalev_D 23:12e6183f04d4 51 {
Kovalev_D 23:12e6183f04d4 52 //êîýôô.äåëåíèÿ N âèáðîïðèâîäà (ïåðèîä êîëåáàíèé) ÂÏ = T_Vibro äëèòåëüíîñòü èìïóëüñà âèáðîïðèâîäà>>
Kovalev_D 23:12e6183f04d4 53
Kovalev_D 23:12e6183f04d4 54
Kovalev_D 23:12e6183f04d4 55 Device_blk.Str.VB_N = Output.Str.T_Vibro;
Kovalev_D 23:12e6183f04d4 56 LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;//#define SHIFT_7680_12500 15 ñìåøåíèå äëÿ êîíâåðòàöèè ÷àñòîòû èç 7680 â 12500
Kovalev_D 23:12e6183f04d4 57 #if defined DITHERSIM
Kovalev_D 23:12e6183f04d4 58 6565 LIM0 = (Output.Str.T_Vibro*86)>>16;
Kovalev_D 23:12e6183f04d4 59 #endif
Kovalev_D 23:12e6183f04d4 60 #if !defined CONSTCYCLE
Kovalev_D 23:12e6183f04d4 61 5655 SwitchCntInq = 1; //to enable inquiry timer reloading
Kovalev_D 23:12e6183f04d4 62 #endif
Kovalev_D 23:12e6183f04d4 63 }
Kovalev_D 23:12e6183f04d4 64 /******************************************************************************
Kovalev_D 23:12e6183f04d4 65 ** Function name: VibroDither_SwitchOn
Kovalev_D 23:12e6183f04d4 66 **
Kovalev_D 23:12e6183f04d4 67 ** Descriptions: VibroDither switching on.
Kovalev_D 23:12e6183f04d4 68 **
Kovalev_D 23:12e6183f04d4 69 ** parameters: None
Kovalev_D 23:12e6183f04d4 70 ** Returned value: None
Kovalev_D 23:12e6183f04d4 71 **
Kovalev_D 23:12e6183f04d4 72 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 73 void VibroDither_SwitchOn()
Kovalev_D 23:12e6183f04d4 74 {
Kovalev_D 23:12e6183f04d4 75 LPC_MCPWM->CON_SET = 1<<8; //start vibro dither
Kovalev_D 23:12e6183f04d4 76 }
Kovalev_D 23:12e6183f04d4 77 /******************************************************************************
Kovalev_D 23:12e6183f04d4 78 ** Function name: VibroDither_SwitchOff
Kovalev_D 23:12e6183f04d4 79 **
Kovalev_D 23:12e6183f04d4 80 ** Descriptions: VibroDither switching off.
Kovalev_D 23:12e6183f04d4 81 **
Kovalev_D 23:12e6183f04d4 82 ** parameters: None
Kovalev_D 23:12e6183f04d4 83 ** Returned value: None
Kovalev_D 23:12e6183f04d4 84 **
Kovalev_D 23:12e6183f04d4 85 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 86 void VibroDither_SwitchOff()
Kovalev_D 23:12e6183f04d4 87 {
Kovalev_D 23:12e6183f04d4 88 LPC_MCPWM->CON_CLR = 1<<8; //stop vibro dither
Kovalev_D 23:12e6183f04d4 89 }
Kovalev_D 23:12e6183f04d4 90
Kovalev_D 23:12e6183f04d4 91 /******************************************************************************
Kovalev_D 23:12e6183f04d4 92 ** Function name: VB_MeanderDelay
Kovalev_D 23:12e6183f04d4 93 **
Kovalev_D 23:12e6183f04d4 94 ** Descriptions: Routine for addition of delay to meander
Kovalev_D 23:12e6183f04d4 95 **
Kovalev_D 23:12e6183f04d4 96 ** parameters: meander, delay magnitude, max delay
Kovalev_D 23:12e6183f04d4 97 ** Returned value: delayed meander
Kovalev_D 23:12e6183f04d4 98 **
Kovalev_D 23:12e6183f04d4 99 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 100 int VB_MeanderDelay(int VB_Meander, int Delay100uS, int MaxDly)
Kovalev_D 23:12e6183f04d4 101 {
Kovalev_D 23:12e6183f04d4 102 static int poz_counter = 0, neg_counter = 0, flg_delay;
Kovalev_D 23:12e6183f04d4 103
Kovalev_D 23:12e6183f04d4 104 if (Delay100uS == 0)
Kovalev_D 23:12e6183f04d4 105 {
Kovalev_D 23:12e6183f04d4 106 return (VB_Meander);
Kovalev_D 23:12e6183f04d4 107 }
Kovalev_D 23:12e6183f04d4 108
Kovalev_D 23:12e6183f04d4 109 if (Delay100uS > 0)
Kovalev_D 23:12e6183f04d4 110 {
Kovalev_D 23:12e6183f04d4 111 if (Delay100uS > MaxDly) { Delay100uS = MaxDly; }
Kovalev_D 23:12e6183f04d4 112 if (VB_Meander) //e. outgoing WP_flg flag, which delayed by the WP_ref //r. ôîðìèðîâàíèå çàäåðæàííîãî íà âåëè÷èíó WP_ref ôëàãà poz_sin_flag
Kovalev_D 23:12e6183f04d4 113 {
Kovalev_D 23:12e6183f04d4 114 neg_counter = 0;
Kovalev_D 23:12e6183f04d4 115 poz_counter++;
Kovalev_D 23:12e6183f04d4 116 }
Kovalev_D 23:12e6183f04d4 117 else
Kovalev_D 23:12e6183f04d4 118 {
Kovalev_D 23:12e6183f04d4 119 poz_counter = 0;
Kovalev_D 23:12e6183f04d4 120 neg_counter++;
Kovalev_D 23:12e6183f04d4 121 }
Kovalev_D 23:12e6183f04d4 122 if (poz_counter == Delay100uS) { flg_delay = 1; }
Kovalev_D 23:12e6183f04d4 123 if (neg_counter == Delay100uS) { flg_delay = 0; }
Kovalev_D 23:12e6183f04d4 124 }
Kovalev_D 23:12e6183f04d4 125 else
Kovalev_D 23:12e6183f04d4 126 {
Kovalev_D 23:12e6183f04d4 127 Delay100uS = -Delay100uS;
Kovalev_D 23:12e6183f04d4 128 if (Delay100uS > MaxDly) { Delay100uS = MaxDly; }
Kovalev_D 23:12e6183f04d4 129 if (VB_Meander) //e. outgoing WP_flg flag, which delayed by the WP_ref //r. ôîðìèðîâàíèå çàäåðæàííîãî íà âåëè÷èíó WP_ref ôëàãà poz_sin_flag
Kovalev_D 23:12e6183f04d4 130 {
Kovalev_D 23:12e6183f04d4 131 neg_counter = MaxDly + 1;
Kovalev_D 23:12e6183f04d4 132 poz_counter--;
Kovalev_D 23:12e6183f04d4 133 }
Kovalev_D 23:12e6183f04d4 134 else
Kovalev_D 23:12e6183f04d4 135 {
Kovalev_D 23:12e6183f04d4 136 poz_counter = MaxDly + 1;
Kovalev_D 23:12e6183f04d4 137 neg_counter--;
Kovalev_D 23:12e6183f04d4 138 }
Kovalev_D 23:12e6183f04d4 139 if (poz_counter == Delay100uS) { flg_delay = 0; }
Kovalev_D 23:12e6183f04d4 140 if (neg_counter == Delay100uS) { flg_delay = 1; }
Kovalev_D 23:12e6183f04d4 141 }
Kovalev_D 23:12e6183f04d4 142 return (flg_delay);
Kovalev_D 23:12e6183f04d4 143 } // VB_MeanderDelay
Kovalev_D 23:12e6183f04d4 144 /******************************************************************************
Kovalev_D 23:12e6183f04d4 145 ** Function name: VB_PhaseDetectorRate
Kovalev_D 23:12e6183f04d4 146 **
Kovalev_D 23:12e6183f04d4 147 ** Descriptions: Routine for accumulation of dither error
Kovalev_D 23:12e6183f04d4 148 **
Kovalev_D 23:12e6183f04d4 149 ** parameters: None
Kovalev_D 23:12e6183f04d4 150 ** Returned value: None
Kovalev_D 23:12e6183f04d4 151 **
Kovalev_D 23:12e6183f04d4 152 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 153 int VB_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime)
Kovalev_D 23:12e6183f04d4 154 {
Kovalev_D 23:12e6183f04d4 155
Kovalev_D 23:12e6183f04d4 156 static int SampleAndHoldOut = 0, VB_PhasDet_integr = 0;
Kovalev_D 23:12e6183f04d4 157
Kovalev_D 23:12e6183f04d4 158 if (IntegrateTime == DEVICE_SAMPLE_RATE_uks)
Kovalev_D 23:12e6183f04d4 159 {
Kovalev_D 23:12e6183f04d4 160 SampleAndHoldOut = VB_PhasDet_integr;
Kovalev_D 23:12e6183f04d4 161 VB_PhasDet_integr = 0;
Kovalev_D 23:12e6183f04d4 162 }
Kovalev_D 23:12e6183f04d4 163 else
Kovalev_D 23:12e6183f04d4 164 {
Kovalev_D 23:12e6183f04d4 165 VB_PhasDet_integr += PhaseDetInput;
Kovalev_D 23:12e6183f04d4 166 }
Kovalev_D 23:12e6183f04d4 167 return (SampleAndHoldOut);
Kovalev_D 23:12e6183f04d4 168 } // VB_PhaseDetectorRate
Kovalev_D 23:12e6183f04d4 169
Kovalev_D 23:12e6183f04d4 170 /*r.
Kovalev_D 23:12e6183f04d4 171
Kovalev_D 23:12e6183f04d4 172 DelayedDithMeander - çàäåðæàííûé ìåàíäð (íà âåëè÷èíó VB_phs)
Kovalev_D 23:12e6183f04d4 173 Âûõîä
Kovalev_D 23:12e6183f04d4 174 VB_N - êîýôôèöèåíò äåëåíèÿ
Kovalev_D 23:12e6183f04d4 175 */
Kovalev_D 23:12e6183f04d4 176 /******************************************************************************
Kovalev_D 23:12e6183f04d4 177 ** Function name: clc_Dith_regulator
Kovalev_D 23:12e6183f04d4 178 **
Kovalev_D 23:12e6183f04d4 179 ** Descriptions: Routine for dither frequency controller
Kovalev_D 23:12e6183f04d4 180 **
Kovalev_D 23:12e6183f04d4 181 ** parameters: None
Kovalev_D 23:12e6183f04d4 182 ** Returned value: None
Kovalev_D 23:12e6183f04d4 183 **
Kovalev_D 23:12e6183f04d4 184 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 185 void clc_Dith_regulator(void)
Kovalev_D 23:12e6183f04d4 186 {
Kovalev_D 23:12e6183f04d4 187 // static int smooth=0, buf[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, i = 0;
Kovalev_D 23:12e6183f04d4 188 // int ph_error;
Kovalev_D 23:12e6183f04d4 189 static int dith_period = 0;//, accum_error = 0;
Kovalev_D 23:12e6183f04d4 190
Kovalev_D 23:12e6183f04d4 191 RI_diff = DUP_Filt(Dif_Curr_Vib<<2);
Kovalev_D 23:12e6183f04d4 192
Kovalev_D 23:12e6183f04d4 193 if (RI_diff >= 0)
Kovalev_D 23:12e6183f04d4 194 ph_error = 1;
Kovalev_D 23:12e6183f04d4 195 else
Kovalev_D 23:12e6183f04d4 196 ph_error = 0;
Kovalev_D 23:12e6183f04d4 197
Kovalev_D 23:12e6183f04d4 198 if (LPC_MCPWM->INTF & 0x0001) //vibro pulse has been formed
Kovalev_D 23:12e6183f04d4 199 {
Kovalev_D 23:12e6183f04d4 200 LPC_MCPWM->INTF_CLR |= 0x0001;
Kovalev_D 23:12e6183f04d4 201 if (LPC_MCPWM->MAT2 > LPC_MCPWM->MAT1)
Kovalev_D 23:12e6183f04d4 202 {
Kovalev_D 23:12e6183f04d4 203 // LPC_GPIO2->FIOSET = 0x000000FF; // turn on the LED
Kovalev_D 23:12e6183f04d4 204 if (SwitchCntInq) //inquiry cycle duration must be changed
Kovalev_D 23:12e6183f04d4 205 {
Kovalev_D 23:12e6183f04d4 206 LPC_PWM1->MR0 = (Output.Str.T_Vibro*Vibro_2_CountIn)>>SHIFT_C_7680_12500;
Kovalev_D 23:12e6183f04d4 207 LPC_PWM1->LER = LER0_EN ; //e. enable updating of register
Kovalev_D 23:12e6183f04d4 208 SwitchCntInq = 0;
Kovalev_D 23:12e6183f04d4 209 }
Kovalev_D 23:12e6183f04d4 210 LPC_MCPWM->MAT1 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;
Kovalev_D 23:12e6183f04d4 211 LPC_MCPWM->MAT2 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500;
Kovalev_D 23:12e6183f04d4 212 In_Flag = 0;
Kovalev_D 23:12e6183f04d4 213 }
Kovalev_D 23:12e6183f04d4 214 else
Kovalev_D 23:12e6183f04d4 215 {
Kovalev_D 23:12e6183f04d4 216 // LPC_GPIO2->FIOCLR = 0x000000FF; // turn off the LED
Kovalev_D 23:12e6183f04d4 217 LPC_MCPWM->MAT2 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;
Kovalev_D 23:12e6183f04d4 218 LPC_MCPWM->MAT1 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500;
Kovalev_D 23:12e6183f04d4 219 In_Flag = 1;
Kovalev_D 23:12e6183f04d4 220 dith_period++;
Kovalev_D 23:12e6183f04d4 221 }
Kovalev_D 23:12e6183f04d4 222 }
Kovalev_D 23:12e6183f04d4 223
Kovalev_D 23:12e6183f04d4 224 temp3 = VB_MeanderDelay(In_Flag, Device_blk.Str.VB_phs, MaxDelay); //r. ôîðìèðîâàíèå çàäåðæàííîãî ñèãíàëà ìåàíäð
Kovalev_D 23:12e6183f04d4 225 temp2 = ( ( temp3 ^ ph_error ) << 1 ) - 1; //r. àíàëîãîâûé âûõîä XOR ÔÄ(-1..+1, ò.ê. const=1)
Kovalev_D 23:12e6183f04d4 226 accum_error += temp2;
Kovalev_D 23:12e6183f04d4 227
Kovalev_D 23:12e6183f04d4 228 Output.Str.T_VB_pll = VB_PhaseDetectorRate(temp2, time_1_Sec); //r. ôîðìèðîâàíèå ïðîèíòåãðèðîâàííîãî çà 1 ñåê àíàëîãîâîãî ñèãíàëà ÔÄ âèáðîïðèâîäà
Kovalev_D 23:12e6183f04d4 229 if ( dith_period > DITHER_REG_PERIOD ) //r. ïðîâåðêà ñîñòîÿíèÿ ñ÷åò÷èêà dith_period
Kovalev_D 23:12e6183f04d4 230 {
Kovalev_D 23:12e6183f04d4 231 dith_period = 0; //r. 40 ïåðèîäîâ - îáíóëåíèå ñ÷åò÷èêà ïåðèîäîâ âèáðîïðèâîäà
Kovalev_D 23:12e6183f04d4 232 //r. ìàñøòàáèðîâàíèå è ñóììèðîâàíèå ñ îêðóãëåíèåì è íàñûùåíèåì
Kovalev_D 23:12e6183f04d4 233 if ( loop_is_closed(VB_FREQ_ON) )
Kovalev_D 23:12e6183f04d4 234 {
Kovalev_D 23:12e6183f04d4 235 Device_blk.Str.VB_N = mac_r(Device_blk.Str.VB_N << (16 - DITH_VBN_SHIFT),-accum_error,Device_blk.Str.VB_scl) << DITH_VBN_SHIFT;
Kovalev_D 23:12e6183f04d4 236
Kovalev_D 23:12e6183f04d4 237 Saturation(Device_blk.Str.VB_N, Device_blk.Str.VB_Nmax, Device_blk.Str.VB_Nmin); //r. ïðîâåðêà âåðõíåãî äèàïàçîíà ðåãóëèðîâàíèÿ
Kovalev_D 23:12e6183f04d4 238 accum_error = 0; //r. îáíóëåíèå ñóììû _VB_Uab40
Kovalev_D 23:12e6183f04d4 239 }
Kovalev_D 23:12e6183f04d4 240 }
Kovalev_D 23:12e6183f04d4 241
Kovalev_D 23:12e6183f04d4 242 if ( loop_is_closed(VB_FREQ_ON) ) //r. ôðîíò áûë, ïðîâåðèòü âêëþ÷åí ëè êîíòóð ñòàáèëèçàöèè
Kovalev_D 23:12e6183f04d4 243 {
Kovalev_D 23:12e6183f04d4 244 Output.Str.T_Vibro = Device_blk.Str.VB_N;
Kovalev_D 23:12e6183f04d4 245 LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; //r. âêëþ÷åí, çàãðóçèòü âû÷èñëåííûå çíà÷åíèÿ ïåðèîäà
Kovalev_D 23:12e6183f04d4 246 }
Kovalev_D 23:12e6183f04d4 247 // cyclic built-in test
Kovalev_D 23:12e6183f04d4 248 if ((Output.Str.T_Vibro > Device_blk.Str.VB_Nmax) || (Output.Str.T_Vibro < Device_blk.Str.VB_Nmin))
Kovalev_D 23:12e6183f04d4 249 {
Kovalev_D 23:12e6183f04d4 250 Valid_Data |= DITH_FREQ_ERROR;
Kovalev_D 23:12e6183f04d4 251 }
Kovalev_D 23:12e6183f04d4 252 } // clc_Dith_regulator
Kovalev_D 23:12e6183f04d4 253
Kovalev_D 23:12e6183f04d4 254 /******************************************************************************
Kovalev_D 23:12e6183f04d4 255 ** Function name: clc_OutFreq_regulator
Kovalev_D 23:12e6183f04d4 256 **
Kovalev_D 23:12e6183f04d4 257 ** Descriptions: Routine for output frequency controller
Kovalev_D 23:12e6183f04d4 258 **
Kovalev_D 23:12e6183f04d4 259 ** parameters: None
Kovalev_D 23:12e6183f04d4 260 ** Returned value: None
Kovalev_D 23:12e6183f04d4 261 **
Kovalev_D 23:12e6183f04d4 262 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 263 void clc_OutFreq_regulator(void)
Kovalev_D 23:12e6183f04d4 264 {
Kovalev_D 23:12e6183f04d4 265 static int out_freq_sum = 0;
Kovalev_D 23:12e6183f04d4 266 static int temp;
Kovalev_D 23:12e6183f04d4 267 /*
Kovalev_D 23:12e6183f04d4 268 if (Dif_Curr_Vib > 0) //e. angular speed > 0 //r.ñêîðîñòü ïîëîæèòåëüíà
Kovalev_D 23:12e6183f04d4 269 {
Kovalev_D 23:12e6183f04d4 270 if (RI_diff > 0)
Kovalev_D 23:12e6183f04d4 271 out_freq_sum += (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
Kovalev_D 23:12e6183f04d4 272 else
Kovalev_D 23:12e6183f04d4 273 out_freq_sum -= (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
Kovalev_D 23:12e6183f04d4 274 }
Kovalev_D 23:12e6183f04d4 275 else //e. angular speed < 0 //r.ñêîðîñòü îòðèöàòåëüíà
Kovalev_D 23:12e6183f04d4 276 {
Kovalev_D 23:12e6183f04d4 277 if (RI_diff < 0)
Kovalev_D 23:12e6183f04d4 278 out_freq_sum += (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
Kovalev_D 23:12e6183f04d4 279 else
Kovalev_D 23:12e6183f04d4 280 out_freq_sum -= (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
Kovalev_D 23:12e6183f04d4 281 }
Kovalev_D 23:12e6183f04d4 282 */
Kovalev_D 23:12e6183f04d4 283 if(Dif_Curr_Vib>0)
Kovalev_D 23:12e6183f04d4 284 out_freq_sum += Dif_Curr_Vib;
Kovalev_D 23:12e6183f04d4 285 else
Kovalev_D 23:12e6183f04d4 286 out_freq_sum -= Dif_Curr_Vib;
Kovalev_D 23:12e6183f04d4 287
Kovalev_D 23:12e6183f04d4 288
Kovalev_D 23:12e6183f04d4 289 if (time_1_Sec == DEVICE_SAMPLE_RATE_uks) //e. second has elapsed, fix the output frequency value //r. ñåêóíäà ïðîøëà, çàôèêñèðîâàòü çíà÷åíèå ÷àñòîòû ðàñùåïëåíèÿ
Kovalev_D 23:12e6183f04d4 290 {
Kovalev_D 23:12e6183f04d4 291 if (loop_is_closed(VB_TAU_ON)) //e. the regulator loop is closed //r. êîíòóð çàìêíóò
Kovalev_D 23:12e6183f04d4 292 {
Kovalev_D 23:12e6183f04d4 293 temp = Device_blk.Str.VB_Fdf_Hi << 16;
Kovalev_D 23:12e6183f04d4 294 temp |= Device_blk.Str.VB_Fdf_Lo;
Kovalev_D 23:12e6183f04d4 295 temp = L_sub(out_freq_sum, temp) >> 3; // (out_freq_sum - temp) with saturation, then >> 3
Kovalev_D 23:12e6183f04d4 296 Saturation(temp, 32767, -32768); // error saturation if error is out of range
Kovalev_D 23:12e6183f04d4 297 //e. scaling and summing with rounding and saturation //r. ìàñøòàáèðîâàíèå è ñóììèðîâàíèå ñ îêðóãëåíèåì è íàñûùåíèåì
Kovalev_D 23:12e6183f04d4 298 VB_tau_Ins = mac_r( VB_tau_Ins << (16 - DITH_VB_TAU_SHIFT),
Kovalev_D 23:12e6183f04d4 299 temp,
Kovalev_D 23:12e6183f04d4 300 Device_blk.Str.VB_Fsc ); // << DITH_VB_TAU_SHIFT;
Kovalev_D 23:12e6183f04d4 301 //e. reduction the VB_Err value to 16 digits (arithmetic right shift to 3 digits) //r. ñâåäåíèå âåëè÷èíû VB_Err ê 16 ðàçðÿäàì (àðèôìåòè÷åñêèé ñäâèã âïðàâî íà 3 ðàçðÿäà)
Kovalev_D 23:12e6183f04d4 302
Kovalev_D 23:12e6183f04d4 303 Saturation(VB_tau_Ins, \
Kovalev_D 23:12e6183f04d4 304 (int)Device_blk.Str.VB_Tmax >> DITH_VB_TAU_SHIFT, \
Kovalev_D 23:12e6183f04d4 305 (int)Device_blk.Str.VB_Tmin >> DITH_VB_TAU_SHIFT); //e. checking upper and lower levels in sign range
Kovalev_D 23:12e6183f04d4 306 VB_tau_Ins <<= DITH_VB_TAU_SHIFT;
Kovalev_D 23:12e6183f04d4 307 }
Kovalev_D 23:12e6183f04d4 308
Kovalev_D 23:12e6183f04d4 309 Output.Str.F_ras = out_freq_sum >> 5; //e. once more divide output frequency by 2, in order to coincide with frequency meter //r. ïîäåëèòü ÷àñòîòó ðàñùåïëåíèÿ åùå íà 2, ÷òîáû ñîâïàëî ñ ÷àñòîòîìåðîì
Kovalev_D 23:12e6183f04d4 310 out_freq_sum = 0; //e. reset accumulated values for next cycle of measurement //r. ñáðîñèòü íàêîïëåííûå çíà÷åíèÿ äëÿ ñëåäóþùåãî öèêëà èçìåðåíèÿ
Kovalev_D 23:12e6183f04d4 311
Kovalev_D 23:12e6183f04d4 312 // cyclic built-in test
Kovalev_D 23:12e6183f04d4 313 // if output frequency is less than 3/4 of nominal then data is invalid
Kovalev_D 23:12e6183f04d4 314 if (Output.Str.F_ras < ((temp >> 7)*3))
Kovalev_D 23:12e6183f04d4 315 {
Kovalev_D 23:12e6183f04d4 316 Valid_Data |= OUT_FREQ_ERROR;
Kovalev_D 23:12e6183f04d4 317 }
Kovalev_D 23:12e6183f04d4 318 else
Kovalev_D 23:12e6183f04d4 319 {
Kovalev_D 23:12e6183f04d4 320 Valid_Data &= ~OUT_FREQ_ERROR;
Kovalev_D 23:12e6183f04d4 321 }
Kovalev_D 23:12e6183f04d4 322 }
Kovalev_D 23:12e6183f04d4 323
Kovalev_D 23:12e6183f04d4 324 clc_Noise_regulator();
Kovalev_D 23:12e6183f04d4 325
Kovalev_D 23:12e6183f04d4 326 if ( loop_is_closed(VB_TAU_ON) ) //r. êîíòóð ñòàáèëèçàöèè âêëþ÷åí?
Kovalev_D 23:12e6183f04d4 327 {
Kovalev_D 23:12e6183f04d4 328 Output.Str.L_Vibro = Device_blk.Str.VB_tau; //r. èíà÷å çàãðóçèòü íîâîå çíà÷åíèå
Kovalev_D 23:12e6183f04d4 329 //r. äëèòåëüíîñòè èìïóëüñîâ âèáðîïðèâîäà
Kovalev_D 23:12e6183f04d4 330 }
Kovalev_D 23:12e6183f04d4 331
Kovalev_D 23:12e6183f04d4 332 } // clc_OutFreq_regulator
Kovalev_D 23:12e6183f04d4 333
Kovalev_D 23:12e6183f04d4 334 //e. noise regulator //r. ñèñòåìà ýëåêòðîííîãî îøóìëåíèÿ âèáðîïðèâîäà
Kovalev_D 23:12e6183f04d4 335 /*r.
Kovalev_D 23:12e6183f04d4 336 PeriodCount (VBN_Cnt) - ñ÷åò÷èê ïåðèîäîâ ñèãíàëà Meander.
Kovalev_D 23:12e6183f04d4 337 Tnoise (VBN_Per)- òåêóùèé ïåðèîä îøóìëåíèÿ.
Kovalev_D 23:12e6183f04d4 338 PeriodNoise (VBN_Tzd) - ñðåäíèé ïåðèîä îøóìëåíèÿ, çàäàííûé ïîëüçîâàòåëåì.
Kovalev_D 23:12e6183f04d4 339 AmpNoise(VBN_Ran) - ìàêñèìàëüíàÿ àìïëèòóäà ïåðèîäà îøóìëåíèÿ (çàäàåòñÿ ïîëüçîâàòåëåì).
Kovalev_D 23:12e6183f04d4 340 Delta (VBN_k) - ãëóáèíà îøóìëåíèÿ (çàäàåòñÿ ïîëüçîâàòåëåì).
Kovalev_D 23:12e6183f04d4 341 Flag(VBN_Mod) - ôëàã çíàêà èçìåíåíèÿ àìïëèòóäû.
Kovalev_D 23:12e6183f04d4 342 Tu(VBN_Tau) - äëèòåëüíîñòü èìïóëüñà îäíîâèáðàòîðà.
Kovalev_D 23:12e6183f04d4 343 Tp(VBN_tau_Ins) - äëèòåëüíîñòü èìïóëüñà îäíîâèáðàòîðà, çàäàâàåìàÿ ñèñòåìîé ðåãóëèðîâêè ÷àñòîòû ðàñùåïëåíèÿ.
Kovalev_D 23:12e6183f04d4 344 */
Kovalev_D 23:12e6183f04d4 345
Kovalev_D 23:12e6183f04d4 346 void clc_Noise_regulator(void)
Kovalev_D 23:12e6183f04d4 347 {
Kovalev_D 23:12e6183f04d4 348 int temp;
Kovalev_D 23:12e6183f04d4 349 static uint32_t Flag = 0;
Kovalev_D 23:12e6183f04d4 350 static int PeriodCount = 0, Tnoise = 0;
Kovalev_D 23:12e6183f04d4 351
Kovalev_D 23:12e6183f04d4 352 if ( PeriodCount >= Tnoise )
Kovalev_D 23:12e6183f04d4 353 {
Kovalev_D 23:12e6183f04d4 354 PeriodCount = 0;
Kovalev_D 23:12e6183f04d4 355 srand(Device_blk.Str.VB_N);// Srand(ïåðèîä êîëåáàíèé ÂÏ) -èíèöèàëèçàöèÿ ãåíåðàòîðà ñëó÷àéíûõ ÷èñåë ñ çåðíîì (VB_N)
Kovalev_D 23:12e6183f04d4 356 //çàäàííûé ïåðèîä îøóìëåíèÿ
Kovalev_D 23:12e6183f04d4 357 Tnoise = add( Device_blk.Str.VBN_Tzd, mult_r(Device_blk.Str.VBN_Ran, rand())); // Tnoise = Device_blk.Str.VBN_Tzd + MULT_RND_SAT( Device_blk.Str.VBN_Ran, rand() );
Kovalev_D 23:12e6183f04d4 358 if ( Flag ) //e. calculation +dF/-dF //r. ðàñ÷åò +dF/-dF
Kovalev_D 23:12e6183f04d4 359 {
Kovalev_D 23:12e6183f04d4 360 temp = Device_blk.Str.VBN_k; //r. 25 - çàäàííàÿ êîíñòàíòà îøóìëåíèÿ
Kovalev_D 23:12e6183f04d4 361 }
Kovalev_D 23:12e6183f04d4 362 else
Kovalev_D 23:12e6183f04d4 363 {
Kovalev_D 23:12e6183f04d4 364 temp = -Device_blk.Str.VBN_k;
Kovalev_D 23:12e6183f04d4 365 }
Kovalev_D 23:12e6183f04d4 366 ///Äëèòåëüíîñòü èìïóëüñà äî îøóìëåíèÿ
Kovalev_D 23:12e6183f04d4 367 Device_blk.Str.VB_tau = add(VB_tau_Ins, (mult_r( VB_tau_Ins, temp ) << 1)); // VB_tau = VB_tau_Ins + VB_tau_Ins * temp; with saturation
Kovalev_D 23:12e6183f04d4 368 Saturation(Device_blk.Str.VB_tau, Device_blk.Str.VB_Tmax, Device_blk.Str.VB_Tmin); //e. checking upper and lower levels of control range //r. ïðîâåðêà âåðõíåãî äèàïàçîíà ðåãóëèðîâàíèÿ
Kovalev_D 23:12e6183f04d4 369 Flag = !Flag;
Kovalev_D 23:12e6183f04d4 370 }
Kovalev_D 23:12e6183f04d4 371 else
Kovalev_D 23:12e6183f04d4 372 {
Kovalev_D 23:12e6183f04d4 373 PeriodCount++;
Kovalev_D 23:12e6183f04d4 374 }
Kovalev_D 23:12e6183f04d4 375
Kovalev_D 23:12e6183f04d4 376 } // clc_Noise_regulator
Kovalev_D 23:12e6183f04d4 377 /******************************************************************************
Kovalev_D 23:12e6183f04d4 378 ** Function name: VibroDither_Init
Kovalev_D 23:12e6183f04d4 379 **
Kovalev_D 23:12e6183f04d4 380 ** Descriptions: VibroDither initialization.
Kovalev_D 23:12e6183f04d4 381 **
Kovalev_D 23:12e6183f04d4 382 ** parameters: None
Kovalev_D 23:12e6183f04d4 383 ** Returned value: None
Kovalev_D 23:12e6183f04d4 384 **
Kovalev_D 23:12e6183f04d4 385 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 386 void VibroDither_Init()
Kovalev_D 23:12e6183f04d4 387 {
Kovalev_D 23:12e6183f04d4 388 LPC_SC->PCONP |= 0x00020000; //âêëþ÷åíèå ØÈÌ.
Kovalev_D 23:12e6183f04d4 389 LPC_SC->PCLKSEL1 |= 0xC0000000; //CLK=12.5MHz âûáîð ÷àñòîòû
Kovalev_D 23:12e6183f04d4 390
Kovalev_D 23:12e6183f04d4 391 /* P1.25,1.26 as PhA_vibro; P1.28,1.29 as PhB_vibro*///âûáîð íîæåê äëÿ äâóõ (òðåõ) êàíàëîâ ØÈÌ(PhA è PhB).
Kovalev_D 23:12e6183f04d4 392 // LPC_PINCON->PINSEL3 &= ~(0x3CF<<18);
Kovalev_D 23:12e6183f04d4 393 // LPC_PINCON->PINSEL3 |= (0x145 << 18) |(1<<6)|(1<<12);//P1.19 - MCOA0; P1.22 - MCOB0; P1.25 - MCOA1; P1.26 - MCOB1; P1.28 - MCOA2; P1.29 - MCOB2;
Kovalev_D 23:12e6183f04d4 394 //âûõîäû ØÈÌ (MCOA è MCOB) ðàçíîé ïîëÿðíîñòè.
Kovalev_D 23:12e6183f04d4 395
Kovalev_D 23:12e6183f04d4 396 LPC_MCPWM->CON_SET |= 1<<30; //e. set AC mode (Pha, PhB periods are set by LIM0 )
Kovalev_D 23:12e6183f04d4 397 //ÀÑ ðåæèì (3-õ ôàçíûé ÀÑ ðåæèì) âñå ØÈÌ èñïîëüçóþò
Kovalev_D 23:12e6183f04d4 398 //ñ÷åò÷èê âðåìåíè è ðåãèñòð ïåðèîä êàíàëà 0.
Kovalev_D 23:12e6183f04d4 399
Kovalev_D 23:12e6183f04d4 400 LPC_MCPWM->TC0 = 0;// èíèöèàëèçàöèÿ (îáíóëåíèå) òàéìåðà 0;
Kovalev_D 23:12e6183f04d4 401 LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //ïåðèîä ØÈÌ(Âèðîïðèâîäà).
Kovalev_D 23:12e6183f04d4 402
Kovalev_D 23:12e6183f04d4 403 LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; // set LPC_MCPWM->MAT0 for defineteness | óñòàíîâëåíèå âðåìåííûõ èíòåðâàëîâ
Kovalev_D 23:12e6183f04d4 404 LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500; // pulse width of the PhA dither drive | (MAT) ïðè äîñòèæåíèè êîòîðûõ
Kovalev_D 23:12e6183f04d4 405 LPC_MCPWM->MAT1 = ((Device_blk.Str.VB_N - Device_blk.Str.VB_tau)*MULT_7680_12500)>>SHIFT_7680_12500; // pulse width of the PhB dither drive at first time | òàéìåðîì, ÷òî òî ïðîèñõîäèò.
Kovalev_D 23:12e6183f04d4 406
Kovalev_D 23:12e6183f04d4 407 LPC_MCPWM->DT &= ~0x3FF; //e. reset dead timer register
Kovalev_D 23:12e6183f04d4 408 LPC_MCPWM->INTEN_SET = 1; //e. enable lim0 interrupt
Kovalev_D 23:12e6183f04d4 409
Kovalev_D 23:12e6183f04d4 410 LPC_MCPWM->CON_SET |= (1<<8) |1 |(1<<16); //start PWM channel 0,1,2
Kovalev_D 23:12e6183f04d4 411
Kovalev_D 23:12e6183f04d4 412 VB_tau_Ins = Device_blk.Str.VB_tau; // VB_tau_Ins - âíóòðåííåå çíà÷åíèå êîíòóðà ðåãóëèðîâàíèÿ Òàó
Kovalev_D 23:12e6183f04d4 413
Kovalev_D 23:12e6183f04d4 414 Output.Str.L_Vibro = Device_blk.Str.VB_tau; //to update the period and pulse duration for displaying
Kovalev_D 23:12e6183f04d4 415 Output.Str.T_Vibro = Device_blk.Str.VB_N; //çàïèñü â âûõîäíîé ìàñèâ äëèòåëüíîñòè è ïåðèóäà èìïóëüñîâ äëÿ îòîáðàæåíèÿ
Kovalev_D 23:12e6183f04d4 416 return;
Kovalev_D 23:12e6183f04d4 417 }
Kovalev_D 23:12e6183f04d4 418 /******************************************************************************
Kovalev_D 23:12e6183f04d4 419 ** Function name: init_Dither_reg
Kovalev_D 23:12e6183f04d4 420 **
Kovalev_D 23:12e6183f04d4 421 ** Descriptions: Initialization of dither regulator.
Kovalev_D 23:12e6183f04d4 422 **
Kovalev_D 23:12e6183f04d4 423 ** parameters: None
Kovalev_D 23:12e6183f04d4 424 ** Returned value: None
Kovalev_D 23:12e6183f04d4 425 **
Kovalev_D 23:12e6183f04d4 426 ******************************************************************************/
Kovalev_D 23:12e6183f04d4 427 void init_Dither_reg()
Kovalev_D 23:12e6183f04d4 428 {
Kovalev_D 23:12e6183f04d4 429
Kovalev_D 23:12e6183f04d4 430 init_VibroReduce(); // ðàñ÷åò êîýôèöèåíòîâ (âèáðî àïåðòóðû)
Kovalev_D 23:12e6183f04d4 431 Device_blk.Str.VB_N = 29538; //êîýôô.äåëåíèÿ N âèáðîïðèâîäà (ïåðèîä êîëåáàíèé) ÂÏ (? çàäàåòñÿ òîëüêî ñäåñü íî èñïîëüçóåòñÿ óæå ïðè âû÷èñëåíèè Vibro_Filter_Aperture â ïðåäúèäóùåé ôóíêöèè)
Kovalev_D 23:12e6183f04d4 432 VibroDither_Init();// Âûáîð íîæåê äëÿ äâóõ êàíàëîâ ØÈÌ(1-2(Êàíàë 0 òîæå îïðåäåëåí)),ïåðèîä ØÈÌ,ðåæèì è òä.
Kovalev_D 23:12e6183f04d4 433 VibroDither_SwitchOn(); //LPC_MCPWM->CON_SET = 1<<8; ñòàðò òàéìåðà 1. âûñòàâëåíèå 8 áèòà mscon_set èçìåíÿåò 8 áèò â ðåãèñòðå mscon (PDF CTP. - 526)
Kovalev_D 23:12e6183f04d4 434 init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP); //ëèíåéíûé ôèëüòð (òî æå ñàìîå ÷òî è â ÑÐÏ)ïîëîñîôîé ôèëüòð äëÿ âûäåëåíèÿ ÷àñòîòû êîëåáàíèÿ.
Kovalev_D 23:12e6183f04d4 435 MaxDelay = Vibro_Filter_Aperture >> 1; //r. ìàêñ. çàäåðæêà ìåàíäðà âèáðîïðèâîäà (Vibro_Filter_Aperture îïðåäåëÿåòñ â init_VibroReduce();)
Kovalev_D 23:12e6183f04d4 436 CounterIquiryCycle_Init((Device_blk.Str.VB_N*Vibro_2_CountIn)>>SHIFT_C_7680_12500); //çàäàíèå ïåðèîäà ñáðîñà ñ÷åò÷èêà, çàïðåò ïðåðûâàíèÿ.
Kovalev_D 23:12e6183f04d4 437 }
Kovalev_D 23:12e6183f04d4 438 /******************************************************************************
Kovalev_D 23:12e6183f04d4 439 ** End Of File
Kovalev_D 23:12e6183f04d4 440 ******************************************************************************/