diff --git a/src/bits/ISyncDetectListener.java b/src/bits/ISyncDetectListener.java new file mode 100644 index 00000000..dfa4e5f2 --- /dev/null +++ b/src/bits/ISyncDetectListener.java @@ -0,0 +1,6 @@ +package bits; + +public interface ISyncDetectListener +{ + public void syncDetected(); +} diff --git a/src/bits/ISyncProcessor.java b/src/bits/ISyncProcessor.java new file mode 100644 index 00000000..131faca7 --- /dev/null +++ b/src/bits/ISyncProcessor.java @@ -0,0 +1,6 @@ +package bits; + +public interface ISyncProcessor +{ + public void checkSync( long value ); +} diff --git a/src/bits/MultiSyncPatternMatcher.java b/src/bits/MultiSyncPatternMatcher.java new file mode 100644 index 00000000..38f4188a --- /dev/null +++ b/src/bits/MultiSyncPatternMatcher.java @@ -0,0 +1,104 @@ +/******************************************************************************* + * SDR Trunk + * Copyright (C) 2014 Dennis Sheirer + * + * 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 + ******************************************************************************/ +package bits; + +import java.util.ArrayList; +import java.util.List; + +/** + * Multi-Sync pattern matcher. Accepts multiple sync detector objects for + * comparision against the incoming bit stream. + * + * Note: works for sync patterns up to 63 bits (integer size - 1 ) long. + * If a 64 bit or larger sync pattern is applied, then the wrapping feature + * of the Long.rotate methods will wrap the MSB around to the LSB and + * corrupt the matching value. + * + */ +public class MultiSyncPatternMatcher +{ + private List mSyncProcessors = new ArrayList(); + + private long mBits = 0; + private long mMask = 0; + + public MultiSyncPatternMatcher( int syncSize ) + { + assert( syncSize < 64 ); + + //Setup a bit mask of all ones the length of the sync pattern + mMask = (long)( ( Math.pow( 2, syncSize ) ) - 1 ); + } + + /** + * Processes two bits before checking sync processors for a match. + */ + public void receive( boolean bit1, boolean bit2 ) + { + mBits = Long.rotateLeft( mBits, 1 ); + + mBits &= mMask; + + if( bit1 ) + { + mBits += 1; + } + + mBits = Long.rotateLeft( mBits, 1 ); + + mBits &= mMask; + + if( bit2 ) + { + mBits += 1; + } + + for( ISyncProcessor processor: mSyncProcessors ) + { + processor.checkSync( mBits ); + } + } + + /** + * Processes one bit before checking sync processors for a match. + */ + public void receive( boolean bit ) + { + mBits = Long.rotateLeft( mBits, 1 ); + + mBits &= mMask; + + if( bit ) + { + mBits += 1; + } + + for( ISyncProcessor processor: mSyncProcessors ) + { + processor.checkSync( mBits ); + } + } + + /** + * Adds a sync processor to receive the bit stream. + */ + public void add( ISyncProcessor processor ) + { + mSyncProcessors.add( processor ); + } +} diff --git a/src/bits/SoftSyncDetector.java b/src/bits/SoftSyncDetector.java new file mode 100644 index 00000000..9c89f479 --- /dev/null +++ b/src/bits/SoftSyncDetector.java @@ -0,0 +1,38 @@ +package bits; + +public class SoftSyncDetector implements ISyncProcessor +{ + private ISyncDetectListener mListener; + private long mPattern; + private int mThreshold; + + public SoftSyncDetector( ISyncDetectListener listener, long pattern, int threshold ) + { + this( pattern, threshold ); + + mListener = listener; + } + + public SoftSyncDetector( long pattern, int threshold ) + { + mPattern = pattern; + mThreshold = threshold; + } + + @Override + public void checkSync( long value ) + { + long difference = value ^ mPattern; + + if( ( difference == 0 || Long.bitCount( difference ) <= mThreshold ) && + mListener != null ) + { + mListener.syncDetected(); + } + } + + public void setListener( ISyncDetectListener listener ) + { + mListener = listener; + } +} diff --git a/src/bits/SyncDetector.java b/src/bits/SyncDetector.java new file mode 100644 index 00000000..aaeb4b0a --- /dev/null +++ b/src/bits/SyncDetector.java @@ -0,0 +1,33 @@ +package bits; + +public class SyncDetector implements ISyncProcessor +{ + private long mPattern; + private ISyncDetectListener mListener; + + public SyncDetector( long pattern ) + { + mPattern = pattern; + } + + public SyncDetector( long pattern, ISyncDetectListener listener ) + { + this( pattern ); + + mListener = listener; + } + + public void setListener( ISyncDetectListener listener ) + { + mListener = listener; + } + + @Override + public void checkSync( long value ) + { + if( value == mPattern && mListener != null ) + { + mListener.syncDetected(); + } + } +} diff --git a/src/decode/p25/P25Decoder.java b/src/decode/p25/P25Decoder.java index a24928d8..251f6063 100644 --- a/src/decode/p25/P25Decoder.java +++ b/src/decode/p25/P25Decoder.java @@ -88,8 +88,8 @@ public class P25Decoder extends Decoder private C4FMSlicer mC4FMSlicer; private QPSKStarSlicer mCQPSKSlicer; - /* Message framers and handlers */ - private P25MessageFramer mNormalFramer; + /* Message framers and processors */ + private P25MessageFramer mMessageFramer; private P25MessageProcessor mMessageProcessor; /* Audio */ @@ -111,9 +111,19 @@ public class P25Decoder extends Decoder mMessageProcessor = new P25MessageProcessor( mAliasList ); mMessageProcessor.addMessageListener( this ); - mNormalFramer = new P25MessageFramer( - FrameSync.P25_PHASE1.getSync(), 64, false, mAliasList ); - mNormalFramer.setListener( mMessageProcessor ); + if( modulation == Modulation.CQPSK ) + { + /* Provide message framer with reference to the demod so that it + * can detect and issue corrections for costas phase lock errors */ + mCQPSKDemodulator = new CQPSKDemodulator(); + mMessageFramer = new P25MessageFramer( mAliasList, mCQPSKDemodulator ); + } + else + { + mMessageFramer = new P25MessageFramer( mAliasList ); + } + + mMessageFramer.setListener( mMessageProcessor ); /* Setup demodulation chains based on sample type (real or complex) and * modulation (C4FM or CQPSK) */ @@ -136,13 +146,12 @@ public class P25Decoder extends Decoder .getRootRaisedCosine( 10, 34, 0.2 ), 1.0 ); mAGC.setListener( mRootRaisedCosineFilter ); - mCQPSKDemodulator = new CQPSKDemodulator(); mRootRaisedCosineFilter.setListener( mCQPSKDemodulator ); mCQPSKSlicer = new QPSKStarSlicer(); mCQPSKDemodulator.setListener( mCQPSKSlicer ); - mCQPSKSlicer.addListener( mNormalFramer ); + mCQPSKSlicer.addListener( mMessageFramer ); } else /* C4FM */ { @@ -172,7 +181,7 @@ public class P25Decoder extends Decoder mC4FMSlicer = new C4FMSlicer(); mSymbolFilter.setListener( mC4FMSlicer ); - mC4FMSlicer.addListener( mNormalFramer ); + mC4FMSlicer.addListener( mMessageFramer ); } mAudioOutput = new P25AudioOutput( resourceManager ); diff --git a/src/decode/p25/P25MessageFramer.java b/src/decode/p25/P25MessageFramer.java index 7e4286ae..91b714e6 100644 --- a/src/decode/p25/P25MessageFramer.java +++ b/src/decode/p25/P25MessageFramer.java @@ -11,7 +11,10 @@ import sample.Listener; import alias.AliasList; import bits.BinaryMessage; import bits.BitSetFullException; -import bits.SyncPatternMatcher; +import bits.ISyncDetectListener; +import bits.MultiSyncPatternMatcher; +import bits.SoftSyncDetector; +import bits.SyncDetector; import decode.p25.message.P25Message; import decode.p25.message.hdu.HDUMessage; import decode.p25.message.ldu.LDU1Message; @@ -27,20 +30,32 @@ import decode.p25.message.tdu.lc.TDULinkControlMessage; import decode.p25.message.tsbk.TSBKMessage; import decode.p25.message.tsbk.TSBKMessageFactory; import decode.p25.reference.DataUnitID; +import dsp.psk.CQPSKDemodulator; import dsp.symbol.Dibit; +import dsp.symbol.FrameSync; import edac.BCH_63_16_11; import edac.CRC; import edac.CRCP25; public class P25MessageFramer implements Listener { - private static final int SYNC_PATTERN_MISMATCH_THRESHOLD = 2; - private final static Logger mLog = - LoggerFactory.getLogger( P25MessageFramer.class ); + LoggerFactory.getLogger( P25MessageFramer.class ); + + /* Determines the threshold for sync pattern soft matching */ + private static final int SYNC_PATTERN_SOFT_MATCH_THRESHOLD = 4; + + /* Costas Loop phase lock error correction values. A phase lock error of + * 90 degrees requires a correction of 1/4 of the symbol rate (1200Hz). An + * error of 180 degrees requires a correction of 1/2 of the symbol rate + * (2400Hz) */ + public static final double SYMBOL_FREQUENCY = 2.0d * Math.PI * 4800.0d / 48000.0d; + public static final double PHASE_CORRECTION_90_DEGREES = SYMBOL_FREQUENCY / 4.0d; + public static final double PHASE_CORRECTION_180_DEGREES = SYMBOL_FREQUENCY / 2.0d; + private MultiSyncPatternMatcher mMatcher = new MultiSyncPatternMatcher( 48 ); private ArrayList mAssemblers = - new ArrayList(); + new ArrayList(); public static final int TSBK_BEGIN = 64; public static final int TSBK_CRC_START = 144; @@ -60,31 +75,41 @@ public class P25MessageFramer implements Listener private Listener mListener; private AliasList mAliasList; - private SyncPatternMatcher mMatcher; - private boolean mInverted = false; + private Trellis_1_2_Rate mHalfRate = new Trellis_1_2_Rate(); private Trellis_3_4_Rate mThreeQuarterRate = new Trellis_3_4_Rate(); private BCH_63_16_11 mNIDDecoder = new BCH_63_16_11(); /** - * Constructs a C4FM message framer to receive a stream of C4FM symbols and + * Constructs a P25 message framer to receive a stream of symbols and * detect the sync pattern then capture the following stream of symbols up * to the message length, and then broadcast that bit buffer to the registered * listener. - * - * @param sync - sync pattern (maximum of 63 bits) - * @param messageLength - in bits - * @param inverted - optional flag to indicate the symbol stream should be - * uninverted prior to processing */ - public P25MessageFramer( long sync, - int messageLength, - boolean inverted, - AliasList aliasList ) + public P25MessageFramer( AliasList aliasList ) { - mMatcher = new SyncPatternMatcher( sync, SYNC_PATTERN_MISMATCH_THRESHOLD ); - mInverted = inverted; mAliasList = aliasList; + + SoftSyncDetector primarySyncDetector = new SoftSyncDetector( + FrameSync.P25_PHASE1_NORMAL.getSync(), SYNC_PATTERN_SOFT_MATCH_THRESHOLD ); + + primarySyncDetector.setListener( new ISyncDetectListener() + { + @Override + public void syncDetected() + { + for( P25MessageAssembler assembler: mAssemblers ) + { + if( !assembler.isActive() ) + { + assembler.setActive( true ); + break; + } + } + } + } ); + + mMatcher.add( primarySyncDetector ); /** * We use two message assemblers to catch any sync detections, so that @@ -98,6 +123,22 @@ public class P25MessageFramer implements Listener mAssemblers.add( new P25MessageAssembler() ); } + public P25MessageFramer( AliasList aliasList, CQPSKDemodulator demodulator ) + { + this( aliasList ); + + /* For CQPSK, we include 3 additional sync detectors to watch for and + * correction +/-90 and 180 degree costas loop phase lock errors */ + mMatcher.add( new CostasPhaseErrorDetector( FrameSync.P25_PHASE1_ERROR_90_CCW, + demodulator, PHASE_CORRECTION_90_DEGREES ) ); + + mMatcher.add( new CostasPhaseErrorDetector( FrameSync.P25_PHASE1_ERROR_90_CW, + demodulator, -PHASE_CORRECTION_90_DEGREES ) ); + + mMatcher.add( new CostasPhaseErrorDetector( FrameSync.P25_PHASE1_ERROR_180, + demodulator, PHASE_CORRECTION_180_DEGREES ) ); + } + private void dispatch( Message message ) { if( mListener != null ) @@ -109,9 +150,6 @@ public class P25MessageFramer implements Listener @Override public void receive( Dibit symbol ) { - mMatcher.receive( symbol.getBit1() ); - mMatcher.receive( symbol.getBit2() ); - for( P25MessageAssembler assembler: mAssemblers ) { if( assembler.isActive() ) @@ -125,27 +163,7 @@ public class P25MessageFramer implements Listener } } - /* Check for sync match and activate a message assembler, if we can - * find an inactive assembler. Otherwise, ignore and log the issue */ - if( mMatcher.matches() ) - { - boolean found = false; - - for( P25MessageAssembler assembler: mAssemblers ) - { - if( !assembler.isActive() ) - { - assembler.setActive( true ); - found = true; - break; - } - } - - if( !found ) - { - mLog.debug( "no inactive P25 message assemblers available" ); - } - } + mMatcher.receive( symbol.getBit1(), symbol.getBit2() ); } public void setListener( Listener listener ) @@ -258,12 +276,10 @@ public class P25MessageFramer implements Listener break; case HDU: mComplete = true; - mMatcher.setSoftMode( true ); dispatch( new HDUMessage( mMessage.copy(), mDUID, mAliasList ) ); break; case LDU1: mComplete = true; - mMatcher.setSoftMode( true ); LDU1Message ldu1 = new LDU1Message( mMessage.copy(), mDUID, mAliasList ); @@ -273,11 +289,9 @@ public class P25MessageFramer implements Listener break; case LDU2: mComplete = true; - mMatcher.setSoftMode( true ); dispatch( new LDU2Message( mMessage.copy(), mDUID, mAliasList ) ); break; case PDU0: - mMatcher.setSoftMode( false ); /* Remove interleaving */ P25Interleave.deinterleaveData( mMessage, PDU0_BEGIN, PDU0_END ); @@ -397,8 +411,6 @@ public class P25MessageFramer implements Listener mComplete = true; break; case PDUC: - mMatcher.setSoftMode( true ); - /* De-interleave the latest block*/ P25Interleave.deinterleaveData( mMessage, mMessage.size() - 196, mMessage.size() ); @@ -447,12 +459,10 @@ public class P25MessageFramer implements Listener } break; case TDU: - mMatcher.setSoftMode( true ); dispatch( new TDUMessage( mMessage.copy(), mDUID, mAliasList ) ); mComplete = true; break; case TDULC: - mMatcher.setSoftMode( true ); TDULinkControlMessage tdulc = new TDULinkControlMessage( mMessage.copy(), mDUID, mAliasList ); @@ -463,7 +473,6 @@ public class P25MessageFramer implements Listener mComplete = true; break; case TSBK1: - mMatcher.setSoftMode( true ); /* Remove interleaving */ P25Interleave.deinterleaveData( mMessage, TSBK_BEGIN, TSBK_END ); @@ -602,4 +611,50 @@ public class P25MessageFramer implements Listener return mActive; } } + + /** + * Sync pattern detector to listen for costas loop phase lock errors and + * apply a phase correction to the costas loop so that we don't miss any + * messages. + * + * When the costas loop locks with a +/- 90 degree or 180 degree phase + * error, the slicer will incorrectly apply the symbol pattern rotated left + * or right by the phase error. However, we can detect these rotated sync + * patterns and apply immediate phase correction so that message processing + * can continue. + */ + public class CostasPhaseErrorDetector extends SyncDetector + { + private CQPSKDemodulator mDemodulator; + private double mCorrection; + + public CostasPhaseErrorDetector( FrameSync frameSync, + CQPSKDemodulator demodulator, double correction ) + { + super( frameSync.getSync() ); + + mDemodulator = demodulator; + + mCorrection = correction; + + setListener( new ISyncDetectListener() + { + @Override + public void syncDetected() + { + mDemodulator.correctPhaseError( mCorrection ); + + /* Since we detected a sync pattern, start a message assembler */ + for( P25MessageAssembler assembler: mAssemblers ) + { + if( !assembler.isActive() ) + { + assembler.setActive( true ); + break; + } + } + } + } ); + } + } } diff --git a/src/dsp/psk/CQPSKDemodulator.java b/src/dsp/psk/CQPSKDemodulator.java index 3eb4bd61..84eed452 100644 --- a/src/dsp/psk/CQPSKDemodulator.java +++ b/src/dsp/psk/CQPSKDemodulator.java @@ -76,6 +76,8 @@ public class CQPSKDemodulator implements Instrumentable, private GardnerSymbolTiming mGardnerDetector = new GardnerSymbolTiming(); + private CostasLoop mCostasLoop = new CostasLoop(); + private FrequencyChangeListener mFrequencyChangeListener; private EyeDiagramDataTap mEyeDiagramDataTap; @@ -90,6 +92,17 @@ public class CQPSKDemodulator implements Instrumentable, mGardnerDetector.receive( sample ); } + /** + * 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 ) */ @@ -207,7 +220,7 @@ public class CQPSKDemodulator implements Instrumentable, /* Sampling point */ private float mMu = 10.0f; - private float mGainMu = 0.25f; + private float mGainMu = 0.1f; /* Samples per symbol */ private float mOmega = 10.0f; @@ -215,8 +228,6 @@ public class CQPSKDemodulator implements Instrumentable, private float mOmegaRel = 0.005f; private float mOmegaMid = 10.0f; - private CostasLoop mCostasLoop = new CostasLoop(); - private QPSKInterpolator mInterpolator = new QPSKInterpolator( 1.0f ); private ComplexSample mPreviousSample = new ComplexSample( 0.0f, 0.0f ); @@ -347,7 +358,7 @@ public class CQPSKDemodulator implements Instrumentable, public static final double TWO_PI = 2.0 * Math.PI; private static final float MAXIMUM_FREQUENCY = - ( 1200.0f * (float)TWO_PI ) / 48000.0f; + ( 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; @@ -374,6 +385,33 @@ public class CQPSKDemodulator implements Instrumentable, { } + /** + * 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 */ diff --git a/src/dsp/psk/QPSKStarSlicer.java b/src/dsp/psk/QPSKStarSlicer.java index 74778746..44b80fcf 100644 --- a/src/dsp/psk/QPSKStarSlicer.java +++ b/src/dsp/psk/QPSKStarSlicer.java @@ -13,9 +13,9 @@ public class QPSKStarSlicer implements Listener * Slices a ComplexSample representing a phase shifted symbol according to * the following constellation pattern: * - * 11 | 01 + * 00 | 01 * ---|--- - * 10 | 00 + * 10 | 11 */ public QPSKStarSlicer() { diff --git a/src/dsp/symbol/FrameSync.java b/src/dsp/symbol/FrameSync.java index 523d4fcf..66eb611f 100644 --- a/src/dsp/symbol/FrameSync.java +++ b/src/dsp/symbol/FrameSync.java @@ -2,8 +2,10 @@ package dsp.symbol; public enum FrameSync { - P25_PHASE1( 0x5575F5FF77FFl ), //+3 +3 +3 +3 +3 -3 +3 +3 -3 -3 +3 +3 -3 -3 -3 -3 +3 -3 +3 -3 -3 -3 -3 -3 - P25_PHASE1_INVERTED( 0xFFDF5F55DD55l ); + P25_PHASE1_ERROR_90_CCW( 0xFFEFAFAAEEAAl ), + P25_PHASE1_NORMAL( 0x5575F5FF77FFl ), // +33333 -3 +33 -33 +33 -3333 +3 -3 +3 -33333 + P25_PHASE1_ERROR_90_CW( 0x001050551155l ), + P25_PHASE1_ERROR_180( 0xAA8A0A008800l ); private long mSync; diff --git a/src/sample/complex/ComplexSample.java b/src/sample/complex/ComplexSample.java index de67df53..4c220f47 100644 --- a/src/sample/complex/ComplexSample.java +++ b/src/sample/complex/ComplexSample.java @@ -293,6 +293,14 @@ public class ComplexSample implements Serializable return (float)Math.atan2( y(), x() ); } + /** + * Angle of this sample in degrees + */ + public float polarAngle() + { + return (float)Math.toDegrees( angle() ); + } + /** * Provides an approximate magnitude value for this sample. */ @@ -313,13 +321,18 @@ public class ComplexSample implements Serializable public static void main( String[] args ) { - ComplexSample s1 = new ComplexSample( 0.24414134f, -0.6026385f ); - ComplexSample s2 = new ComplexSample( -0.09531293f, -0.7052474f ); + double SYMBOL_FREQUENCY = 2.0d * Math.PI * 4800.0d / 48000.0d; + double PHASE_CORRECTION_90_DEGREES = SYMBOL_FREQUENCY / 4.0d; + double PHASE_CORRECTION_180_DEGREES = SYMBOL_FREQUENCY / 2.0d; + + double angle = Math.PI / 4; + + ComplexSample s1 = new ComplexSample( (float)Math.sin( angle ), (float)Math.cos( angle ) ); + ComplexSample s2 = new ComplexSample( (float)Math.sin( -angle ), (float)Math.cos( -angle ) ); ComplexSample symbol = ComplexSample.multiply( s1, s2.conjugate() ); - mLog.debug( "S1 Mag:" + s1.magnitude() + " S2 Mag:" + s2.magnitude() + " SymMag:" + symbol.magnitude() ); - mLog.debug( "S1:" + s1 + " S2:" + s2 + " Symbol:" + symbol ); + mLog.debug( "S1:" + s1.polarAngle() + " S2:" + s2.polarAngle() + " Symbol:" + symbol.polarAngle() ); } }