File content as of revision 0:8ad47e2b6f00:

#include "mathDSP.h"
#include "SIP.h"
#include "CyclesSync.h"
#include "CntrlGLD.h"
#include "InputOutput.h"
#include "ThermoCalc.h"
#include "el_lin.h"


  		 uint32_t	 Old_Cnt_Vib = 0;
    	 uint32_t	 Old_Cnt = 0;
		   int32_t	 RefMeand_Cnt_Dif;
     	 int32_t	 PSdif_sum_Vib_32 = 0;
       __int64	 PSdif_sum_Vib_64 = 0;
		   int32_t	 dif_Curr_32_Ext; 		//r. ðàçíîñòü (÷èñëî) äëÿ ðåæèìà âíåøíåé çàùåëêè
     	 int32_t	 dif_Curr_32_previous; 	//e. Previous (in comparison with Dif_Curr_32) number //r. ïðåäûäóùåå (ïî ñðàâíåíèþ ñ Dif_Curr_32) ÷èñëî
		   int32_t	temp22=0;
//+++++++++++++++++++++++++++++++INPUT DATA++++++++++++++++++++++++++++++++++++++++++++++++
uint32_t	Curr_Cnt_Vib;
uint32_t 	Cnt_curr;

//+++++++++++++++++++++++++++++++variables for output++++++++++++++++++++++++++++++++++++++
int32_t	 	Dif_Curr_Vib;	    //e. current difference output for dithering control in LightUp mode and Dither regulator
int32_t	    Dif_Curr_32;		//e. current difference without dithering for dithering control

#if (defined GLOBALRATE)//íå âõîäèò

//variables for rate mode
    int32_t	 cntPls_sum_32;
    ñññ int32_t	 last_Cnt_Plus;
     int32_t	 dif_sum_32;
     int32_t	 Cnt_Pls;
     int32_t	 Cnt_Mns;
     int32_t	 preLast_Cnt_Plus;
    // uint32_t   sumCnt_Mns = 0;	   
     //uint32_t   sumCnt_Pls = 0;
  extern int32_t dif_cur_test[30];
  extern   unsigned ii;
     uint32_t halfPeriod = 0;
#endif

void ResetBitsOfWord(int * x32, int truncate_bits)
{
	int hi_part;

	hi_part = *x32 >> truncate_bits;
	*x32 -= hi_part << truncate_bits;  //r. îñòàâëÿåì ìëàäøèå 16 áèò
} 
/******************************************************************************
** Function name:		interpolation
**
** Descriptions: êîëè÷åñòâî èòåðïîëÿöèé äî ïîÿâëåíèÿ âíåøíåé çàùåëêè		
**
** Returned value:	Number in moment of external latch	appearing 	
** parameters:		y_curr - current number, y_prev - number at one cycle before time
**					x_interp - moment of external latch appearing,
					
** 	Precision of interpolation is 1/8 of impulse (3 digits after point in 14.18 format)
******************************************************************************/
int interpolation(int y_curr, int x_interp)
{
	__int64 temp,temp3; 

	temp = (__int64)y_curr *(__int64)x_interp;
	temp /= PrevPeriod;  //	 void ServiceTime(void) ||| PrevPeriod = LPC_PWM1->MR0; ||| (CyclesSync.c)

	temp3 = (__int64)y_curr *(__int64)(x_interp+1);
	temp3 /= PrevPeriod;
	temp22 = (int)temp3;

	return ((int)temp);
} // interpolation
/******************************************************************************
** Function name:		clc_Pulses
**
** Descriptions: 	Processing of information from SPOI		
**
** parameters:	None		
** Returned value:	None	
** 
******************************************************************************/

void clc_Pulses()
{

#if (!defined GLOBALRATE)
  static   int32_t	 cntPls_sum_32;
  static   int32_t	 last_Cnt_Plus;
  static   int32_t	 dif_sum_32;
  static   int32_t	 Cnt_Pls;
  static   int32_t	 Cnt_Mns;
  static   int32_t	 preLast_Cnt_Plus;
#endif

    Dif_Curr_Vib = Curr_Cnt_Vib - Old_Cnt_Vib; //Âû÷èñëåíèå ïðèðàùåíèÿ òåêóùåãî ñ÷åò÷èêà èìïóëüñîâ.

   	Old_Cnt_Vib = Curr_Cnt_Vib;				         //ñîõðàíåíèå òåêóùåãî ñ÷åò÷èêà èìïóëüñîâ äëÿ ñëåäóþùåãî öèêëà èçìåðåíèé

	  
		Cnt_Overload(Dif_Curr_Vib, INT32MAX_DIV2, INT32MIN_DIV2);  //ïðîâåðêà íà ïåðåïîëíåíèå (Dif_Curr_Vib).
   //                Uin           UpSat          DwnSat
	 
	//#define	Cnt_Overload(Uin, UpSat, DwnSat)	
/*  if (Uin > UpSat) 
	
       {
				 Uin -= INT32_MAX; 	
			 }
	  if (Uin < DwnSat) 
		    {
		     Uin += INT32_MAX;
	      }	*/
	
	
		Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // Òî÷íîñòü ôèëüòðàöèè 1/(2^18)					
	 
	 
	 
	 
   switch (RgConB) //r. äîïîëíèòåëüíûé ðåãèñòð óïðàâëåíèÿ
  	{
   		case RATE_VIBRO_1:                                              //r. ðàçíîñòü âèáðîñ÷åò÷èêîâ ïîñëå ôèëüòðà ñêîëüçÿùíãî ñðåäíåãî
		
			if (Latch_Rdy)	 //e. latch has arrived
			{	
				//dif_Curr_32_Ext ðàçíîñòü (÷èñëî) äëÿ ðåæèìà âíåøíåé çàùåëêè
			  dif_Curr_32_Ext = interpolation(Dif_Curr_32, LatchPhase );  //èíòåðïîëÿöèÿ
				Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff); 
				Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff); 
			                                                                //r. äîáàâëÿåì ê íàêîïëåííîé ñóììå èíòåðïîëèðîâàííûé îòñ÷åò âíåøíåé çàùåëêè
				PSdif_sum_Vib_32 += dif_Curr_32_Ext; 
				PSdif_sum_Vib_64 += dif_Curr_32_Ext; //e. receive last data
				//count--;
	
				//r. ïîäãîòîâèòü ÷èñëî äëÿ âûäà÷è
				Output.Str.BINS_dif = PSdif_sum_Vib_32 - TermoCompens_Sum;	  //r. èç íàêîïëåííîãî ÷èñëà âû÷èòàåì íàêîïëåííóþ òåðìîêîìïåíñàöèîííóþ ñîñòàâëÿþùóþ
				Output.Str.PS_dif = Output.Str.BINS_dif >> 16;	
				LatchPhase = INT32_MAX;	//INT32_MAX=2147483647                //in Latch_Event it's indicator of latch appearing
				Output.Str.SF_dif = PSdif_sum_Vib_64; 
				TermoCompens_Sum = 0;                                         //r. îáíóëÿåì íàêîïëåííóþ òåðìîêîìïåíñàöèþ äëÿ íà÷àëà íîâîãî öèêëà íàêîïëåíèÿ
														 				
					if ((Device_Mode == DM_EXT_LATCH_DELTA_BINS_PULSE)||((Device_Mode == DM_EXT_LATCH_DELTA_SF_PULSE) && Ext_Latch_ResetEnable))
						  {	 
							PSdif_sum_Vib_32 = 0;                                   //r. èíèöèàëèçèðîâàòü íîâûé öèêë èçìåðåíèÿ ïî çàùåëêå 
							PSdif_sum_Vib_64 = 0;					
						  }
					else
							ResetBitsOfWord(&PSdif_sum_Vib_32, 16);	  
							
				 dif_Curr_32_Ext = Dif_Curr_32 - temp22;//dif_Curr_32_Ext;

				PSdif_sum_Vib_32 +=  dif_Curr_32_Ext; // preserve rest of counters difference for next measure cycle: PSdif_sum_Vib_32 += Dif_Curr_32 - dif_Curr_32_Ext;
				PSdif_sum_Vib_64 +=  dif_Curr_32_Ext; //ñîõðàíèòü îñòàëüíûå ñ÷åò÷èêè ðàçíèöû äëÿ ñëåäóþùåãî òàêòà öèêëà
							 
			}
			else //r. çàùåëêè íà íàñòîÿùèé ìîìåíò íå áûëî 
			{	
				//r. ïðîäîëæàåì íàêàïëèâàòü ñóììó èç âíóòðåííèõ îòñ÷åòîâ
				PSdif_sum_Vib_32 += Dif_Curr_32; // PSdif_sum_Vib_32 += Dif_Curr_32 ;			
				PSdif_sum_Vib_64 += Dif_Curr_32;	 //e. sum for scale factor measurement mode
			} 

			dif_Curr_32_previous = Dif_Curr_32; //r. çàïîìèíàåì ïðåäûäóùåå ÷èñëî 
	   break;

  	 case RATE_REPER_OR_REFMEANDR:

	 	if (data_Rdy & HALF_PERIOD)			   //e. calculate Cnt_Mns or Cnt_Pls
		{
		  RefMeand_Cnt_Dif = Cnt_curr - Old_Cnt;
		  Old_Cnt = Cnt_curr;
		//  LPC_GPIO2->FIOCLR = 0x10;
		  Cnt_Overload(RefMeand_Cnt_Dif, INT32MAX_DIV2, INT32MIN_DIV2);

		  	if (LPC_QEI->STAT)						//e. "+" direction //r. ñòàëè âðàùàòüñÿ â "+" ñòîðîíó
			    {
	  	  	 	//sumCnt_Mns += -RefMeand_Cnt_Dif;    //e. accumulation during 1 sec
			    	Cnt_Mns = RefMeand_Cnt_Dif;
		      }
	 		    else
			        {	
				         //r. ñòàëè âðàùàòüñÿ â "-" ñòîðîíó
	  		         //sumCnt_Pls += RefMeand_Cnt_Dif;		//e. accumulation during 1 sec
				         Cnt_Pls = -RefMeand_Cnt_Dif;
			        }

		    //	UART1_SendByte((dif_sum_32>>8) & 0xff);
	    	//	UART1_SendByte((dif_sum_32) & 0xff);
		 
		   if (data_Rdy & WHOLE_PERIOD)		   //e. period of vibro elapsed
		      {
		     	  // LPC_GPIO2->FIOSET = 0x10;
		      	last_Cnt_Plus = Cnt_Pls;	
		      	dif_sum_32 += Cnt_Pls - Cnt_Mns;
		      }
		   data_Rdy &= ~RESET_PERIOD;
		 }
			if (Latch_Rdy)			   //e it's time for output 
		    {
					LatchPhase = INT32_MAX;

					Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff); 
					Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff); 

					cntPls_sum_32 += last_Cnt_Plus - preLast_Cnt_Plus;
       
					Output.Str.Cnt_Dif = dif_sum_32;            
					Output.Str.Cnt_Dif += cntPls_sum_32 >> 1;            
			
					dif_sum_32 = 0;				                    //r. è ñáðîñèòü ðåãèñòðû íàêîïëåíèÿ
					ResetBitsOfWord(&cntPls_sum_32, 1); 			//r. îñòàâëÿåì ìëàäøèé ðàçðÿä, ÷òîáû íå òåðÿëàñü òî÷íîñòü ïðè ñóììèðîâàíèè
					preLast_Cnt_Plus = last_Cnt_Plus;					//r. òåêóùèé ïîñëåäíèé îòñ÷åò ñòàë ïðåäûäóùèì
		
					Output.Str.Cnt_Mns = Cnt_Mns;//sumCnt_Mns;	   //e. rewrite accumulated data to output
					Output.Str.Cnt_Pls = Cnt_Pls;//sumCnt_Pls;	   

		  //sumCnt_Mns = 0;	   //e. prepare for new accumulation
		  //sumCnt_Pls = 0;
	   	}		 
	 break; // RATE_REPER_OR_REFMEANDR  
   }	 
    //e. WP_scope1, WP_scope2 - variables for control in the Rate3 mode //r. WP_scope1, WP_scope2 - ïåðåìåííûå äëÿ êîíòðîëÿ â ðåæèìå rate3
 	Output.Str.WP_scope1 = Dif_Curr_Vib;  
 	Output.Str.WP_scope2 = (Dif_Curr_32 >> (SHIFT_TO_FRACT-2)); //r. 2 äðîáíûõ ðàçðÿäà îñòàâëÿåì äëÿ áîëüøåé íàãëÿäíîñòè ïðè àíàëèçå ñèãíàëà rate3
}
		// clc_Pulses


 /******************************************************************************
** Function name:		SOI_Init
**
** Descriptions:		Quadrature encoder initialization.
**
** parameters:			None
** Returned value:		None
** 
******************************************************************************/
void SOI_Init(void)
{
  LPC_SC->PCONP |= (1<<18);//0x00040000;		âêëþ÷åíèå êâàäðàòóðíîãî ýíêîäåðà.
  LPC_SC->PCLKSEL1 |= 0x00000001;	   // âûáîð ÷àñòîòû äëÿ êâ.ý. CLK=100MHz
  LPC_PINCON->PINSEL3 &= ~0x4100;
  LPC_PINCON->PINSEL3 |= 0x4100;	  //P1.20, p1.23 óñòàíîâèòü êàê âõîäû êâ.ý
  LPC_PINCON->PINMODE3 |= 0x3C300;	  //P1.20, p1.23, p1.24 ??are pulled-down??(ïðèòÿíóòü ê ïèòàíèþ)????

  LPC_QEI->MAXPOS = MAX_QEI_CNT;	//LPC_QEI->"ðåãèñòîð ìàêñèìàëüíîãî ïîëîæåíèÿ" = 2147483646
  LPC_QEI->FILTER = 2;		   // ôèëüòð( ?? )
  LPC_QEI->CON = 0xF;				//ñáðîñèòü â íîëü âñå ñ÷åò÷èêè êâ.ý. (ñêîðîñòü, íàïðàâëåíèå, ïîçèöèþ è òä)
  LPC_QEI->CONF = (0<<2) |(0<<1);	//Quadrature inputs, no inverting,only A pulses are counted 

  LPC_QEI->CLR = 0x1fff; 			 //r. ñáðîñèòü âñå ïðåðûâàíèÿ
  LPC_QEI->IEC = 0x1fff;			 //r. çàïðåòèòü ïðåðûâàíèå ïðè èçìåíåíèè íàïðàâëåíèÿ
  
  NVIC_SetPriority(QEI_IRQn, 0);
  NVIC_EnableIRQ(QEI_IRQn);
  return;
 }


/******************************************************************************
**                            End Of File
******************************************************************************/