GitHub/sdrtrunk
1
0
Fork 0
mirror of https://github.com/DSheirer/sdrtrunk.git synced 2026-11-14 01:27:34 -05:00
Code Issues Packages Projects Releases 2 Wiki Activity

P25 LSM work in progress. CQPSK demodulator is working now, but there is a slight phase error in the constellation (it's rotated right by 5-10 degrees). I'm not sure if this is an error coming from the decoder, or if it's reflecting a phase imbalance from the source tuner i/q data.

Browse Source
This commit is contained in:
dennis.sheirer
2015-02-16 23:14:23 +00:00
parent 78163caf4f
commit 7d948c4af4
6 changed files with 322 additions and 281 deletions
Download Patch File Download Diff File Expand all files Collapse all files

488
src/dsp/psk/CQPSKDemodulator.java
View File

@@ -2,8 +2,20 @@ package dsp.psk;
/*******************************************************************************
* SDR Trunk
* Copyright (C) 2015 Dennis Sheirer
* 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
@@ -26,153 +38,38 @@ import sample.Provider;
import sample.complex.ComplexSample;
import dsp.filter.interpolator.QPSKInterpolator;
/**
* Implements a CQPSK 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 CQPSKDemodulator implements Listener<ComplexSample>,
Provider<ComplexSample>
{
private final static Logger mLog =
LoggerFactory.getLogger( CQPSKDemodulator.class );
/* Sample Rate 48000 / Symbol Rate 4800 = Samples Per Symbol 10 */
public static final int SAMPLES_PER_SYMBOL = 10;
public static final int d_twice_sps = 20;
public static final int half_sps = 5;
public static final double TWO_PI = 2.0 * Math.PI;
/* CQPSK constellation is offset from normal QPSK by 45 degrees or pi/4 */
public static final float THETA = (float)( Math.PI / 4.0d );
private float d_mu = 10.5f;
private float d_gain_mu = 0.05f;
/* samples per symbol */
private float d_omega = 10.0f;
private float d_half_omega = 5.0f;
private float d_gain_omega = 0.1f * d_gain_mu * d_gain_mu;
private float d_omega_rel = 0.005f;
private float d_min_omega = d_omega * ( 1.0f - d_omega_rel );
private float d_max_omega = d_omega * ( 1.0f + d_omega_rel );
private float d_omega_mid = 10.0f;
private float d_phase = 0.0f;
private float d_freq = 0.0f;
private ComplexSample[] d_dl = new ComplexSample[ 40 ];
private int d_dl_index = 0;
private Listener<ComplexSample> mListener;
private ComplexSample[] mConstellationPoints;
private SymbolTracker mSymbolTracker = new SymbolTracker();
private QPSKPhaseErrorTracker mPhaseTracker = new QPSKPhaseErrorTracker();
private QPSKInterpolator mInterpolator = new QPSKInterpolator();
private ComplexSample d_last_sample = new ComplexSample( 0.0f, 0.0f );
private GardnerDetector mGardnerDetector = new GardnerDetector();
public CQPSKDemodulator()
{
createConstellationPoints();
/* prefill the delay line */
for( int x = 0; x < 40; x++ )
{
d_dl[ x ] = new ComplexSample( 0.0f, 0.0f );
}
}
private void createConstellationPoints()
{
mConstellationPoints = new ComplexSample[ 4 ];
for( int x = 0; x < 4; x++ )
{
mConstellationPoints[ x ] = ComplexSample.fromAngle(
( (float)( TWO_PI / 4.0f ) * (float)x ) );
}
}
@Override
public void receive( ComplexSample sample )
{
/* skip a number of symbols between sampling */
d_mu--;
/* increment phase based on the frequency of the rotation */
d_phase += d_freq;
/* keep phase clamped and not walk to infinity */
unwrapPhase();
/* NCO value for derotating the current sample */
ComplexSample nco = ComplexSample.fromAngle( d_phase + THETA );
/* Mixed/downconverted symbol */
ComplexSample symbol = ComplexSample.multiply( nco, sample );
/* Fill up the delay line for the interpolator */
d_dl[ d_dl_index ] = symbol;
d_dl[ d_dl_index + d_twice_sps ] = symbol;
/* Keep the delay line index in bounds */
d_dl_index = ( d_dl_index + 1 ) % d_twice_sps;
if( d_mu <= 1.0f )
{
ComplexSample interp_samp_mid = mInterpolator.filter( d_dl, d_dl_index, d_mu );
ComplexSample interp_samp = mInterpolator.filter( d_dl, d_dl_index + half_sps, d_mu );
float error_real = ( d_last_sample.real() - interp_samp.real() ) * interp_samp_mid.real();
float error_imag = ( d_last_sample.imaginary() - interp_samp.imaginary() ) * interp_samp_mid.imaginary();
ComplexSample diffdec = ComplexSample.multiply( interp_samp, d_last_sample.conjugate() );
d_last_sample = interp_samp;
float symbol_error = normalize( error_real + error_imag ); //Garner loop error
d_omega = d_omega + d_gain_omega * symbol_error * interp_samp.magnitude();
d_omega = d_omega_mid + clip( d_omega_mid, d_omega_rel );
d_mu += d_omega + d_gain_mu * symbol_error;
// mSymbolTracker.receive( interpolated );
mPhaseTracker.receive( diffdec );
if( mListener != null )
{
// mListener.receive( interp_samp );
mListener.receive( diffdec );
}
}
}
private float normalize( float symbol_error )
{
if( Float.isNaN( symbol_error ) )
{
return 0.0f;
}
if( symbol_error < -1.0 )
{
return -1.0f;
}
if( symbol_error > 1.0 )
{
return 1.0f;
}
return symbol_error;
mGardnerDetector.receive( sample );
}
/**
* Constrains value to the range of ( -maximum <> maximum )
*/
private float clip( float value, float maximum )
public static float clip( float value, float maximum )
{
if( value > maximum )
{
@@ -185,186 +82,285 @@ public class CQPSKDemodulator implements Listener<ComplexSample>,
return value;
}
public static int getQPSKConstellationPoint( ComplexSample sample )
{
if( sample.real() >= 0.0f )
{
if( sample.imaginary() >= 0.0f )
{
return 0;
}
else
{
return 2;
}
}
else
{
if( sample.imaginary() >= 0.0f )
{
return 1;
}
else
{
return 3;
}
}
}
private void unwrapPhase()
{
while( d_phase > TWO_PI )
{
d_phase -= TWO_PI;
}
while( d_phase < -TWO_PI )
{
d_phase += TWO_PI;
}
}
/**
* Mueller & Mueller symbol timing tracker
* Gardner symbol timing detector
*/
public class SymbolTracker implements Listener<ComplexSample>
public class GardnerDetector implements Listener<ComplexSample>
{
ComplexSample mPoint2T = new ComplexSample();
ComplexSample mPoint1T = new ComplexSample();
ComplexSample mPoint0T = new ComplexSample();
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;
ComplexSample mConstellation2T = new ComplexSample();
ComplexSample mConstellation1T = new ComplexSample();
ComplexSample mConstellation0T = new ComplexSample();
private ComplexSample[] mDelayLine =
new ComplexSample[ 2 * TWICE_SAMPLES_PER_SYMBOL ];
private int mDelayLinePointer = 0;
/* Sampling point */
private float mMu = 10.5f;
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 CostasLoop mCostasLoop = new CostasLoop();
private QPSKInterpolator mInterpolator = new QPSKInterpolator();
private ComplexSample mPreviousSample = new ComplexSample( 0.0f, 0.0f );
public GardnerDetector()
{
for( int x = 0; x < ( 2 * TWICE_SAMPLES_PER_SYMBOL ); x++ )
{
mDelayLine[ x ] = new ComplexSample( 0.0f, 0.0f );
}
}
@Override
public void receive( ComplexSample sample )
{
mPoint2T = mPoint1T;
mPoint1T = mPoint0T;
mPoint0T = sample;
mConstellation2T = mConstellation1T;
mConstellation1T = mConstellation0T;
int currentConstellation = getQPSKConstellationPoint( mPoint0T );
mConstellation0T = mConstellationPoints[ currentConstellation ];
/* Count down samples per symbol until we calculate the symbol */
mMu--;
ComplexSample x = ComplexSample.multiply(
ComplexSample.subtract( mConstellation0T, mConstellation2T ),
mPoint1T.conjugate() );
ComplexSample y = ComplexSample.multiply(
ComplexSample.subtract( mPoint0T, mPoint2T ),
mConstellation1T.conjugate() );
mCostasLoop.increment();
ComplexSample u = ComplexSample.subtract( y, x );
/* Mix incoming sample with costas loop to remove any rotation */
ComplexSample unrotatedSample = ComplexSample
.multiply( mCostasLoop.getCurrentVector(), sample );
/* Real part of u contains error -- constrain to -1.0 to 1.0 */
float mm_error = clip( u.real(), 1.0f );
/* Fill up the delay line for the interpolator */
mDelayLine[ mDelayLinePointer ] = unrotatedSample;
mDelayLine[ mDelayLinePointer + TWICE_SAMPLES_PER_SYMBOL ] = unrotatedSample;
/* Keep the delay line pointer in bounds */
mDelayLinePointer = ( mDelayLinePointer + 1 ) % TWICE_SAMPLES_PER_SYMBOL;
/* Calculate the symbol once we've stored enough samples */
if( mMu <= 1.0f )
{
ComplexSample middleSample = mInterpolator
.filter( mDelayLine, mDelayLinePointer, mMu );
middleSample.normalize();
ComplexSample currentSample = mInterpolator
.filter( mDelayLine, mDelayLinePointer +
HALF_SAMPLES_PER_SYMBOL, mMu );
/* Update omega (samples per symbol) based on loop error */
d_omega = d_omega + d_gain_omega * mm_error;
currentSample.normalize();
/* Gardner timing error calculations */
float errorReal = ( mPreviousSample.real() -
currentSample.real() ) * middleSample.real();
float errorImaginary = ( mPreviousSample.imaginary() -
currentSample.imaginary() ) * middleSample.imaginary();
float gardnerError = normalize( errorReal + errorImaginary );
mOmega = mOmega + mGainOmega * gardnerError;
mOmega = mOmegaMid + clip( mOmegaMid, mOmegaRel );
mMu += mOmega + mGainMu * gardnerError;
/* Calculate symbol as delta between previous and current samples */
ComplexSample symbol = ComplexSample.multiply(
currentSample, mPreviousSample.conjugate() );
mPreviousSample = currentSample;
/* Update costas loop phase error from error in current symbol */
mCostasLoop.receive( symbol );
/* Dispatch the symbol to the registered listener */
if( mListener != null )
{
mListener.receive( symbol );
}
}
}
/**
* Constrains timing error to +/- 1
*/
private float normalize( float error )
{
if( Float.isNaN( error ) )
{
return 0.0f;
}
/* Make sure we don't walk away */
d_omega = d_omega_mid + clip( d_omega - d_omega_mid, d_omega_rel );
if( error < -1.0 )
{
return -1.0f;
}
/* update mu based on loop error */
d_mu += d_omega + d_gain_mu * mm_error;
if( error > 1.0 )
{
return 1.0f;
}
return error;
}
}
/**
* Implements a control loop for QPSK
* Costas Loop - phase locked loop synchronized to the incoming signal.
*
* Ported to java from gnuradio/control_loop and gnuradio/mpsk_receiver_cc
* using initialization values from OP25/cqpsk.py
* Ported from gnuradio/control_loop and gnuradio/mpsk_receiver_cc
* using initialization values from KA1RBI's OP25/cqpsk.py
*/
public class QPSKPhaseErrorTracker implements Listener<ComplexSample>
public class CostasLoop implements Listener<ComplexSample>
{
private float d_loop_bw = 0.03f;
private float d_damping = (float)Math.sqrt( 2.0 ) / 2.0f;
private float d_alpha = 0.03f;
private float d_beta = 0.125f * d_alpha * d_alpha;
private float d_max_freq = ( 2.0f * (float)Math.PI * 1200.0f ) / 48000.0f;
public static final double TWO_PI = 2.0 * Math.PI;
public QPSKPhaseErrorTracker()
/* 45 degree rotation angle */
public static final float THETA = (float)( Math.PI / 4.0d );
/* 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 and 200 to adjust control level */
private float mLoopBandwidth = (float)( ( 2.0d * Math.PI ) / 150.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 float mMaximumFrequency = ( 2.0f * (float)Math.PI * 1200.0f ) /
48000.0f;
public CostasLoop()
{
}
/**
* 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 ComplexSample getCurrentVector()
{
return ComplexSample.fromAngle( mLoopPhase + THETA );
}
@Override
public void receive( ComplexSample sample )
{
float phase_error = -getPhaseError( sample );
/* Normalize magnitude of incoming signal to 1.0 */
sample.normalize();
advance_loop(phase_error, sample );
/* Calculate phase error */
float phaseError = getPhaseError( sample );
/* Adjust for phase error */
adjust( phaseError );
/* Maintain phase between +/- 2 * PI */
unwrapPhase();
frequency_limit();
/* Limit frequency to +/- maximum frequency */
limitFrequency();
}
/**
* 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;
}
private void advance_loop( float phase_error, ComplexSample sample )
private void limitFrequency()
{
float abs = sample.magnitude();
d_freq += d_beta * phase_error * abs;
d_phase += d_freq + d_alpha * phase_error * abs;
}
private void frequency_limit()
{
if( d_freq > d_max_freq )
if( mLoopFrequency > mMaximumFrequency )
{
d_freq = d_max_freq;
mLoopFrequency = mMaximumFrequency;
}
if( d_freq < -d_max_freq )
if( mLoopFrequency < -mMaximumFrequency )
{
d_freq = -d_max_freq;
mLoopFrequency = -mMaximumFrequency;
}
}
/**
* Calculates the phase error for a qpsk sample. Standard qpsk samples
* should be aligned along the x/y axis and any deviation from either
* axis represents an error. Whichever component, real or imaginary,
* that has the lesser absolute value, represents the error, since that
* component should be zero valued.
* Calculates the phase error as the difference between the real and
* imaginary values of the sample. When balanced, both absolute values
* should be equal to ~ 0.707. The difference between the real and
* imaginary values is used as the error signal. Mapping the sample
* vector to the correct quadrant allows us to change the sign of the
* error to correctly reflect early (-) or late (+) phase timing.
*
* For example, constellation point 0 is zero degrees and is represented
* by a sample that contains a real = +1 and an imaginary = 0 component.
* If we sample too early or too late, that will cause a +/- phase
* deviation in the resultant sample and the sample will reflect that
* error value in the imaginary component. That deviation is the error.
* Expects a normalized (to unit circle) sample value.
*
* The feedback error signal is small when the sample's phase is closely
* aligned with a perfect constellation and grows larger as the sample's
* phase diverges from alignment with a perfect constellation.
*/
public float getPhaseError( ComplexSample sample )
{
float error = Math.abs( Math.abs( sample.imaginary() ) -
Math.abs( sample.real() ) );
if( sample.realAbsolute() > sample.imaginaryAbsolute() )
{
if( sample.real() > 0 )
if( sample.imaginary() > 0 )
{
return -sample.imaginary();
return sample.real() > 0 ? -error : error;
}
else
{
return sample.imaginary();
return sample.real() > 0 ? error : -error;
}
}
else
{
if( sample.imaginary() > 0 )
if( sample.real() > 0 )
{
return sample.real();
return sample.real() > 0 ? error : -error;
}
else
{
return -sample.real();
return sample.real() > 0 ? -error : error;
}
}
}