← Back to branch summary

~ubuntu-branches/ubuntu/quantal/linpsk/quantal

« back to all changes in this revision

Viewing changes to linpsk/cpskdemodulator.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Bruce Walker
  • Date: 2002-02-06 11:43:38 UTC
  • Revision ID: james.westby@ubuntu.com-20020206114338-xqmjmhh01lpjm0g4
Tags: upstream-0.6.2
Import upstream version 0.6.2

Show diffs side-by-side

added added

removed removed

Lines of Context:
linpsk/cpskdemodulator.cpp
 
1
/***************************************************************************
 
2
                          cpskdemodulator.cpp  -  description
 
3
                             -------------------
 
4
    begin                : Sat Jun 2 2001
 
5
    copyright            : (C) 2001 by Volker Schroer
 
6
    email                : dl1ksv@gmx.de
 
7
 ***************************************************************************/
 
8
 
 
9
/***************************************************************************
 
10
 *                                                                         *
 
11
 *   This program is free software; you can redistribute it and/or modify  *
 
12
 *   it under the terms of the GNU General Public License as published by  *
 
13
 *   the Free Software Foundation; either version 2 of the License, or     *
 
14
 *   (at your option) any later version.                                   *
 
15
 *    based on the work of  Moe Wheatly, AE4JY                             *  
 
16
 ***************************************************************************/
 
17
 
 
18
 
 
19
 
 
20
 
 
21
#include "cpskdemodulator.h"
 
22
#include "fircoeffs.h"
 
23
#include "psktable.h"
 
24
 
 
25
 
 
26
//define the PSK31 symbols
 
27
#define SYM_NOCHANGE 0  //Stay the same phase
 
28
#define SYM_P90 1               //Plus 90  deg
 
29
#define SYM_P180 2              //Plus 180 deg
 
30
#define SYM_M90 3               //Minus 90 deg
 
31
 
 
32
// phase wraparound correction table for viterbi decoder
 
33
const double AngleTbl1[4] = { 3.0*PI2/4.0, 0.0, PI2/4.0, PI2/2.0};
 
34
const double AngleTbl2[4] = { 3.0*PI2/4.0, PI2, PI2/4.0, PI2/2.0 };
 
35
 
 
36
 
 
37
//////////////////////////////////////////////////////////////////////
 
38
// Construction/Destruction
 
39
//////////////////////////////////////////////////////////////////////
 
40
 
 
41
CPskDemodulator::CPskDemodulator(): CDemodulator()
 
42
{
 
43
        m_pQue1 = NULL;
 
44
        m_pQue2 = NULL;
 
45
        m_pQue3 = NULL;
 
46
        m_VaricodeDecTbl = NULL;
 
47
}
 
48
 
 
49
CPskDemodulator::~CPskDemodulator()
 
50
{
 
51
        if(m_pQue1)
 
52
                delete m_pQue1;
 
53
        if(m_pQue2)
 
54
                delete m_pQue2;
 
55
        if(m_pQue3)
 
56
                delete m_pQue3;
 
57
        if(m_VaricodeDecTbl)
 
58
                delete m_VaricodeDecTbl;
 
59
 
 
60
}
 
61
 
 
62
/////////////////////////////////////////////////////////////////
 
63
//       Initialize PskDet buffers and pointers
 
64
/////////////////////////////////////////////////////////////////
 
65
bool CPskDemodulator::Init( double Fs, int BlockSize )
 
66
{
 
67
unsigned short int wTemp;
 
68
int i;
 
69
//circular delay lines.(data stays put and the pointers move)
 
70
        if( (m_pQue1 = new double_complex[sizeof(double_complex)*DEC3_LPFIR_LENGTH] ) == NULL)
 
71
                return false;
 
72
        if( (m_pQue2 = new double_complex[sizeof(double_complex)*DEC3_LPFIR_LENGTH] ) == NULL)
 
73
                return false;
 
74
        if( (m_pQue3 = new double_complex[sizeof(double_complex)*BITFIR_LENGTH] ) == NULL)
 
75
                return false;
 
76
        if( (   m_VaricodeDecTbl = new unsigned char[sizeof(unsigned char)*2048] )== NULL)
 
77
                return false;
 
78
        for(int i=0; i<DEC3_LPFIR_LENGTH; i++)
 
79
        {
 
80
        // fill delay buffer with zero
 
81
                m_pQue1[i] = double_complex(0.0,0.0);
 
82
        }
 
83
        for(i=0; i<DEC3_LPFIR_LENGTH; i++)
 
84
        {
 
85
        // fill delay buffer with zero
 
86
                m_pQue2[i] = double_complex(0.0,0.0);
 
87
 
 
88
        }
 
89
        for( i=0; i<BITFIR_LENGTH; i++)
 
90
        {
 
91
        // fill delay buffer with zero
 
92
                m_pQue3[i] = double_complex(0.0,0.0);
 
93
        }
 
94
 
 
95
        for( int j=0; j<2048; j++)              //init inverse varicode lookup decoder table
 
96
        {
 
97
                m_VaricodeDecTbl[j] = 0;
 
98
                for(int i=0; i<256;i++)
 
99
 
 
100
                {
 
101
                        wTemp = VARICODE_TABLE[i];
 
102
                        wTemp >>= 4;
 
103
                        while( !(wTemp&1) )
 
104
                                wTemp >>= 1;
 
105
                        wTemp >>= 1;
 
106
                        if( wTemp == j)
 
107
                                m_VaricodeDecTbl[j] = (unsigned char)i;
 
108
                }
 
109
        }
 
110
        m_pInPtr1 = m_pQue1;            //initialize que ptrs
 
111
        m_pInPtr2 = m_pQue2;
 
112
        m_pInPtr3 = m_pQue3;
 
113
        m_Fs = Fs;                                      //sample rate
 
114
        m_BlockSize = BlockSize;        //size data input buffer
 
115
        m_phzinc = 1000.0*PI2/m_Fs;     
 
116
        m_BitPhaseInc = 9.0/m_Fs;               //bit oversampling period
 
117
        m_SignalLevel = 1.0;
 
118
        m_BitPhasePos = 0.0;
 
119
        m_BitAcc = 0;
 
120
        m_LastBitZero = false;
 
121
        m_SampCnt = 0;
 
122
        m_OnCount = 0;
 
123
        m_OffCount = 0;
 
124
 
 
125
        for( i=0; i<21; i++)
 
126
                m_SyncAve[i] = 0.0;                                     // initialize the array
 
127
 
 
128
 
 
129
// Init a bunch of "static" variables used in various member fuctions
 
130
        m_AGCave = 0.0;
 
131
        m_FreqError = 0.0;
 
132
        m_VcoPhz = 0.0;
 
133
        m_LastFreq = 1000.0;
 
134
        m_PkPos = 0;
 
135
        m_NewPkPos = 5;
 
136
        m_BitPos = 0;
 
137
        m_I1 = 0.0;
 
138
        m_I0 = 0.0;
 
139
        m_Q1 = 0.0;
 
140
        m_Q0 = 0.0;
 
141
        m_DevAve = .4;
 
142
 
 
143
  m_z1=double_complex(0.0,0.0);
 
144
        m_z2=m_z1;
 
145
        m_FerAve = 0.0;
 
146
        m_QFreqError = 0.0;
 
147
        m_LastPkPos = 0;
 
148
        m_ClkErrCounter = 0;
 
149
        m_ClkErrTimer = 0;
 
150
        m_ClkError = 0;
 
151
// Initializing Variables for AFC
 
152
        fe1=0.0;
 
153
        fe2=0.0;
 
154
        ferror=0.0;
 
155
        ferror1=0.0;
 
156
        ferror2=0.0;
 
157
        dp=0.0;
 
158
        dp1=0.0;
 
159
        dp2=0.0;
 
160
                
 
161
return true;
 
162
}
 
163
 
 
164
//////////////////////////////////////////////////////////////////////
 
165
// Main routine called to process the next block of data 'pIn'.
 
166
//  The center frequency is specified by 'Freq'.
 
167
//  The function returns the new AFC'd frequency.
 
168
//  'UseAFC' specifies whether to automatically lock to the frequency.
 
169
//  30mSec for BPSK, 38mSec for QPSK( 133MHz Pentium )
 
170
//////////////////////////////////////////////////////////////////////
 
171
 
 
172
void CPskDemodulator::ProcessInput( double* pIn)
 
173
{
 
174
double freqerror;
 
175
double vcophz = m_VcoPhz;
 
176
int i,j;
 
177
double_complex acc;
 
178
const double* Kptr;
 
179
double_complex* Firptr;
 
180
double_complex* Inptr1;                 //decimation FIR #1 variables
 
181
double_complex* Qptr1;
 
182
double_complex* Inptr2;                 //decimation FIR #2 variables
 
183
double_complex* Qptr2;
 
184
 
 
185
if ( UseAfc)
 
186
                freqerror= m_FreqError;
 
187
        else
 
188
                freqerror= 0.0; 
 
189
 
 
190
        m_DispTrig = false;
 
191
        j=0;
 
192
        if( RxFrequency != m_LastFreq ) //caller is changing center frequency
 
193
        {                                                                                                       //so recalc freq error value
 
194
///             freqerror = freqerror + m_phzinc;       //current center freq inc
 
195
                m_phzinc = (PI2/m_Fs)*RxFrequency;      //new center freq inc
 
196
///             if( UseAfc )
 
197
///                     freqerror = m_phzinc - freqerror;
 
198
///             else
 
199
                        freqerror = 0.0;
 
200
                m_LastFreq = RxFrequency;
 
201
        }
 
202
        Inptr1 = m_pInPtr1;             //use local copies of member variables
 
203
        Qptr1 = m_pQue1;                // for better speed.
 
204
        Inptr2 = m_pInPtr2;
 
205
        Qptr2 = m_pQue2;
 
206
        for( i = 0; i<m_BlockSize; i++ )        // put new samples into Queue
 
207
        {
 
208
                if( --Inptr1 < Qptr1 )          //deal with FIR pointer wrap around
 
209
                        Inptr1 = Qptr1+DEC3_LPFIR_LENGTH-1;
 
210
//Generate complex sample by mixing input sample with NCO's sin/cos
 
211
                vcophz = vcophz + (m_phzinc + freqerror);
 
212
//                      *Inptr1=pIn[i]*exp(float_complex(0,vcophz));
 
213
                *Inptr1=pIn[i]*double_complex(cos(vcophz),sin(vcophz));
 
214
//              vcophz = vcophz + (m_phzinc + freqerror);
 
215
                if(vcophz > PI2)                //handle 2 Pi wrap around
 
216
                        vcophz -= PI2;
 
217
//decimate by 3 filter
 
218
//  10uSec( 133MHz Pentium )
 
219
                if( ( (++m_SampCnt)%3 ) == 0 )  //calc first decimation filter every 3 samples
 
220
                {
 
221
 
 
222
                        acc=double_complex(0.0,0.0);
 
223
                        Firptr = Inptr1;
 
224
                        Kptr = Dec3LPCoef;
 
225
                        while( Kptr < (Dec3LPCoef + DEC3_LPFIR_LENGTH) )        //do the MAC's
 
226
                        {
 
227
 
 
228
                                acc +=(*Firptr)*(*Kptr++);
 
229
                                if( (++Firptr) >= Qptr1+DEC3_LPFIR_LENGTH ) //deal with wraparound
 
230
                                        Firptr = Qptr1;
 
231
                        }
 
232
                        if( --Inptr2 < Qptr2 )          //deal with FIR pointer wrap around
 
233
                                Inptr2 = Qptr2+DEC3_LPFIR_LENGTH-1;
 
234
 
 
235
                                *Inptr2=acc;
 
236
 
 
237
//decimate by 3 filter
 
238
//  10uSec( 133MHz Pentium )
 
239
                        if( (m_SampCnt%9) == 0 )        //calc second decimation filter every 9 samples
 
240
                        {
 
241
 
 
242
                                acc=double_complex(0.0,0.0);
 
243
                                Firptr = Inptr2;
 
244
                                Kptr = Dec3LPCoef;
 
245
                                while( Kptr < (Dec3LPCoef + DEC3_LPFIR_LENGTH) )        //do the MAC's
 
246
                                {
 
247
 
 
248
                                        acc=(*Firptr)*(*Kptr);
 
249
                                        Kptr++;
 
250
                                        if( (++Firptr) >= Qptr2+DEC3_LPFIR_LENGTH ) //deal with wraparound
 
251
                                                Firptr = Qptr2;
 
252
                                }
 
253
// here at Fs/9 = 612.5 Hz rate with latest sample in acc.
 
254
// Matched Filter the I and Q data and also a frequency error filter
 
255
//      filter puts filtered signals in m_FreqSignal and m_BitSignal.
 
256
                                CalcBitFilter( acc );           // 25 uSec( 133MHz Pentium )
 
257
// Perform AGC operation
 
258
                                m_SignalLevel = CalcAGC( m_FreqSignal );        // 9 uSec( 133MHz Pentium )
 
259
 
 
260
                                if( m_DispTrig  )       //j ranges 0 to 2048/9 or 227
 
261
                                
 
262
                                        emit newPhaseValue(j++,float_complex(m_BitSignal));
 
263
// Calculate frequency error
 
264
 
 
265
                                if( UseAfc )
 
266
                                        freqerror = CalcFreqError(m_FreqSignal);        // 10 uSec( 133MHz Pentium )
 
267
//                      Perhaps I should introduce a narrow / wid switch for the AFC
 
268
//                              freqerror = CalcFreqError(m_BitSignal); // 10 uSec( 133MHz Pentium )
 
269
                                else
 
270
                                        {
 
271
#if AFC_DEBUG
 
272
                                        freqerror = CalcFreqError(m_BitSignal); // 10 uSec( 133MHz Pentium )
 
273
#endif                                  
 
274
                                        freqerror = 0.0;
 
275
                                        }
 
276
// Bit Timing synchronization
 
277
                                
 
278
                                if( SymbSync(m_BitSignal) )     // 8 uSec( 133MHz Pentium )
 
279
                                {
 
280
                                        DecodeSymbol( m_BitSignal);             //615uS(QPSK)
 
281
                                
 
282
                                }       m_DispTrig = true;
 
283
                        }
 
284
                }
 
285
        }
 
286
        m_SampCnt = m_SampCnt%9;
 
287
        m_pInPtr1 = Inptr1;             // save position in circular delay line
 
288
        m_pInPtr2 = Inptr2;             // save position in circular delay line
 
289
        
 
290
        m_FreqError = freqerror;
 
291
        m_VcoPhz = vcophz;
 
292
        
 
293
                if (UseAfc)                     // Change RxFrequency, but slowly
 
294
                {
 
295
                        freqerror=freqerror*m_Fs/PI2;           
 
296
                        RxFrequency = RxFrequency + freqerror;
 
297
                        emit rxFrequencyChanged(RxFrequency);
 
298
                }
 
299
        
 
300
 
 
301
  settings.clockerror=m_ClkError;
 
302
}
 
303
 
 
304
 
 
305
//////////////////////////////////////////////////////////////////////
 
306
// Called at Fs/9 rate to calculate the symbol sync position
 
307
// Returns true if at center of symbol.
 
308
// Sums up the energy at each sample time, averages it, and picks the
 
309
//   sample time with the highest energy content.
 
310
//////////////////////////////////////////////////////////////////////
 
311
 
 
312
bool CPskDemodulator::SymbSync(double_complex sample)
 
313
 
 
314
{
 
315
bool Trigger;
 
316
double energy;
 
317
 
 
318
 
 
319
int BitPos = m_BitPos;
 
320
        energy = sample.real()*sample.real()+sample.imag()*sample.imag();
 
321
        if( energy > 4.0)               //wait for AGC to settle down
 
322
                energy = 1.0;
 
323
        m_SyncAve[BitPos] = (1.0-1.0/100.0)*m_SyncAve[BitPos] + (1.0/100.0)*energy;
 
324
        if( BitPos == m_PkPos ) // see if at middle of symbol
 
325
        {
 
326
                Trigger = true;
 
327
//              display->m_SyncHist[m_PkPos] = (int)(10000.0*m_SyncAve[m_PkPos]);
 
328
        }
 
329
        else
 
330
        {
 
331
                Trigger = false;
 
332
//              display->m_SyncHist[BitPos] = (int)(7000.0*m_SyncAve[BitPos]);
 
333
 
 
334
        }
 
335
        if( BitPos == HALF_TBL[m_PkPos] )       //don't change pk pos until
 
336
                m_PkPos = m_NewPkPos;                   // halfway into next bit.
 
337
        BitPos++;
 
338
        m_BitPhasePos += (m_BitPhaseInc);
 
339
        if( m_BitPhasePos >= Ts )
 
340
        {                                                                       // here every symbol time
 
341
                m_BitPhasePos -= Ts;                    //keep phase bounded
 
342
                BitPos = 0;
 
343
                max = -1e10;
 
344
                for( int i=0; i<19; i++)                //find maximum energy pk
 
345
                {
 
346
                        energy = m_SyncAve[i];
 
347
                        if( energy > max )
 
348
                        {
 
349
                                m_NewPkPos = i;
 
350
                                max = energy;
 
351
                        }
 
352
                }
 
353
                if( m_PkPos == m_LastPkPos+1 )  //calculate clock error
 
354
                        m_ClkErrCounter++;
 
355
                else
 
356
                        if( m_PkPos == m_LastPkPos-1 )
 
357
                                m_ClkErrCounter--;
 
358
                if( m_ClkErrTimer++ > 313 )     // every 10 seconds sample clk drift
 
359
                {
 
360
                        m_ClkError = m_ClkErrCounter*180;
 
361
                        m_ClkErrCounter = 0;
 
362
                        m_ClkErrTimer = 0;
 
363
                }
 
364
                m_LastPkPos = m_PkPos;
 
365
        }
 
366
        m_BitPos = BitPos;
 
367
        return Trigger;
 
368
}
 
369
 
 
370
 
 
371
//////////////////////////////////////////////////////////////////////
 
372
//  Frequency error calculator
 
373
// calculates the derivative of the tan(I/Q).
 
374
// returns frequency error ~= .0034 per Hz error.
 
375
//////////////////////////////////////////////////////////////////////
 
376
double CPskDemodulator::CalcFreqError( double_complex IQ )
 
377
{
 
378
 
 
379
 
 
380
double ftotal ;
 
381
 
 
382
ferror2=ferror1;
 
383
ferror1=ferror;
 
384
dp2=dp1;
 
385
dp1=m_QFreqError;
 
386
dp=0.98*dp+0.01*dp1-0.01*dp2;
 
387
ferror = (IQ.real() - m_z2.real()) * m_z1.imag() - (IQ.imag() - m_z2.imag()) * m_z1.real();
 
388
 
 
389
        m_z2 = m_z1;
 
390
        m_z1 = IQ;
 
391
        if( ferror > .15 )              //clamp range
 
392
                ferror = .15;
 
393
        if( ferror < -.15 )
 
394
                ferror = -.15;
 
395
//ftotal=0.03*ferror+0.02*ferror1-0.02*ferror2+0.5*fe1+0.4*fe2;
 
396
ftotal=0.01*ferror+0.01*ferror1-0.01*ferror2+0.5*fe1+0.4*fe2;
 
397
fe2=fe1;
 
398
fe1=ftotal;
 
399
 
 
400
                m_FerAve = 0.99 *m_FerAve + 0.008*ftotal + 0.002 * dp;
 
401
 
 
402
                return m_FerAve;
 
403
}
 
404
 
 
405
//////////////////////////////////////////////////////////////////////
 
406
// Automatic gain control calculator
 
407
//////////////////////////////////////////////////////////////////////
 
408
double CPskDemodulator::CalcAGC( double_complex Samp)
 
409
{
 
410
double mag;
 
411
 
 
412
        mag = sqrt(Samp.real()*Samp.real() + Samp.imag()*Samp.imag());
 
413
        if( mag > m_AGCave )
 
414
 
 
415
                m_AGCave = (1.0-1.0/250.0)*m_AGCave + (1.0/250.0)*mag;
 
416
        else
 
417
                m_AGCave = (1.0-1.0/1000.0)*m_AGCave + (1.0/1000.0)*mag;
 
418
        if( m_AGCave >= 1.0 )   // divide signal by ave if not almost zero
 
419
        {
 
420
                m_BitSignal /= m_AGCave;
 
421
//              m_BitSignal.y /= m_AGCave;
 
422
                m_FreqSignal /= m_AGCave;
 
423
//              m_FreqSignal.y /= m_AGCave;
 
424
        }
 
425
        return(m_AGCave);
 
426
}
 
427
 
 
428
 
 
429
//////////////////////////////////////////////////////////////////////
 
430
// BIT FIR filters. A narrow matched(?) data filter for data
 
431
//                  and wider filter for the AFC/AGC functions
 
432
//////////////////////////////////////////////////////////////////////
 
433
void CPskDemodulator::CalcBitFilter(double_complex Samp)
 
434
{
 
435
double_complex acc1;
 
436
double_complex acc2;
 
437
const double* Kptr1;
 
438
const double* Kptr2;
 
439
double_complex* Firptr;
 
440
double_complex* Qptr;
 
441
double_complex* Inptr;
 
442
        Inptr = m_pInPtr3;              //use local copies of member variables
 
443
        Qptr = m_pQue3;                 // for better speed.
 
444
        if( --Inptr < Qptr )    //deal with LPFIR pointer wrap around
 
445
                Inptr = Qptr+BITFIR_LENGTH-1;
 
446
//      Inptr->x = Samp.x;      //place in circular Queue
 
447
        *Inptr = Samp;
 
448
//      acc1.x = 0.0;
 
449
        acc1 = double_complex(0.0,0.0);
 
450
//      acc2.x = 0.0;
 
451
        acc2 = double_complex(0.0,0.0);
 
452
        Firptr = Inptr;
 
453
        Kptr1 = FreqFirCoef;    //frequency error filter
 
454
        Kptr2 = BitFirCoef;     //bit data filter
 
455
        while( Kptr2 < (BitFirCoef + BITFIR_LENGTH) )   //do the MAC's
 
456
        {
 
457
//              acc1.x += ( (Firptr->x)*(*Kptr1) );
 
458
                acc1 += ( (*Firptr)*(*Kptr1++) );
 
459
                                                
 
460
//              acc2.x += ( (Firptr->x)*(*Kptr2) );
 
461
                acc2 += ( (*Firptr)*(*Kptr2++) );
 
462
                                                
 
463
                if( (++Firptr) >= (Qptr+BITFIR_LENGTH) ) //deal with wraparound
 
464
                        Firptr = Qptr;
 
465
        }
 
466
        m_pInPtr3 = Inptr;              // save position in circular delay line
 
467
//      m_FreqSignal.x = acc1.x;
 
468
        m_FreqSignal = acc1;
 
469
//      m_BitSignal.x = acc2.x;
 
470
        m_BitSignal = acc2;
 
471
}
 
472
 
 
473
 
 
474
 
 
475
 
 
476