freqsyncacq.cpp

Dream.exe soft source (Visual C++)

/******************************************************************************\
 * Technische Universitaet Darmstadt, Institut fuer Nachrichtentechnik
 * Copyright (c) 2001
 *
 * Author(s):
 *	Volker Fischer
 *
 * Description:
 *	Frequency synchronization acquisition (FFT-based)
 *
 * The input data is not modified by this module, it is just a measurement
 * of the frequency offset. The data is fed through this module.
 *
 ******************************************************************************
 *
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License as published by the Free Software
 * Foundation; either version 2 of the License, or (at your option) any later 
 * version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT 
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more 
 * details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc., 
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
\******************************************************************************/

#include "FreqSyncAcq.h"


/* Implementation *************************************************************/
void CFreqSyncAcq::ProcessDataInternal(CParameter& ReceiverParam)
{
	int			i, j;
	int			iMaxIndex;
	fftw_real	rMaxValue;
	int			iNumDetPeaks;
	int			iDiffTemp;
	CReal		rLevDiff;
	_BOOLEAN	bNoPeaksLeft;
	CRealVector	vecrPSDPilPoin(3);

	if (bAquisition == TRUE)
	{
		/* Do not transfer any data to the next block if no frequency
		   acquisition was successfully done */
		iOutputBlockSize = 0;

		/* Add new symbol in history (shift register) */
		vecrFFTHistory.AddEnd((*pvecInputData), iInputBlockSize);


		/* Start algorithm when history memory is filled -------------------- */
		/* Wait until history memory is filled for the first FFT operation.
		   ("> 1" since, e.g., if we would have only one buffer, we can start
		   immediately) */
		if (iAquisitionCounter > 1)
		{
			/* Decrease counter */
			iAquisitionCounter--;
		}
		else
		{
			/* Copy vector to matlib vector and calculate real-valued FFTW */
			const int iStartIdx = iHistBufSize - iFrAcFFTSize;
			for (i = 0; i < iFrAcFFTSize; i++)
				vecrFFTInput[i] = vecrFFTHistory[i + iStartIdx];

			/* Calculate power spectrum (X = real(F)^2 + imag(F)^2) */
			vecrSqMagFFTOut =
				SqMag(rfft(vecrFFTInput * vecrHammingWin, FftPlan));

			/* Calculate moving average for better estimate of PSD */
			vvrPSDMovAv.Add(vecrSqMagFFTOut);

			/* Wait until we have sufficient data for averaging */
			if (iAverageCounter > 1)
			{
				/* Decrease counter */
				iAverageCounter--;
			}
			else
			{
				/* Get PSD estimate */
				const CRealVector vecrPSD(vvrPSDMovAv.GetAverage());


				/* -------------------------------------------------------------
				   Low pass filtering over frequency axis. We do the filtering
				   from both sides, once from right to left and then from left
				   to the right side. Afterwards, these results are averaged
				   This way, the noise floor is estimated */

// TODO: Introduce offset to debar peak at DC frequency (cause by DC offset of
// sound cards). Set "iStartFilt" in Init() routine!
const int iStartFilt = 0; // <- no offset right now

				/* Reset vectors for intermediate filtered result */
				vecrFiltResLR.Reset((CReal) 0.0);
				vecrFiltResRL.Reset((CReal) 0.0);

				/* From the left edge to the right edge */
				vecrFiltResLR[iStartFilt] = vecrPSD[iStartFilt];
				for (i = iStartFilt + 1; i < iHalfBuffer; i++)
				{
					vecrFiltResLR[i] = (vecrFiltResLR[i - 1] - vecrPSD[i]) *
						LAMBDA_FREQ_IIR_FILT + vecrPSD[i];
				}

				/* From the right edge to the left edge */
				vecrFiltResRL[iHalfBuffer - 1] =
					vecrPSD[iHalfBuffer - 1];
				for (i = iHalfBuffer - 2; i >= iStartFilt; i--)
				{
					vecrFiltResRL[i] = (vecrFiltResRL[i + 1] - vecrPSD[i]) *
						LAMBDA_FREQ_IIR_FILT + vecrPSD[i];
				}

				/* Average RL and LR filter outputs */
				vecrFiltRes = (vecrFiltResLR + vecrFiltResRL) / 2;

#ifdef _DEBUG_
#if 0
/* Stores curves for PSD estimation and filtering */
FILE* pFile2 = fopen("test/freqacqFilt.dat", "w");
for (i = 0; i < iHalfBuffer; i++)
	fprintf(pFile2, "%e %e\n", vecrPSD[i], vecrFiltRes[i]);
fclose(pFile2);
#endif
#endif

				/* Equalize PSD by "noise floor estimate" */
				for (i = 0; i < iHalfBuffer; i++)
				{
					/* Make sure we do not devide by zero */
					if (vecrFiltRes[i] != 0.0)
						vecrPSD[i] /= vecrFiltRes[i];
					else
						vecrPSD[i] = 0.0;
				}

				/* Correlate known frequency-pilot structure with equalized
				   power spectrum */
				for (i = 0; i < iSearchWinSize; i++)
				{
					vecrPSDPilCor[i] =
						vecrPSD[i + veciTableFreqPilots[0]] +
						vecrPSD[i + veciTableFreqPilots[1]] +
						vecrPSD[i + veciTableFreqPilots[2]];
				}


				/* Detect peaks --------------------------------------------- */
				/* Get peak indices of detected peaks */
				iNumDetPeaks = 0;
				for (i = iStartDCSearch; i < iEndDCSearch; i++)
				{
					/* Test peaks against a bound */
					if (vecrPSDPilCor[i] > rPeakBoundFiltToSig)
					{
						veciPeakIndex[iNumDetPeaks] = i;
						iNumDetPeaks++;
					}
				}

				/* Check, if at least one peak was detected */
				if (iNumDetPeaks > 0)
				{
					/* ---------------------------------------------------------
					   The following test shall exclude sinusoid interferers in
					   the received spectrum */
					CVector<int> vecbFlagVec(iNumDetPeaks, 1);

					/* Check all detected peaks in the "PSD-domain" if there are
					   at least two peaks with approx the same power at the
					   correct places (positions of the desired pilots) */
					for (i = 0; i < iNumDetPeaks; i++)
					{
						/* Fill the vector with the values at the desired
						   pilot positions */
						vecrPSDPilPoin[0] =
							vecrPSD[veciPeakIndex[i] + veciTableFreqPilots[0]];
						vecrPSDPilPoin[1] =
							vecrPSD[veciPeakIndex[i] + veciTableFreqPilots[1]];
						vecrPSDPilPoin[2] =
							vecrPSD[veciPeakIndex[i] + veciTableFreqPilots[2]];

						/* Sort, to extract the highest and second highest
						   peak */
						vecrPSDPilPoin = Sort(vecrPSDPilPoin);

						/* Debar peak, if it is much higher than second highest
						   peak (most probably a sinusoid interferer). The
						   highest peak is stored at "vecrPSDPilPoin[2]". Also
						   test for lowest peak */
						if ((vecrPSDPilPoin[1] / vecrPSDPilPoin[2] <
							MAX_RAT_PEAKS_AT_PIL_POS_HIGH) &&
							(vecrPSDPilPoin[0] / vecrPSDPilPoin[2] <
							MAX_RAT_PEAKS_AT_PIL_POS_LOW))
						{
							/* Reset "good-flag" */
							vecbFlagVec[i] = 0;
						}
					}


					/* Get maximum ------------------------------------------ */
					/* First, get the first valid peak entry and init the
					   maximum with this value. We also detect, if a peak is
					   left */
					bNoPeaksLeft = TRUE;
					for (i = 0; i < iNumDetPeaks; i++)
					{
						if (vecbFlagVec[i] == 1)
						{
							/* At least one peak is left */
							bNoPeaksLeft = FALSE;

							/* Init max value */
							iMaxIndex = veciPeakIndex[i];
							rMaxValue = vecrPSDPilCor[veciPeakIndex[i]];
						}
					}

					if (bNoPeaksLeft == FALSE)
					{
						/* Actual maximum detection, take the remaining peak
						   which has the highest value */
						for (i = 0; i < iNumDetPeaks; i++)
						{
							if ((vecbFlagVec[i] == 1) &&
								(vecrPSDPilCor[veciPeakIndex[i]] >
								rMaxValue))
							{
								iMaxIndex = veciPeakIndex[i];
								rMaxValue = vecrPSDPilCor[veciPeakIndex[i]];
							}
						}