sdrtrunk/src/dsp/psk/LSMDemodulator.java

package dsp.psk;

/*******************************************************************************
 *     SDR Trunk 
 *     Copyright (C) 2014, 2015 Dennis Sheirer
 * 
 * 	   Gardner detector, Costas loop and Interpolator derived from:
 * 
 * 	   OP25 - gardner_costas_cc_impl.cc
 *     Copyright 2010, 2011, 2012, 2013 KA1RBI (gardner_costas_cc_impl.cc)
 *     
 *     OP25 - scope.py
 *     Copyright 2008-2011 Steve Glass
 *     Copyright 2011, 2012, 2013 KA1RBI
 *     
 *     GNURadio - control_loop and mpsk_receiver_cc
 *     Copyright 2011,2013 Free Software Foundation, Inc.
 *     
 *     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 3 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, see <http://www.gnu.org/licenses/>
 ******************************************************************************/

import instrument.Instrumentable;
import instrument.tap.Tap;
import instrument.tap.TapGroup;
import instrument.tap.stream.EyeDiagramData;
import instrument.tap.stream.EyeDiagramDataTap;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import sample.Listener;
import sample.complex.Complex;
import sample.complex.ComplexSampleListener;
import source.tuner.frequency.IFrequencyChangeListener;
import buffer.FloatAveragingBuffer;
import dsp.filter.interpolator.RealInterpolator;
import dsp.symbol.Dibit;

/**
 * Implements a LSM (Pi/4) demodulator using a Gardner Detector to determine 
 * optimal sampling timing and a Costas Loop as a phase locked loop synchronized 
 * with the incoming signal carrier frequency.
 * 
 * Sample Rate: 48000
 * Symbol Rate: 4800
 */
public class LSMDemodulator implements Instrumentable, ComplexSampleListener
{
	private List<TapGroup> mAvailableTaps;
	
	/* 45 degree rotation angle */
	public static final float THETA = (float)( Math.PI / 4.0d ); 

	/* 45 degree point */
	public static final Complex POINT_45_DEGREES = 
		new Complex( (float)Math.sin( THETA ), (float)Math.cos( THETA ) );
	
	private Listener<Complex> mSymbolListener;
	
	private GardnerSymbolTiming mGardnerDetector = new GardnerSymbolTiming();
	
	private CostasLoop mCostasLoop = new CostasLoop();
	
	private IFrequencyChangeListener mFrequencyChangeListener;
	
	private EyeDiagramDataTap mEyeDiagramDataTap;
	
	public LSMDemodulator()
	{
	}
	
	public void dispose()
	{
		mSymbolListener = null;
		mFrequencyChangeListener = null;
	}
	
	public void setSymbolListener( Listener<Complex> listener )
	{
		mSymbolListener = listener;
	}
	
	public void removeSymbolListener()
	{
		mSymbolListener = null;
	}
	
	@Override
	public void receive( float inphase, float quadrature )
	{
		mGardnerDetector.receive( inphase, quadrature );
	}
	
	/**
	 * Applies a phase correction value to the costas loop to correct when a
	 * phase lock error is detected in the binary output stream.
	 * 
	 * @param correction - value in radians
	 */
	public void correctPhaseError( double correction )
	{
		mCostasLoop.correctPhaseError( correction );
	}
	
	/**
	 * Constrains value to the range of ( -maximum <> maximum )
	 */
	public static float clip( float value, float maximum )
	{
		if( value > maximum )
		{
			return maximum;
		}
		else if( value < -maximum )
		{
			return -maximum;
		}
		
		return value;
	}
	
	/**
	 * Constrains timing error to +/- the maximum value and corrects any
	 * floating point invalid numbers
	 */
	private float normalize( float error, float maximum )
	{
		if( Float.isNaN( error ) )
		{
			return 0.0f;
		}
		else
		{
			return clip( error, maximum );
		}
	}
	
	public void addListener( IFrequencyChangeListener listener )
	{
		mFrequencyChangeListener = listener;
	}
	
	public void removeListener( IFrequencyChangeListener listener )
	{
		mFrequencyChangeListener = null;
	}

	/**
	 * Processes the current loop frequency of the costas loop and broadcasts
	 * tuner frequency offset adjustments 10 times a second as needed to keep
	 * the frequency offset below half of the maximum frequency
	 */
	public class FrequencyControl
	{	
		/* Set the trigger threshold at half of the maximum costas loop control
		 * frequency */
		private static final float CORRECTION_THRESHOLD_FREQUENCY = 
			( 2.0f * (float)Math.PI * 600.0f ) / 48000.0f;

		private FloatAveragingBuffer mBuffer = new FloatAveragingBuffer( 40 );
		
		private long mFrequencyError = 0;
		
		private int mCounter = 0;
		
		public void receive( float frequency )
		{
//			mCounter++;
//			
//			if( mCounter >= 480 )
//			{
//				float average = mBuffer.get( frequency );
//				mLog.debug( "Avg:" + average + " Threshold:" + CORRECTION_THRESHOLD_FREQUENCY);
//
//				boolean correctionNeeded = false;
//				
//				if( average > CORRECTION_THRESHOLD_FREQUENCY )
//				{
//					mFrequencyError--;
//
//					correctionNeeded = true;
//				}
//				else if( average < -CORRECTION_THRESHOLD_FREQUENCY )
//				{
//					mFrequencyError++;
//					
//					correctionNeeded = true;
//				}
//				
//				if( correctionNeeded && mFrequencyChangeListener != null )
//				{
//					mLog.debug( "Issuing Frequency Correction: " + mFrequencyError );
//					mFrequencyChangeListener.frequencyChanged( 
//						new FrequencyChangeEvent( Attribute.FREQUENCY_ERROR, 
//								mFrequencyError ) );
//				}
//				
//				mCounter = 0;
//			}
//			else
//			{
//				mBuffer.get( frequency );
//			}
		}
	}

	/**
	 * Gardner symbol timing detector
	 */
	public class GardnerSymbolTiming implements ComplexSampleListener
	{
		public static final int HALF_SAMPLES_PER_SYMBOL = 5;
		public static final int SAMPLES_PER_SYMBOL = 10;
		public static final int TWICE_SAMPLES_PER_SYMBOL = 20;

		private float[] mDelayLineInphase = new float[ 2 * TWICE_SAMPLES_PER_SYMBOL ];
		private float[] mDelayLineQuadrature = new float[ 2 * TWICE_SAMPLES_PER_SYMBOL ];

		private int mDelayLinePointer = 0;

		/* Sampling point */
		private float mMu = 10.0f;
		private float mGainMu = 0.05f;
		
		/* Samples per symbol */
		private float mOmega = 10.0f;
		private float mGainOmega = 0.1f * mGainMu * mGainMu;
		private float mOmegaRel = 0.005f;
		private float mOmegaMid = 10.0f;

		private RealInterpolator mInterpolator = new RealInterpolator( 1.0f );
		
		private Complex mPreviousSample = new Complex( 0, 0 );
		private Complex mPreviousMiddleSample = new Complex( 0, 0 );
		private Complex mPreviousSymbol = new Complex( 0, 0 );
		
		/**
		 * Provides symbol sampling timing control
		 */
		public GardnerSymbolTiming()
		{
		}

		@Override
		public void receive( float inphase, float quadrature )
		{
			/* Count down samples per symbol until we calculate the symbol */
			mMu--;

			mCostasLoop.increment();
			
			/* Mix incoming sample with costas loop to remove any rotation 
			 * that is present from a mis-tuned carrier frequency */
			
			Complex costas = mCostasLoop.getCurrentVector();
			
			float derotatedInphase = Complex.multiplyInphase( inphase, quadrature, costas.inphase(), costas.quadrature() );
			float derotatedQuadrature = Complex.multiplyQuadrature( inphase, quadrature, costas.inphase(), costas.quadrature() );
			
			/* Fill up the delay line to use with the interpolator */
			mDelayLineInphase[ mDelayLinePointer ] = derotatedInphase;
			mDelayLineInphase[ mDelayLinePointer + TWICE_SAMPLES_PER_SYMBOL ] = derotatedInphase;

			mDelayLineQuadrature[ mDelayLinePointer ] = derotatedQuadrature;
			mDelayLineQuadrature[ mDelayLinePointer + TWICE_SAMPLES_PER_SYMBOL ] = derotatedQuadrature;

			/* Increment pointer and keep pointer in bounds */
			mDelayLinePointer = ( mDelayLinePointer + 1 ) % TWICE_SAMPLES_PER_SYMBOL;
			
			/* Calculate the symbol once we've stored enough samples */
			if( mMu <= 1.0f )
			{
				float half_omega = mOmega / 2.0f;
				int half_sps = (int)Math.floor( half_omega );
				float half_mu = mMu + half_omega - (float)half_sps;
				
				if( half_mu > 1.0 )
				{
					half_mu -= 1.0;
					half_sps += 1;
				}

				/* Calculate interpolated middle sample and current sample */
				float middleSampleInphase = mInterpolator.filter( 
						mDelayLineInphase, mDelayLinePointer, mMu );
				float middleSampleQuadrature = mInterpolator.filter( 
						mDelayLineQuadrature, mDelayLinePointer, mMu );

				int index = mDelayLinePointer + half_sps;
				
				float currentSampleInphase = mInterpolator.filter( 
						mDelayLineInphase, index, half_mu );
				float currentSampleQuadrature = mInterpolator.filter( 
						mDelayLineQuadrature, index, half_mu );

				/* Multiply current sample and conjugate (negated quadrature) of 
				 * previous sample to get symbols to use for gardner error feedback */
				Complex middleSymbol = Complex.multiply( middleSampleInphase, 
						middleSampleQuadrature, mPreviousMiddleSample.conjugate() );
				Complex currentSymbol = Complex.multiply( currentSampleInphase, 
						currentSampleQuadrature, mPreviousSample.conjugate() );

				/* Set gain to unity */
				middleSymbol.normalize();
				currentSymbol.normalize();

				/* Gardner timing error calculations */
				float errorInphase = ( mPreviousSymbol.inphase() - 
						currentSymbol.inphase() ) * middleSymbol.inphase();

				float errorQuadrature = ( mPreviousSymbol.quadrature() - 
						currentSymbol.quadrature() ) * middleSymbol.quadrature();

				float gardnerError = normalize( errorInphase + errorQuadrature, 1.0f );
				
				if( mEyeDiagramDataTap != null )
				{
					mEyeDiagramDataTap.receive( 
						new EyeDiagramData( 
								Arrays.copyOfRange( mDelayLineInphase, 0, 20 ), 
								Arrays.copyOfRange( mDelayLineQuadrature, 0, 20 ), 
							mMu, (float)half_sps + half_mu, gardnerError ) );
				}
				
				/* mOmega is samples per symbol and is constrained to floating
				 * between +/- .005 of the nominal 10.0 samples per symbol */
				mOmega = mOmega + mGainOmega * gardnerError;
				mOmega = mOmegaMid + clip( mOmega - mOmegaMid, mOmegaRel );

				/* Adjust sample timing based on error of current sample */
				mMu += mOmega + ( mGainMu * gardnerError );

				/* Store current samples/symbols to use for the next period */
				mPreviousSample.setValues( currentSampleInphase, currentSampleQuadrature );
				mPreviousMiddleSample.setValues( middleSampleInphase, middleSampleQuadrature );
				mPreviousSymbol = currentSymbol;

				/* Update costas loop using phase error present in current 
				 * symbol.  The symbol is rotated from star orientation to polar
				 * orientation to simplify error calculation */
				mCostasLoop.receive( currentSymbol );
//				mCostasLoop.receive( 
//						ComplexSample.multiply( currentSymbol, POINT_45_DEGREES ) );
				
				/* Dispatch the differentiated symbol to the registered listener */
				if( mSymbolListener != null )
				{
					mSymbolListener.receive( currentSymbol );
				}
			}
		}
	}
	
	/**
	 * Costas Loop - phase locked loop synchronized to the frequency offset of
	 * the incoming signal to enable the signal to be mixed down to zero offset
	 * for proper synchronization.  The mFrequency value indicates the detected
	 * frequency offset.  We attempt to keep that value close to zero by issuing
	 * frequency adjustments to the tuner channel source.
	 * 
	 * Most of the costas loop code was ported from gnuradio/control_loop and 
	 * gnuradio/mpsk_receiver_cc using initialization values from KA1RBI's 
	 * OP25/cqpsk.py
	 */
	public class CostasLoop implements Listener<Complex>
	{
		public static final double TWO_PI = 2.0 * Math.PI;
		
		private static final float MAXIMUM_FREQUENCY = 
				( 2400.0f * (float)TWO_PI ) / 48000.0f;
		
		/* http://www.trondeau.com/blog/2011/8/13/control-loop-gain-values.html */
		private float mDamping = (float)Math.sqrt( 2.0 ) / 2.0f;
		
		/* Use denominator between 100 (most) and 400 (least) to adjust costas
		 * loop control level */
		private float mLoopBandwidth = (float)( TWO_PI / 400.0d );
		
		private float mAlphaGain = ( 4.0f * mDamping * mLoopBandwidth ) / 
						  ( 1.0f + ( 2.0f * mDamping * mLoopBandwidth ) + 
								   ( mLoopBandwidth * mLoopBandwidth ) );

		private float mBetaGain = ( 4.0f * mLoopBandwidth * mLoopBandwidth ) / 
							   ( 1.0f + ( 2.0f * mDamping * mLoopBandwidth ) + 
								   ( mLoopBandwidth * mLoopBandwidth ) );

		private float mLoopPhase = 0.0f;
		
		private float mLoopFrequency = 0.0f;
		
		private FrequencyControl mFrequencyControl = new FrequencyControl();
		
		public CostasLoop()
		{
		}
		
		/**
		 * Applies a phase correction value to the current phasor.  Use this 
		 * method to apply correction when a +/- 90, or 180 degree phase error
		 * is detected in the binary output stream.
		 * 
		 * If the supplied correction value places the loop frequency outside
		 * of the max frequency, then the frequency will be corrected 360
		 * degrees in the opposite direction to maintain within the max
		 * frequency bounds.
		 * 
		 * @param correction - correction value in radians
		 */
		public void correctPhaseError( double correction )
		{
			mLoopFrequency += correction;
		
			if( mLoopFrequency > MAXIMUM_FREQUENCY )
			{
				mLoopFrequency -= 2.0d * MAXIMUM_FREQUENCY;
			}
			
			if( mLoopFrequency < -MAXIMUM_FREQUENCY )
			{
				mLoopFrequency += 2.0d * MAXIMUM_FREQUENCY;
			}
		}
		
		/**
		 * Increments the phase of the loop
		 */
		public void increment()
		{
			mLoopPhase += mLoopFrequency;

			/* Keep the loop phase in bounds */
			unwrapPhase();
		}

		private void unwrapPhase()
		{
			while( mLoopPhase > TWO_PI )
			{
				mLoopPhase -= TWO_PI;
			}
			
			while( mLoopPhase < -TWO_PI )
			{
				mLoopPhase += TWO_PI;
			}
		}
		
		/**
		 * Current vector of the loop rotated by THETA degrees.
		 */
		public Complex getCurrentVector()
		{
//			return ComplexSample.fromAngle( mLoopPhase + THETA );
			return Complex.fromAngle( mLoopPhase );
		}
		
		@Override
		public void receive( Complex complex )
		{
			/* Calculate phase error */
			float phaseError = getPhaseError( complex );
			
			Dibit d = QPSKPolarSlicer.decide( complex );
//			mLog.debug( "Dibit: " + d.name() + " Error: " + phaseError );

			/* Adjust for phase error */
			adjust( phaseError );
		}

		/**
		 * Updates the costas loop frequency and phase to adjust for the phase
		 * error value 
		 * 
		 * @param phase_error - (-)= late and (+)= early
		 */
		private void adjust( float phase_error )
		{
			mLoopFrequency += mBetaGain * phase_error;
			mLoopPhase += mLoopFrequency + mAlphaGain * phase_error;

			/* Maintain phase between +/- 2 * PI */
			unwrapPhase();

			/* Limit frequency to +/- maximum loop frequency */
			limitFrequency();
		}

		/**
		 * Constrains the frequency within the bounds of +/- loop frequency
		 */
		private void limitFrequency()
		{
			/* Check for and issue tuner offset correction */
			mFrequencyControl.receive( mLoopFrequency );
			
			if( mLoopFrequency > MAXIMUM_FREQUENCY )
			{
				mLoopFrequency = MAXIMUM_FREQUENCY;
			}
			
			if( mLoopFrequency < -MAXIMUM_FREQUENCY )
			{
				mLoopFrequency = -MAXIMUM_FREQUENCY;
			}
		}

		/**
		 * Detects rotational error present when the costas loop is not aligned
		 * with the carrier frequency.  Provides error feedback to adjust 
		 * mixer frequency.
		 */
		public float getPhaseError( Complex complex )
		{
			  float phase_error = 0;
			  
			  if( Math.abs( complex.inphase() ) > Math.abs( complex.quadrature() ) ) 
			  {
				  if( complex.inphase() > 0 )
				  {
					  phase_error = -complex.quadrature();
				  }
				  else
				  {
					  phase_error = complex.quadrature();
				  }
			  }
			  else 
			  {
				  if( complex.quadrature() > 0 )
				  {
					  phase_error = complex.inphase();
				  }
				  else
				  {
					  phase_error = -complex.inphase();
				  }
			  }
			  
			  return phase_error;
		}
	}

	@Override
	public List<TapGroup> getTapGroups()
	{
		if( mAvailableTaps == null )
		{
			mAvailableTaps = new ArrayList<>();
			
			TapGroup group = new TapGroup( "LSM Demodulator" );
			
			group.add( new EyeDiagramDataTap( "Eye Diagram", 0, 4800 ) );

			mAvailableTaps.add( group );
		}
		
		return mAvailableTaps;
	}

	@Override
	public void registerTap( Tap tap )
	{
		if( tap instanceof EyeDiagramDataTap )
		{
			mEyeDiagramDataTap = (EyeDiagramDataTap)tap;
		}
	}

	@Override
	public void unregisterTap( Tap tap )
	{
		if( tap instanceof EyeDiagramDataTap )
		{
			mEyeDiagramDataTap = null;
		}
	}
}