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