igor Apu / Mbed 2 deprecated LG Featured

123

Dependencies:   mbed

Fork of LG by igor Apu

Dither_Reg.c@198:78dd6d14d108, 2017-02-08 (annotated)

Committer:
Diletant
Date:
Wed Feb 08 06:33:40 2017 +0000
Revision:
198:78dd6d14d108
Parent:
197:7a05523bf588 
DeviceXXX.XXX files moved to: https://developer.mbed.org/users/Diletant/code/H4/

Who changed what in which revision?

UserRevisionLine numberNew 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
Kovalev_D 129:406995a91322 49 // Device_blk.Str.VB_N = Output.Str.T_Vibro;
Kovalev_D 197:7a05523bf588 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 {
Kovalev_D 129:406995a91322 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;
Kovalev_D 129:406995a91322 332 // srand(Device_blk.Str.VB_N);// Srand(период колебаний ВП) -инициализация генератора случайных чисел с зерном (VB_N)
igor_v 21:bc8c1cec3da6 333 //заданный период ошумления
Kovalev_D 129:406995a91322 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
Kovalev_D 129:406995a91322 336 // temp = Device_blk.Str.VBN_k; //r. 25 - заданная константа ошумления
igor_v 21:bc8c1cec3da6 337 } else {
Kovalev_D 129:406995a91322 338 // temp = -Device_blk.Str.VBN_k;
igor_v 21:bc8c1cec3da6 339 }
igor_v 21:bc8c1cec3da6 340 ///Длительность импульса до ошумления
Kovalev_D 129:406995a91322 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++;
Kovalev_D 129:406995a91322 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;
Kovalev_D 129:406995a91322 373 // LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //период ШИМ(Виропривода).
igor_v 0:8ad47e2b6f00 374
Kovalev_D 129:406995a91322 375 //LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; // set LPC_MCPWM->MAT0 for defineteness | установление временных интервалов
Kovalev_D 129:406995a91322 376 // LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500; // pulse width of the PhA dither drive | (MAT) при достижении которых
Kovalev_D 129:406995a91322 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
Kovalev_D 129:406995a91322 384 // VB_tau_Ins = Device_blk.Str.VB_tau; // VB_tau_Ins - внутреннее значение контура регулирования Тау
igor_v 0:8ad47e2b6f00 385
Kovalev_D 129:406995a91322 386 // Output.Str.L_Vibro = Device_blk.Str.VB_tau; //to update the period and pulse duration for displaying
Kovalev_D 129:406995a91322 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
Kovalev_D 197:7a05523bf588 402 // init_VibroReduce(); // расчет коэфициентов (вибро апертуры)
Kovalev_D 129:406995a91322 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)
Kovalev_D 197:7a05523bf588 406 // init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP); //линейный фильтр (то же самое что и в СРП)полософой фильтр для выделения частоты колебания.
Kovalev_D 197:7a05523bf588 407 // MaxDelay = Vibro_Filter_Aperture >> 1; //r. макс. задержка меандра вибропривода (Vibro_Filter_Aperture определяетс в init_VibroReduce();)
Kovalev_D 129:406995a91322 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 ******************************************************************************/