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
 ******************************************************************************/
-