agc.c

DSP 5409 plc应用程序,调试通过,是用在电力线通讯上的演示程序.

    return recSignal;
}

#endif


#if COMPILE_MODE == MEX_COMPILE
//==========================================================================================
// Function:		lookForPacket()
//
// Description: 	Look for the start of a packet by comparing power in bins near the top
//					of the transmission band to bins just beyond the transmission band.
//
// Modifies:		SNRflag, agcState
//
// Revision History:
//==========================================================================================
i16 *lookForPacket( dCplxPtr recAgc,  dCplxPtr rec )
{
	i16				sampleCount = 0;
	iCplx			*fftBuf;

	i16				phError = 0;
	i16				preambleDetCount = 0;

	i16				overflow = 0;

	
	//---- set up debug buffer ----------------------------------------
	#if DEBUGIT == DEBUG_AGC
		diagArray = mxCreateDoubleMatrix(3, rec.len, mxREAL);
		mxSetName(diagArray, "agcCtrl");
		diag.r = mxGetPr(diagArray);
	#endif
	#if ( (DEBUGIT == DEBUG_SNR) || (DEBUGIT == DEBUG_DISTANCE2) )
		diagArray = mxCreateDoubleMatrix(4, rec.len, mxREAL);
		mxSetName(diagArray, "snrDiag");
		diag.r = mxGetPr(diagArray);
	#endif

	memset(recSignalArray, 0x80, RX_CIRC_BUFFER_LEN);		// see where we are 

	//---- main AGC loop ----------------------------------------------
	fakeInteruptCount = 0;
	agcState = AgcIdle;
	pgaGainReg = 0;

	//---- get pointer to rx buffer ---------------------------
	fakeCodecAndDMA( &recAgc.r, &rec.r,  pgaGainReg );
	sampleCount++;
	recSignal = ReadRxDMAPointer();			// init global var recSignal

	//---- loop through the received data -----------------------
	while( agcState != AgcHold )
	{
		//---- model getting ADC sample ---------------------------
		fakeCodecAndDMA( &recAgc.r, &rec.r,  pgaGainReg );
		sampleCount++;

		if( fakeInteruptCount > AGC_INTERVAL )
		{
			fakeInteruptCount = 0;
			sample_interupt();				// update agc gain using recSignal
		}
		
		if( SNRflag == 1 )
		{
			SNRflag = 0;

			backFFT( fftArray, recSignal );		// grab the last FFT_LEN samples and get freq resp

			fftBuf  = fftArray + CARRIER_LOW;
			phError = ddphase(PreambleArray, fftBuf, CARRIER_LEN );

			#if DEBUGIT == DEBUG_SNR
			{
				static i16	cnt = 0;
				char	vnameStr[20];
				sprintf(vnameStr,"ddDet%d",cnt);
				diagData(fftArray, FFT_LEN, vnameStr, diagICPLX);
				mexPrintf("%d: pre phaseError %d\n", cnt++, phError );
			}
			#endif

			if( phError  > DDPHASE_HI_LIM  )	//	decide if this lookes like a packet
			{									//	if so, switch to preamble state
				preambleDetCount = 0;			//	start over
				agcState = AgcIdle;
			}
			else
			{
				if( phError <  DDPHASE_LO_LIM )
				{
					preambleDetCount++;		  	//	and count # of good preamble snrs seen
					agcState = AgcPreamble;
				}
			}

			if( preambleDetCount >= SNR_PRE_DET_COUNT )
				agcState = AgcHold;
			
		}	// end of fft processing

		#if ( (DEBUGIT == DEBUG_SNR) || (DEBUGIT == DEBUG_DISTANCE2) )
			*diag.r++ = (double)(*(rxDMApointer-1));
			*diag.r++ = (double)preambleDetCount;
			*diag.r++ = (double)pgaGainReg;
			*diag.r++ = (double)phError;
		#endif

		if( sampleCount > (rec.len-1) )
		{
			#if ( (DEBUGIT == DEBUG_SNR) || (DEBUGIT == DEBUG_DISTANCE2) )
				mexPutArray(diagArray, "caller");
			#endif
			postError("Failed to find preamble.");
		}

	}		// end of while loop
		
	//mexPrintf( "Packet detected at sample count %d\n", sampleCount );

	//---- finish out the waveform for Matlab --------------
	for( ; sampleCount < rec.len ; sampleCount++ )
	{
		overflow += fakeCodecAndDMA( &recAgc.r, &rec.r,  pgaGainReg );
		if( overflow > AGC_OVERFLOW_COUNT )
		{
			diagData(recSignalArray, RX_CIRC_BUFFER_LEN, "rxArray", diagI16);
			postError("Too many overflowed samples");
		}
	}

	#if DEBUGIT == DEBUG_AGC
		mexPutArray(diagArray, "caller");
	#endif
	#if ( (DEBUGIT == DEBUG_SNR) || (DEBUGIT == DEBUG_DISTANCE2) )
		mexPutArray(diagArray, "caller");
	#endif
	
   return recSignal;
}
#endif


//==========================================================================================
// Function:		ddphase()
//
// Description: 	Calculate the double difference phase from the carriers passed.
//
//		    dphase = carrier2-carrier1,  carrier3-carrier2, ...  
//		 
//		                    y2           y1             x1*y2 - y1*x2 
//		    dphase1 = atan(----) - atan(----)  =  atan(---------------)
//		                    x2           x1             x1*x2 + y1*y2
//		 
//		    ddphase = dphase2-dphase1,  dphase3-dphase2, ...
//		 
//		                     x2*y3 - y2*x3 	         x1*y2 - y1*x2 
//		    ddphase1 = atan(---------------) - atan(---------------)
//		                     x2*x3 + y2*y3		     x1*x2 + y1*y2
//		 
//		 					 ( x1*x2 +y1*y2)*( x2*y3 -y2*x3) - ( x1*y2 -y1*x2)*( x2*x3 +y2*y3)
//		    ddphase1 = atan(-------------------------------------------------------------------)
//		 					 ( x1*x2 +y1*y2)*( x2*x3 +y2*y3) + ( x1*y2 -y1*x2)*( x2*y3 -y2*x3)
//		 					 y  
//		             = atan(---)
//		 					 x  
//		    y = ( x1*x2 +y1*y2)*( x2*y3 -y2*x3) 
//		      - ( x1*y2 -y1*x2)*( x2*x3 +y2*y3) 
//		 
//		    x = ( x1*x2 +y1*y2)*( x2*x3 +y2*y3) 
//		      + ( x1*y2 -y1*x2)*( x2*y3 -y2*x3)
//		   
//		  then convenient groupings are:
//		    pc = [ (x1*x2 +y1*y2)  (x1*y2 -y1*x2) ]     // previous coefficients
//		    cc = [ (x2*x3 +y2*y3)  (x2*y3 -y2*x3) ]     // current coefficients
//		    y  = pc1*cc2 - pc2*cc1;
//		    x  = pc1*cc1 + pc2*cc2;
//		    b1 =  -y > x;
//		    b0 =  |y|>|x|;
//			
//		  c-style notation:
//		    pc = [ (x0*x1 +y0*y1)  (x0*y1 -y0*x1) ]     // previous coefficients
//		    cc = [ (x1*x2 +y1*y2)  (x1*y2 -y1*x2) ]     // current coefficients
//		    y  = pc0*cc1 - pc1*cc0;
//		    x  = pc0*cc0 + pc1*cc1;
//		    b1 =  -y > x;
//		    b0 =  |y|>|x|;
//
//
// Revision History:
//==========================================================================================
#define MSHIFT		8
u16 ddphase(const i16 *preamble, iCplx *carriers, i16 carrierLen )
{
	i16				n;
	i16				c0r, c0i, c1r, c1i;
	i16				pc0, pc1, cc0, cc1;
	i32				x,   y;
	i16				b1,  b0;
	
	i16				ddph;
	i16				phError = 0;
	
	static const u16 ddphaseDistance[16] = 
				{	0, 1, 2, 1, 
					1, 0, 1, 2,
					2, 1, 0, 1,
					1, 2, 1, 0 
				};

#define	DEBUG_MSHIFT	FALSE	// Set TRUE to enable diagnostics
	#if (DEBUG_MSHIFT == TRUE)
		i32				xMax=0;
		i32				yMax = 0;
		i16				cc0Max = 0;
		i16				cc1Max = 0;
	#endif

	#if DEBUGIT == DEBUG_DDPH
		diagArray = mxCreateDoubleMatrix(4, carrierLen, mxREAL);
		mxSetName(diagArray, "ddphDiag");
		diag.r = mxGetPr(diagArray);
	#endif

	preamble += 2;				// ignoring first two symbols
	
	c0r = carriers->re;			// carrier 0
	c0i = carriers->im;
	carriers++;
	c1r = carriers->re;			// carrier 1
	c1i = carriers->im;
	carriers++;

	cc0 = ( (i32)c0r * (i32)c1r + (i32)c0i * (i32)c1i ) >> MSHIFT;
	cc1 = ( (i32)c0r * (i32)c1i - (i32)c0i * (i32)c1r ) >> MSHIFT;

	//----calc double differenece phase --------------------------------
	for( n = 2;  n < carrierLen;  n++ )
	{
		c0r = c1r;
		c0i = c1i;
		c1r = carriers->re;		// carriers 2 to end
		c1i = carriers->im;
		carriers++;

		pc0 = cc0;				// do the trig math
		pc1 = cc1;
		cc0 = ( (i32)c0r * (i32)c1r + (i32)c0i * (i32)c1i ) >> MSHIFT;
		cc1 = ( (i32)c0r * (i32)c1i - (i32)c0i * (i32)c1r ) >> MSHIFT;

		y	= (i32)pc0*(i32)cc1 - (i32)pc1*(i32)cc0;
		x	= (i32)pc0*(i32)cc0 + (i32)pc1*(i32)cc1;
	    
	    b1 = -y > x;
	    b0 = labs(y) > labs(x);
	    //ddph = b1*2 + b0;		// here is the double-difference phase
		ddph = b1*8 + b0*4;		// here is the double diff phase * 4

		phError += ddphaseDistance[ddph + *preamble++];  // get sum of distances as phase error

		#if DEBUGIT == DEBUG_DDPH
			*diag.r++ = (double)cc0;
			*diag.r++ = (double)cc1;
			*diag.r++ = (double)y;
			*diag.r++ = (double)x;
		#endif

		#if (DEBUG_MSHIFT == TRUE)
			if (abs(cc0) > cc0Max)
			{
				cc0Max = abs(cc0);
			}
			if (abs(cc1) > cc1Max)
			{
				cc1Max = abs(cc1);
			}
			if (labs(x) > xMax)
			{
				xMax = labs(x);
			}
			if (labs(y) > yMax)
			{
				yMax = labs(y);
			}
		#endif

	}

	#if DEBUGIT == DEBUG_DDPH
		mexPutArray(diagArray, "caller");
	#endif

		
	#if (DEBUG_MSHIFT == TRUE)
		#if SAVETRACE == TRUE
			SaveTraceData(0x9999);
			SaveTraceData((u16)cc0Max);
			SaveTraceData((u16)cc1Max);
			SaveTraceData(0x0000);
			SaveTraceData((u16)xMax);
			SaveTraceData((u16)yMax);
		#endif
	#endif
	
	return phError;
}