123
Fork of LG by
Diff: Dither_Reg.c
- Revision:
- 21:bc8c1cec3da6
- Parent:
- 1:f2adcae3d304
- Child:
- 112:4a96133a1311
diff -r 18e3fd7b92d0 -r bc8c1cec3da6 Dither_Reg.c --- a/Dither_Reg.c Tue Feb 02 17:14:25 2016 +0000 +++ b/Dither_Reg.c Wed Feb 03 07:19:30 2016 +0000 @@ -4,30 +4,30 @@ #define SHIFT_7680_12500 15 //e. 14 digits for 7680 to 12500 clock converting and 1 division digit #define SHIFT_C_7680_12500 11 // -#define DITH_VBN_SHIFT 2 //e. //r. ���������� ����� (������� �� 4) ������������ ������� ������������, ����� ����� ����� �� ������������� +#define DITH_VBN_SHIFT 2 //e. //r. определяет сдвиг (деление на 4) коэффициента деления вибропривода, чтобы иметь запас на регулирование #define DITH_VB_TAU_SHIFT 2 - int32_t RI_diff; //e.input signal of "recovery" APS //r. ������� ������ "����������������" ��� - int32_t MaxDelay; - int32_t VB_tau_Ins; //r. ���������� �������� ������� ������������� ��� - int32_t VB_Nmin0; //r. ������� ��������� �������� ���������� ������� ��� ����������� Device_blk.Str.TemperNormal - int32_t VB_Nmax0; //r. �������� ��������� �������� ���������� ������� ��� ����������� Device_blk.Str.TemperNormal +int32_t RI_diff; //e.input signal of "recovery" APS //r. входной сигнал "восстановленного" ДУП +int32_t MaxDelay; +int32_t VB_tau_Ins; //r. внутреннее значение контура регулирования Тау +int32_t VB_Nmin0; //r. минимум выходного значения регулятора периода для температуры Device_blk.Str.TemperNormal +int32_t VB_Nmax0; //r. максимум выходного значения регулятора периода для температуры Device_blk.Str.TemperNormal uint32_t In_Flag = 0; uint32_t SwitchCntInq = 0; - int32_t accum_error = 0; - int32_t ph_error = 0; - int32_t accum_error_old = 0; - int32_t PhaseShift; - int32_t temp2; - int32_t temp3; +int32_t accum_error = 0; +int32_t ph_error = 0; +int32_t accum_error_old = 0; +int32_t PhaseShift; +int32_t temp2; +int32_t temp3; #if defined DITHERSIM - int32_t timeDither = 0; - int32_t LIM0; +int32_t timeDither = 0; +int32_t LIM0; #endif extern uint32_t Vibro_2_CountIn; void clc_Noise_regulator(void); @@ -39,20 +39,20 @@ ** ** parameters: duration of vibro pulses, period of dither ** Returned value: None -** +** ******************************************************************************/ void VibroDither_Set() -{ -//�����.������� N ������������ (������ ���������) �� = T_Vibro ������������ �������� ������������>> - +{ +//коэфф.деления N вибропривода (период колебаний) ВП = T_Vibro длительность импульса вибропривода>> + - Device_blk.Str.VB_N = Output.Str.T_Vibro; - LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;//#define SHIFT_7680_12500 15 �������� ��� ����������� ������� �� 7680 � 12500 + Device_blk.Str.VB_N = Output.Str.T_Vibro; + LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;//#define SHIFT_7680_12500 15 смешение для конвертации частоты из 7680 в 12500 #if defined DITHERSIM - 6565 LIM0 = (Output.Str.T_Vibro*86)>>16; + 6565 LIM0 = (Output.Str.T_Vibro*86)>>16; #endif #if !defined CONSTCYCLE - 5655 SwitchCntInq = 1; //to enable inquiry timer reloading + 5655 SwitchCntInq = 1; //to enable inquiry timer reloading #endif } /****************************************************************************** @@ -62,11 +62,11 @@ ** ** parameters: None ** Returned value: None -** +** ******************************************************************************/ void VibroDither_SwitchOn() { - LPC_MCPWM->CON_SET = 1<<8; //start vibro dither + LPC_MCPWM->CON_SET = 1<<8; //start vibro dither } /****************************************************************************** ** Function name: VibroDither_SwitchOff @@ -75,11 +75,11 @@ ** ** parameters: None ** Returned value: None -** +** ******************************************************************************/ void VibroDither_SwitchOff() { - LPC_MCPWM->CON_CLR = 1<<8; //stop vibro dither + LPC_MCPWM->CON_CLR = 1<<8; //stop vibro dither } /****************************************************************************** @@ -89,51 +89,53 @@ ** ** parameters: meander, delay magnitude, max delay ** Returned value: delayed meander -** +** ******************************************************************************/ int VB_MeanderDelay(int VB_Meander, int Delay100uS, int MaxDly) { - static int poz_counter = 0, neg_counter = 0, flg_delay; - - if (Delay100uS == 0) - { - return (VB_Meander); - } - - if (Delay100uS > 0) - { - if (Delay100uS > MaxDly) { Delay100uS = MaxDly; } - if (VB_Meander) //e. outgoing WP_flg flag, which delayed by the WP_ref //r. ������������ ������������ �� �������� WP_ref ����� poz_sin_flag - { - neg_counter = 0; - poz_counter++; - } - else - { - poz_counter = 0; - neg_counter++; - } - if (poz_counter == Delay100uS) { flg_delay = 1; } - if (neg_counter == Delay100uS) { flg_delay = 0; } - } - else - { - Delay100uS = -Delay100uS; - if (Delay100uS > MaxDly) { Delay100uS = MaxDly; } - if (VB_Meander) //e. outgoing WP_flg flag, which delayed by the WP_ref //r. ������������ ������������ �� �������� WP_ref ����� poz_sin_flag - { - neg_counter = MaxDly + 1; - poz_counter--; - } - else - { - poz_counter = MaxDly + 1; - neg_counter--; - } - if (poz_counter == Delay100uS) { flg_delay = 0; } - if (neg_counter == Delay100uS) { flg_delay = 1; } - } - return (flg_delay); + static int poz_counter = 0, neg_counter = 0, flg_delay; + + if (Delay100uS == 0) { + return (VB_Meander); + } + + if (Delay100uS > 0) { + if (Delay100uS > MaxDly) { + Delay100uS = MaxDly; + } + if (VB_Meander) { //e. outgoing WP_flg flag, which delayed by the WP_ref //r. формирование задержанного на величину WP_ref флага poz_sin_flag + neg_counter = 0; + poz_counter++; + } else { + poz_counter = 0; + neg_counter++; + } + if (poz_counter == Delay100uS) { + flg_delay = 1; + } + if (neg_counter == Delay100uS) { + flg_delay = 0; + } + } else { + Delay100uS = -Delay100uS; + if (Delay100uS > MaxDly) { + Delay100uS = MaxDly; + } + if (VB_Meander) { //e. outgoing WP_flg flag, which delayed by the WP_ref //r. формирование задержанного на величину WP_ref флага poz_sin_flag + neg_counter = MaxDly + 1; + poz_counter--; + } else { + poz_counter = MaxDly + 1; + neg_counter--; + } + if (poz_counter == Delay100uS) { + flg_delay = 0; + } + if (neg_counter == Delay100uS) { + flg_delay = 1; + } + } + return (flg_delay); } // VB_MeanderDelay /****************************************************************************** ** Function name: VB_PhaseDetectorRate @@ -142,30 +144,27 @@ ** ** parameters: None ** Returned value: None -** +** ******************************************************************************/ -int VB_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime) +int VB_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime) { - - static int SampleAndHoldOut = 0, VB_PhasDet_integr = 0; - - if (IntegrateTime == DEVICE_SAMPLE_RATE_uks) - { - SampleAndHoldOut = VB_PhasDet_integr; - VB_PhasDet_integr = 0; - } - else - { - VB_PhasDet_integr += PhaseDetInput; - } - return (SampleAndHoldOut); + + static int SampleAndHoldOut = 0, VB_PhasDet_integr = 0; + + if (IntegrateTime == DEVICE_SAMPLE_RATE_uks) { + SampleAndHoldOut = VB_PhasDet_integr; + VB_PhasDet_integr = 0; + } else { + VB_PhasDet_integr += PhaseDetInput; + } + return (SampleAndHoldOut); } // VB_PhaseDetectorRate /*r. - DelayedDithMeander - ����������� ������ (�� �������� VB_phs) -����� - VB_N - ����������� ������� + DelayedDithMeander - задержанный меандр (на величину VB_phs) +Выход + VB_N - коэффициент деления */ /****************************************************************************** ** Function name: clc_Dith_regulator @@ -174,75 +173,66 @@ ** ** parameters: None ** Returned value: None -** +** ******************************************************************************/ void clc_Dith_regulator(void) -{ +{ // static int smooth=0, buf[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, i = 0; // int ph_error; - static int dith_period = 0;//, accum_error = 0; + static int dith_period = 0;//, accum_error = 0; - RI_diff = DUP_Filt(Dif_Curr_Vib<<2); + RI_diff = DUP_Filt(Dif_Curr_Vib<<2); - if (RI_diff >= 0) - ph_error = 1; - else - ph_error = 0; + if (RI_diff >= 0) + ph_error = 1; + else + ph_error = 0; - if (LPC_MCPWM->INTF & 0x0001) //vibro pulse has been formed - { - LPC_MCPWM->INTF_CLR |= 0x0001; - if (LPC_MCPWM->MAT2 > LPC_MCPWM->MAT1) - { -// LPC_GPIO2->FIOSET = 0x000000FF; // turn on the LED - if (SwitchCntInq) //inquiry cycle duration must be changed - { - LPC_PWM1->MR0 = (Output.Str.T_Vibro*Vibro_2_CountIn)>>SHIFT_C_7680_12500; - LPC_PWM1->LER = LER0_EN ; //e. enable updating of register - SwitchCntInq = 0; - } - LPC_MCPWM->MAT1 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; - LPC_MCPWM->MAT2 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500; - In_Flag = 0; - } - else - { -// LPC_GPIO2->FIOCLR = 0x000000FF; // turn off the LED - LPC_MCPWM->MAT2 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; - LPC_MCPWM->MAT1 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500; - In_Flag = 1; - dith_period++; - } - } + if (LPC_MCPWM->INTF & 0x0001) { //vibro pulse has been formed + LPC_MCPWM->INTF_CLR |= 0x0001; + if (LPC_MCPWM->MAT2 > LPC_MCPWM->MAT1) { +// LPC_GPIO2->FIOSET = 0x000000FF; // turn on the LED + if (SwitchCntInq) { //inquiry cycle duration must be changed + LPC_PWM1->MR0 = (Output.Str.T_Vibro*Vibro_2_CountIn)>>SHIFT_C_7680_12500; + LPC_PWM1->LER = LER0_EN ; //e. enable updating of register + SwitchCntInq = 0; + } + LPC_MCPWM->MAT1 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; + LPC_MCPWM->MAT2 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500; + In_Flag = 0; + } else { +// LPC_GPIO2->FIOCLR = 0x000000FF; // turn off the LED + LPC_MCPWM->MAT2 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; + LPC_MCPWM->MAT1 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500; + In_Flag = 1; + dith_period++; + } + } - temp3 = VB_MeanderDelay(In_Flag, Device_blk.Str.VB_phs, MaxDelay); //r. ������������ ������������ ������� ������ - temp2 = ( ( temp3 ^ ph_error ) << 1 ) - 1; //r. ���������� ����� XOR ��(-1..+1, �.�. const=1) - accum_error += temp2; + temp3 = VB_MeanderDelay(In_Flag, Device_blk.Str.VB_phs, MaxDelay); //r. формирование задержанного сигнала меандр + temp2 = ( ( temp3 ^ ph_error ) << 1 ) - 1; //r. аналоговый выход XOR ФД(-1..+1, т.к. const=1) + accum_error += temp2; + + Output.Str.T_VB_pll = VB_PhaseDetectorRate(temp2, time_1_Sec); //r. формирование проинтегрированного за 1 сек аналогового сигнала ФД вибропривода + if ( dith_period > DITHER_REG_PERIOD ) { //r. проверка состояния счетчика dith_period + dith_period = 0; //r. 40 периодов - обнуление счетчика периодов вибропривода + //r. масштабирование и суммирование с округлением и насыщением + if ( loop_is_closed(VB_FREQ_ON) ) { + 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; - Output.Str.T_VB_pll = VB_PhaseDetectorRate(temp2, time_1_Sec); //r. ������������ ������������������� �� 1 ��� ����������� ������� �� ������������ - if ( dith_period > DITHER_REG_PERIOD ) //r. �������� ��������� �������� dith_period - { - dith_period = 0; //r. 40 �������� - ��������� �������� �������� ������������ - //r. ��������������� � ������������ � ����������� � ���������� - if ( loop_is_closed(VB_FREQ_ON) ) - { - 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; - - Saturation(Device_blk.Str.VB_N, Device_blk.Str.VB_Nmax, Device_blk.Str.VB_Nmin); //r. �������� �������� ��������� ������������� - accum_error = 0; //r. ��������� ����� _VB_Uab40 - } - } + Saturation(Device_blk.Str.VB_N, Device_blk.Str.VB_Nmax, Device_blk.Str.VB_Nmin); //r. проверка верхнего диапазона регулирования + accum_error = 0; //r. обнуление суммы _VB_Uab40 + } + } - if ( loop_is_closed(VB_FREQ_ON) ) //r. ����� ���, ��������� ������� �� ������ ������������ - { - Output.Str.T_Vibro = Device_blk.Str.VB_N; - LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; //r. �������, ��������� ����������� �������� ������� - } - // cyclic built-in test - if ((Output.Str.T_Vibro > Device_blk.Str.VB_Nmax) || (Output.Str.T_Vibro < Device_blk.Str.VB_Nmin)) - { - Valid_Data |= DITH_FREQ_ERROR; - } + if ( loop_is_closed(VB_FREQ_ON) ) { //r. фронт был, проверить включен ли контур стабилизации + Output.Str.T_Vibro = Device_blk.Str.VB_N; + LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; //r. включен, загрузить вычисленные значения периода + } + // cyclic built-in test + if ((Output.Str.T_Vibro > Device_blk.Str.VB_Nmax) || (Output.Str.T_Vibro < Device_blk.Str.VB_Nmin)) { + Valid_Data |= DITH_FREQ_ERROR; + } } // clc_Dith_regulator /****************************************************************************** @@ -252,121 +242,109 @@ ** ** parameters: None ** Returned value: None -** -******************************************************************************/ +** +******************************************************************************/ void clc_OutFreq_regulator(void) { - static int out_freq_sum = 0; - static int temp; -/* - if (Dif_Curr_Vib > 0) //e. angular speed > 0 //r.�������� ������������ - { - if (RI_diff > 0) - out_freq_sum += (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); - else - out_freq_sum -= (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); - } - else //e. angular speed < 0 //r.�������� ������������ - { - if (RI_diff < 0) - out_freq_sum += (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); - else - out_freq_sum -= (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); - } -*/ - if(Dif_Curr_Vib>0) - out_freq_sum += Dif_Curr_Vib; - else - out_freq_sum -= Dif_Curr_Vib; + static int out_freq_sum = 0; + static int temp; + /* + if (Dif_Curr_Vib > 0) //e. angular speed > 0 //r.скорость положительна + { + if (RI_diff > 0) + out_freq_sum += (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); + else + out_freq_sum -= (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); + } + else //e. angular speed < 0 //r.скорость отрицательна + { + if (RI_diff < 0) + out_freq_sum += (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); + else + out_freq_sum -= (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); + } + */ + if(Dif_Curr_Vib>0) + out_freq_sum += Dif_Curr_Vib; + else + out_freq_sum -= Dif_Curr_Vib; + - - if (time_1_Sec == DEVICE_SAMPLE_RATE_uks) //e. second has elapsed, fix the output frequency value //r. ������� ������, ������������� �������� ������� ����������� - { - if (loop_is_closed(VB_TAU_ON)) //e. the regulator loop is closed //r. ������ ������� - { - temp = Device_blk.Str.VB_Fdf_Hi << 16; - temp |= Device_blk.Str.VB_Fdf_Lo; - temp = L_sub(out_freq_sum, temp) >> 3; // (out_freq_sum - temp) with saturation, then >> 3 - Saturation(temp, 32767, -32768); // error saturation if error is out of range - //e. scaling and summing with rounding and saturation //r. ��������������� � ������������ � ����������� � ���������� - VB_tau_Ins = mac_r( VB_tau_Ins << (16 - DITH_VB_TAU_SHIFT), - temp, - Device_blk.Str.VB_Fsc ); // << DITH_VB_TAU_SHIFT; - //e. reduction the VB_Err value to 16 digits (arithmetic right shift to 3 digits) //r. �������� �������� VB_Err � 16 �������� (�������������� ����� ������ �� 3 �������) - - Saturation(VB_tau_Ins, \ - (int)Device_blk.Str.VB_Tmax >> DITH_VB_TAU_SHIFT, \ - (int)Device_blk.Str.VB_Tmin >> DITH_VB_TAU_SHIFT); //e. checking upper and lower levels in sign range - VB_tau_Ins <<= DITH_VB_TAU_SHIFT; - } - - 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, ����� ������� � ������������ - out_freq_sum = 0; //e. reset accumulated values for next cycle of measurement //r. �������� ����������� �������� ��� ���������� ����� ��������� + if (time_1_Sec == DEVICE_SAMPLE_RATE_uks) { //e. second has elapsed, fix the output frequency value //r. секунда прошла, зафиксировать значение частоты расщепления + if (loop_is_closed(VB_TAU_ON)) { //e. the regulator loop is closed //r. контур замкнут + temp = Device_blk.Str.VB_Fdf_Hi << 16; + temp |= Device_blk.Str.VB_Fdf_Lo; + temp = L_sub(out_freq_sum, temp) >> 3; // (out_freq_sum - temp) with saturation, then >> 3 + Saturation(temp, 32767, -32768); // error saturation if error is out of range + //e. scaling and summing with rounding and saturation //r. масштабирование и суммирование с округлением и насыщением + VB_tau_Ins = mac_r( VB_tau_Ins << (16 - DITH_VB_TAU_SHIFT), + temp, + Device_blk.Str.VB_Fsc ); // << DITH_VB_TAU_SHIFT; + //e. reduction the VB_Err value to 16 digits (arithmetic right shift to 3 digits) //r. сведение величины VB_Err к 16 разрядам (арифметический сдвиг вправо на 3 разряда) + + Saturation(VB_tau_Ins, \ + (int)Device_blk.Str.VB_Tmax >> DITH_VB_TAU_SHIFT, \ + (int)Device_blk.Str.VB_Tmin >> DITH_VB_TAU_SHIFT); //e. checking upper and lower levels in sign range + VB_tau_Ins <<= DITH_VB_TAU_SHIFT; + } - // cyclic built-in test - // if output frequency is less than 3/4 of nominal then data is invalid - if (Output.Str.F_ras < ((temp >> 7)*3)) - { - Valid_Data |= OUT_FREQ_ERROR; - } - else - { - Valid_Data &= ~OUT_FREQ_ERROR; - } - } + 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, чтобы совпало с частотомером + out_freq_sum = 0; //e. reset accumulated values for next cycle of measurement //r. сбросить накопленные значения для следующего цикла измерения - clc_Noise_regulator(); - - if ( loop_is_closed(VB_TAU_ON) ) //r. ������ ������������ �������? - { - Output.Str.L_Vibro = Device_blk.Str.VB_tau; //r. ����� ��������� ����� �������� - //r. ������������ ��������� ������������ - } - + // cyclic built-in test + // if output frequency is less than 3/4 of nominal then data is invalid + if (Output.Str.F_ras < ((temp >> 7)*3)) { + Valid_Data |= OUT_FREQ_ERROR; + } else { + Valid_Data &= ~OUT_FREQ_ERROR; + } + } + + clc_Noise_regulator(); + + if ( loop_is_closed(VB_TAU_ON) ) { //r. контур стабилизации включен? + Output.Str.L_Vibro = Device_blk.Str.VB_tau; //r. иначе загрузить новое значение + //r. длительности импульсов вибропривода + } + } // clc_OutFreq_regulator -//e. noise regulator //r. ������� ������������ ��������� ������������ +//e. noise regulator //r. система электронного ошумления вибропривода /*r. -PeriodCount (VBN_Cnt) - ������� �������� ������� Meander. -Tnoise (VBN_Per)- ������� ������ ���������. -PeriodNoise (VBN_Tzd) - ������� ������ ���������, �������� �������������. -AmpNoise(VBN_Ran) - ������������ ��������� ������� ��������� (�������� �������������). -Delta (VBN_k) - ������� ��������� (�������� �������������). -Flag(VBN_Mod) - ���� ����� ��������� ���������. -Tu(VBN_Tau) - ������������ �������� �������������. -Tp(VBN_tau_Ins) - ������������ �������� �������������, ���������� �������� ����������� ������� �����������. +PeriodCount (VBN_Cnt) - счетчик периодов сигнала Meander. +Tnoise (VBN_Per)- текущий период ошумления. +PeriodNoise (VBN_Tzd) - средний период ошумления, заданный пользователем. +AmpNoise(VBN_Ran) - максимальная амплитуда периода ошумления (задается пользователем). +Delta (VBN_k) - глубина ошумления (задается пользователем). +Flag(VBN_Mod) - флаг знака изменения амплитуды. +Tu(VBN_Tau) - длительность импульса одновибратора. +Tp(VBN_tau_Ins) - длительность импульса одновибратора, задаваемая системой регулировки частоты расщепления. */ void clc_Noise_regulator(void) { - int temp; - static uint32_t Flag = 0; - static int PeriodCount = 0, Tnoise = 0; + int temp; + static uint32_t Flag = 0; + static int PeriodCount = 0, Tnoise = 0; - if ( PeriodCount >= Tnoise ) - { - PeriodCount = 0; - srand(Device_blk.Str.VB_N);// Srand(������ ��������� ��) -������������� ���������� ��������� ����� � ������ (VB_N) - //�������� ������ ��������� - 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() ); - if ( Flag ) //e. calculation +dF/-dF //r. ������ +dF/-dF - { - temp = Device_blk.Str.VBN_k; //r. 25 - �������� ��������� ��������� - } - else - { - temp = -Device_blk.Str.VBN_k; - } - ///������������ �������� �� ��������� - 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 - 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. �������� �������� ��������� ������������� - Flag = !Flag; - } - else - { - PeriodCount++; - } - + if ( PeriodCount >= Tnoise ) { + PeriodCount = 0; + srand(Device_blk.Str.VB_N);// Srand(период колебаний ВП) -инициализация генератора случайных чисел с зерном (VB_N) + //заданный период ошумления + 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() ); + if ( Flag ) { //e. calculation +dF/-dF //r. расчет +dF/-dF + temp = Device_blk.Str.VBN_k; //r. 25 - заданная константа ошумления + } else { + temp = -Device_blk.Str.VBN_k; + } +///Длительность импульса до ошумления + 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 + 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. проверка верхнего диапазона регулирования + Flag = !Flag; + } else { + PeriodCount++; + } + } // clc_Noise_regulator /****************************************************************************** ** Function name: VibroDither_Init @@ -375,39 +353,39 @@ ** ** parameters: None ** Returned value: None -** +** ******************************************************************************/ void VibroDither_Init() { - LPC_SC->PCONP |= 0x00020000; //��������� ���. - LPC_SC->PCLKSEL1 |= 0xC0000000; //CLK=12.5MHz ����� ������� - - /* P1.25,1.26 as PhA_vibro; P1.28,1.29 as PhB_vibro*///����� ����� ��� ���� (����) ������� ���(PhA � PhB). - // LPC_PINCON->PINSEL3 &= ~(0x3CF<<18); - // 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; - //������ ��� (MCOA � MCOB) ������ ����������. - - LPC_MCPWM->CON_SET |= 1<<30; //e. set AC mode (Pha, PhB periods are set by LIM0 ) - //�� ����� (3-� ������ �� �����) ��� ��� ���������� - //������� ������� � ������� ������ ������ 0. + LPC_SC->PCONP |= 0x00020000; //включение ШИМ. + LPC_SC->PCLKSEL1 |= 0xC0000000; //CLK=12.5MHz выбор частоты - LPC_MCPWM->TC0 = 0;// ������������� (���������) ������� 0; - LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //������ ���(�����������). + /* P1.25,1.26 as PhA_vibro; P1.28,1.29 as PhB_vibro*///выбор ножек для двух (трех) каналов ШИМ(PhA и PhB). +// LPC_PINCON->PINSEL3 &= ~(0x3CF<<18); +// 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; + //выходы ШИМ (MCOA и MCOB) разной полярности. + + LPC_MCPWM->CON_SET |= 1<<30; //e. set AC mode (Pha, PhB periods are set by LIM0 ) + //АС режим (3-х фазный АС режим) все ШИМ используют + //счетчик времени и регистр период канала 0. + + LPC_MCPWM->TC0 = 0;// инициализация (обнуление) таймера 0; + LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //период ШИМ(Виропривода). - LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; // set LPC_MCPWM->MAT0 for defineteness | ������������ ��������� ���������� - LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500; // pulse width of the PhA dither drive | (MAT) ��� ���������� ������� - 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 | ��������, ��� �� ����������. + LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; // set LPC_MCPWM->MAT0 for defineteness | установление временных интервалов + LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500; // pulse width of the PhA dither drive | (MAT) при достижении которых + 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 | таймером, что то происходит. - LPC_MCPWM->DT &= ~0x3FF; //e. reset dead timer register - LPC_MCPWM->INTEN_SET = 1; //e. enable lim0 interrupt + LPC_MCPWM->DT &= ~0x3FF; //e. reset dead timer register + LPC_MCPWM->INTEN_SET = 1; //e. enable lim0 interrupt - LPC_MCPWM->CON_SET |= (1<<8) |1 |(1<<16); //start PWM channel 0,1,2 + LPC_MCPWM->CON_SET |= (1<<8) |1 |(1<<16); //start PWM channel 0,1,2 - VB_tau_Ins = Device_blk.Str.VB_tau; // VB_tau_Ins - ���������� �������� ������� ������������� ��� + VB_tau_Ins = Device_blk.Str.VB_tau; // VB_tau_Ins - внутреннее значение контура регулирования Тау - Output.Str.L_Vibro = Device_blk.Str.VB_tau; //to update the period and pulse duration for displaying - Output.Str.T_Vibro = Device_blk.Str.VB_N; //������ � �������� ����� ������������ � ������� ��������� ��� ����������� - return; + Output.Str.L_Vibro = Device_blk.Str.VB_tau; //to update the period and pulse duration for displaying + Output.Str.T_Vibro = Device_blk.Str.VB_N; //запись в выходной масив длительности и периуда импульсов для отображения + return; } /****************************************************************************** ** Function name: init_Dither_reg @@ -416,20 +394,19 @@ ** ** parameters: None ** Returned value: None -** +** ******************************************************************************/ void init_Dither_reg() { - - init_VibroReduce(); // ������ ������������ (����� ��������) - Device_blk.Str.VB_N = 29538; //�����.������� N ������������ (������ ���������) �� (? �������� ������ ����� �� ������������ ��� ��� ���������� Vibro_Filter_Aperture � ����������� �������) - VibroDither_Init();// ����� ����� ��� ���� ������� ���(1-2(����� 0 ���� ���������)),������ ���,����� � ��. - VibroDither_SwitchOn(); //LPC_MCPWM->CON_SET = 1<<8; ����� ������� 1. ����������� 8 ���� mscon_set �������� 8 ��� � �������� mscon (PDF CTP. - 526) - init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP); //�������� ������ (�� �� ����� ��� � � ���)��������� ������ ��� ��������� ������� ���������. - MaxDelay = Vibro_Filter_Aperture >> 1; //r. ����. �������� ������� ������������ (Vibro_Filter_Aperture ����������� � init_VibroReduce();) - CounterIquiryCycle_Init((Device_blk.Str.VB_N*Vibro_2_CountIn)>>SHIFT_C_7680_12500); //������� ������� ������ ��������, ������ ����������. + + init_VibroReduce(); // расчет коэфициентов (вибро апертуры) + Device_blk.Str.VB_N = 29538; //коэфф.деления N вибропривода (период колебаний) ВП (? задается только сдесь но используется уже при вычислении Vibro_Filter_Aperture в предъидущей функции) + VibroDither_Init();// Выбор ножек для двух каналов ШИМ(1-2(Канал 0 тоже определен)),период ШИМ,режим и тд. + VibroDither_SwitchOn(); //LPC_MCPWM->CON_SET = 1<<8; старт таймера 1. выставление 8 бита mscon_set изменяет 8 бит в регистре mscon (PDF CTP. - 526) + init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP); //линейный фильтр (то же самое что и в СРП)полософой фильтр для выделения частоты колебания. + MaxDelay = Vibro_Filter_Aperture >> 1; //r. макс. задержка меандра вибропривода (Vibro_Filter_Aperture определяетс в init_VibroReduce();) + CounterIquiryCycle_Init((Device_blk.Str.VB_N*Vibro_2_CountIn)>>SHIFT_C_7680_12500); //задание периода сброса счетчика, запрет прерывания. } /****************************************************************************** ** End Of File ******************************************************************************/ -