File content as of revision 23:12e6183f04d4:

#include "lpc17xx.h"
#include "mathDSP.h"
#include "InputOutput.h"
#include "CyclesSync.h"
#include "CntrlGLD.h"
#include "stdlib.h"
#include "sip.h"

//#define 	FREQ_NEW

int out_freq_sum = 0;

#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_VB_TAU_SHIFT		2

 int32_t RI_diff;		//e.input signal of "recovery" APS //r. âõîäíîé ñèãíàë "âîññòàíîâëåííîãî" ÄÓÏ
 int32_t MaxDelay;
 int32_t VB_tau_Ins;   //e. local value of the Tau regulator   //r.  âíóòðåííåå çíà÷åíèå êîíòóðà ðåãóëèðîâàíèÿ Òàó
 int32_t VB_Nmin0;  				//e. minimum of the output value of a regulator of the period for the Device_blk.Str.TemperNormal temperature //r. ìèíèìóì  âûõîäíîãî çíà÷åíèÿ ðåãóëÿòîðà ïåðèîäà äëÿ òåìïåðàòóðû Device_blk.Str.TemperNormal
 int32_t VB_Nmax0;  				//e. maximum of the output value of a regulator of the period for the Device_blk.Str.TemperNormal //r. ìàêñèìóì âûõîäíîãî çíà÷åíèÿ ðåãóëÿòîðà ïåðèîäà äëÿ òåìïåðàòóðû Device_blk.Str.TemperNormal

uint32_t In_Flag;
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 meandr1;
 int32_t temp2;
 int32_t temp3;

extern uint32_t  Vibro_2_CountIn;

void clc_Noise_regulator(void);

/******************************************************************************
** Function name:		VibroDither_Set
**
** Descriptions:		Set period and pulse width for dither.
**
** parameters:			duration of vibro pulses, period of dither
** Returned value:		None
** 
******************************************************************************/
void VibroDither_Set()
{
  Device_blk.Str.VB_N = Output.Str.T_Vibro; 
  LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;
  SwitchCntInq = 1;
}
/******************************************************************************
** Function name:		VibroDither_SwitchOn
**
** Descriptions:		VibroDither switching on.
**
** parameters:			None
** Returned value:		None
** 
******************************************************************************/
void VibroDither_SwitchOn()
{
   LPC_MCPWM->CON_SET = 1<<8;	  //start vibro dither
}
/******************************************************************************
** Function name:		VibroDither_SwitchOff
**
** Descriptions:		VibroDither switching off.
**
** parameters:			None
** Returned value:		None
** 
******************************************************************************/
void VibroDither_SwitchOff()
{
	LPC_MCPWM->CON_CLR = 1<<8;	  //stop vibro dither 
}

/******************************************************************************
** Function name:		VB_MeanderDelay
**
** Descriptions:		Routine for addition of delay to meander
**
** 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);
} // VB_MeanderDelay
/******************************************************************************
** Function name:		VB_PhaseDetectorRate
**
** Descriptions:		Routine for accumulation of dither error
**
** parameters:			None
** Returned value:		None
** 
******************************************************************************/
int VB_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime) 
{
	
	static int SampleAndHoldOut = 0, VB_PhasDet_integr = 0;
	
	if (IntegrateTime == 0)
	{
		SampleAndHoldOut = VB_PhasDet_integr;
		VB_PhasDet_integr = 0;
	}
	else
	{	
		VB_PhasDet_integr += PhaseDetInput;
	}
	return (SampleAndHoldOut);
} // VB_PhaseDetectorRate

/*r.

	DelayedDithMeander - çàäåðæàííûé ìåàíäð (íà âåëè÷èíó VB_phs)
Âûõîä
	VB_N - êîýôôèöèåíò äåëåíèÿ
*/

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;
#if defined FREQ_NEW
	static int RI_diff_old = 0;
	static int PhaseShift_Old = 0, accum_on = 0;
#endif
   RI_diff = DUP_Filt(Dif_Curr_Vib<<2);

	if (RI_diff >= 0)
	{
	 meandr1 = 1;
#if defined 	FREQ_NEW	
	  if (RI_diff_old < 0)
	  if (ph_error !=1) 					   
		ph_error++;		  
#else
	   ph_error = 1;
#endif	   			
	}
	else
	{
	meandr1 = 0;
#if !defined 	FREQ_NEW		
		ph_error = 0;
#endif			
	} 

    if (LPC_MCPWM->INTF & 0x0001) 
	{	 
	  LPC_MCPWM->INTF_CLR |= 0x0001;

	  if (LPC_MCPWM->MAT2 > LPC_MCPWM->MAT1)
	  {
//	  LPC_GPIO2->FIOSET = 0x000000FF;		// turn on the LED 
		if (SwitchCntInq) 
	 	 {
	  	 	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 = 1;
#if defined 	FREQ_NEW
		 if (ph_error > (-1))
		 ph_error--;
#endif
	  } 
	  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 = 0;
		 dith_period++; 
		 
	  }
	}
#if !defined 	FREQ_NEW
	temp3 = VB_MeanderDelay(In_Flag, Device_blk.Str.VB_phs, MaxDelay); //e. outgoing of the delayed menader signal //r. ôîðìèðîâàíèå çàäåðæàííîãî ñèãíàëà ìåàíäð
	temp2 = ( ( temp3 ^ ph_error ) << 1 ) - 1; //e. the PD XOR analog out (-1..+1, since const=1) //r. àíàëîãîâûé âûõîä XOR ÔÄ(-1..+1, ò.ê. const=1)
		accum_error += temp2; 
#else
	if ((ph_error !=0) && (accum_on == 0)) //
	{
	  accum_on = 1;
	    PhaseShift += ph_error;
	}
	else if ((ph_error == 0) && (accum_on == 1))
	{
	      accum_on = 0;
		  accum_error += (PhaseShift - PhaseShift_Old);
	      PhaseShift_Old = PhaseShift;
		  PhaseShift = 0;
	}
	else 
		 PhaseShift += ph_error;
#endif
	Output.Str.T_VB_pll = VB_PhaseDetectorRate(temp2, time_1_Sec); //e. outgoing of the integrated for 1 Sec analog signal of the PD of the dither drive //r. ôîðìèðîâàíèå ïðîèíòåãðèðîâàííîãî çà 1 ñåê àíàëîãîâîãî ñèãíàëà ÔÄ âèáðîïðèâîäà
		if ( dith_period > DITHER_REG_PERIOD ) //e. checking status of the dith_period counter  //r. ïðîâåðêà ñîñòîÿíèÿ ñ÷åò÷èêà dith_period
		{  
		    dith_period = 0; //e. 40 periods - resetting the counter of dither drive periods //r. 40 ïåðèîäîâ - îáíóëåíèå ñ÷åò÷èêà ïåðèîäîâ âèáðîïðèâîäà                       
			//e. scaling and summing with rounding and saturation //r. ìàñøòàáèðîâàíèå è ñóììèðîâàíèå ñ îêðóãëåíèåì è íàñûùåíèåì
			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);	//e. checking upper and lower levels of control range //r. ïðîâåðêà âåðõíåãî äèàïàçîíà ðåãóëèðîâàíèÿ                
			accum_error = 0;               //e. resetting the _VB_Uab40 sum //r. îáíóëåíèå ñóììû _VB_Uab40
		
	   }
	   if ( loop_is_closed(VB_FREQ_ON) ) //e. it was, check the activation of the stabilization regulator //r. ôðîíò áûë, ïðîâåðèòü âêëþ÷åí ëè êîíòóð ñòàáèëèçàöèè
        {
		    Output.Str.T_Vibro = Device_blk.Str.VB_N;          
			LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;  //e. has switched on, load calculated values of period //r. âêëþ÷åí, çàãðóçèòü âû÷èñëåííûå çíà÷åíèÿ ïåðèîäà
			SwitchCntInq = 1;  //e. enable loading counter inquiry timer at the next vibro halfperiod
	    }
	// 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

//e. dither drive output frequency stabilization //r. ñòàáèëèçàöèÿ ÷àñòîòû ðàñùåïëåíèÿ âèáðîïðèâîäà				
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;
	if (time_1_Sec == 0) //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. ñáðîñèòü íàêîïëåííûå çíà÷åíèÿ äëÿ ñëåäóþùåãî öèêëà èçìåðåíèÿ

		// 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) ) //e.  is stabilization regulator switched on?  //r. êîíòóð ñòàáèëèçàöèè âêëþ÷åí?
	{
		Output.Str.L_Vibro = Device_blk.Str.VB_tau; //e. otherwise, load new value of //r. èíà÷å çàãðóçèòü íîâîå çíà÷åíèå
        						//e. pulse width of the dither drive //r. äëèòåëüíîñòè èìïóëüñîâ âèáðîïðèâîäà
	}
	
} // clc_OutFreq_regulator

//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) - äëèòåëüíîñòü èìïóëüñà îäíîâèáðàòîðà, çàäàâàåìàÿ ñèñòåìîé ðåãóëèðîâêè ÷àñòîòû ðàñùåïëåíèÿ.
*/

void clc_Noise_regulator(void)
{
	int temp;
	static uint32_t Flag = 0;
	static int PeriodCount = 0, Tnoise = 0;

	if ( PeriodCount >= Tnoise )
	{
		PeriodCount = 0;
		srand(Device_blk.Str.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;
		}
		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
**
** Descriptions:		VibroDither initialization.
**
** parameters:			None
** Returned value:		None
** 
******************************************************************************/
void VibroDither_Init()
{
  LPC_SC->PCONP |= 0x00020000;		   /* Turn On MCPWM PCLK */
  LPC_SC->PCLKSEL1 |= 0xC0000000;	   //CLK=12.5MHz
  /* P1.25,1.26 as PhA_vibro; P1.28,1.29 as PhB_vibro*/
  LPC_PINCON->PINSEL3 &= ~(0x3CF<<18); 											  
  LPC_PINCON->PINSEL3 |= (0x145 << 18) |(1<<6)|(1<<12);

  LPC_MCPWM->CON_SET |= 1<<30; //e. set AC mode (Pha, PhB periods are set by LIM0 )

  LPC_MCPWM->TC0 = 0;	//e. initial time counter of channel 0
  LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //e. period of the dither drive 
  LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500;	//e. set LPC_MCPWM->MAT0  for defineteness  						
  LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500;	//e. pulse width of the PhA dither drive 
  LPC_MCPWM->MAT1 = ((Device_blk.Str.VB_N - Device_blk.Str.VB_tau)*MULT_7680_12500)>>SHIFT_7680_12500;	//e. 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->CON_SET |= (1<<8) |1 |(1<<16);	  //start PWM channel 0,1,2

  VB_tau_Ins = Device_blk.Str.VB_tau;
  return;
}
/******************************************************************************
** Function name:		init_Dither_reg
**
** Descriptions:		Initialization of dither regulator.
**
** parameters:			None
** Returned value:		None
** 
******************************************************************************/
void init_Dither_reg()
{
  init_VibroReduce();
  VibroDither_Init();
  VibroDither_SwitchOn();
  init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP);	
  MaxDelay = Vibro_Filter_Aperture >> 1; //e. maximal delay of the meander of the dither drive //r. ìàêñ. çàäåðæêà ìåàíäðà âèáðîïðèâîäà
  CounterIquiryCycle_Init((Device_blk.Str.VB_N*Vibro_2_CountIn)>>SHIFT_C_7680_12500);  
}
/******************************************************************************
**                            End Of File
******************************************************************************/