1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
|
// fskdemod.cpp: implementation of the CFskDemod class.
//
// This class takes I/Q baseband data and performs
// FSK demodulation and basic DSC decode into raw bytes
// History:
// 2012-10-19 Initial creation MSW
// 2017-09-17 Modified MSW
//////////////////////////////////////////////////////////////////////
#include "fskdemod.h"
#include "gui/testbench.h"
#include "dsp/datatypes.h"
#include "gui/chatdialog.h"
#include "dsp/fircoef.h"
#include <QDebug>
#define SHIFT_FREQ 170.0
//ATC time constants in seconds
#define ATTACK_TIMECONST 0.025
#define DECAY_TIMECONST 0.3
////////////////////////////////////////////////////////////////////////////////
//index to table is 10 bit DSC symbol to read.
// value in table is 255 if not a valid character
//else returns 7 bit character value for 10 bit symbol.
// lsb is first data bit in transmission order
const quint8 CHARTBL[0x400] = {
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0000 to 000F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0010 to 001F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0020 to 002F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0030 to 003F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0040 to 004F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0050 to 005F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0060 to 006F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,127, //0070 to 007F
255,255,255,255,255,255,255, 7,255,255,255, 11,255, 13, 14,255, //0080 to 008F
255,255,255, 19,255, 21, 22,255,255, 25, 26,255, 28,255,255,255, //0090 to 009F
255,255,255, 35,255, 37, 38,255,255, 41, 42,255, 44,255,255,255, //00A0 to 00AF
255, 49, 50,255, 52,255,255,255, 56,255,255,255,255,255,255,255, //00B0 to 00BF
255,255,255, 67,255, 69, 70,255,255, 73, 74,255, 76,255,255,255, //00C0 to 00CF
255, 81, 82,255, 84,255,255,255, 88,255,255,255,255,255,255,255, //00D0 to 00DF
255, 97, 98,255,100,255,255,255,104,255,255,255,255,255,255,255, //00E0 to 00EF
112,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //00F0 to 00FF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0100 to 010F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, 31, //0110 to 011F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, 47, //0120 to 012F
255,255,255,255,255,255,255, 55,255,255,255, 59,255, 61, 62,255, //0130 to 013F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, 79, //0140 to 014F
255,255,255,255,255,255,255, 87,255,255,255, 91,255, 93, 94,255, //0150 to 015F
255,255,255,255,255,255,255,103,255,255,255,107,255,109,110,255, //0160 to 016F
255,255,255,115,255,117,118,255,255,121,122,255,124,255,255,255, //0170 to 017F
255, 1, 2,255, 4,255,255,255, 8,255,255,255,255,255,255,255, //0180 to 018F
16,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0190 to 019F
32,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //01A0 to 01AF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //01B0 to 01BF
64,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //01C0 to 01CF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //01D0 to 01DF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //01E0 to 01EF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //01F0 to 01FF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0200 to 020F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0210 to 021F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0220 to 022F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, 63, //0230 to 023F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0240 to 024F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, 95, //0250 to 025F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,111, //0260 to 026F
255,255,255,255,255,255,255,119,255,255,255,123,255,125,126,255, //0270 to 027F
255,255,255, 3,255, 5, 6,255,255, 9, 10,255, 12,255,255,255, //0280 to 028F
255, 17, 18,255, 20,255,255,255, 24,255,255,255,255,255,255,255, //0290 to 029F
255, 33, 34,255, 36,255,255,255, 40,255,255,255,255,255,255,255, //02A0 to 02AF
48,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //02B0 to 02BF
255, 65, 66,255, 68,255,255,255, 72,255,255,255,255,255,255,255, //02C0 to 02CF
80,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //02D0 to 02DF
96,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //02E0 to 02EF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //02F0 to 02FF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, 15, //0300 to 030F
255,255,255,255,255,255,255, 23,255,255,255, 27,255, 29, 30,255, //0310 to 031F
255,255,255,255,255,255,255, 39,255,255,255, 43,255, 45, 46,255, //0320 to 032F
255,255,255, 51,255, 53, 54,255,255, 57, 58,255, 60,255,255,255, //0330 to 033F
255,255,255,255,255,255,255, 71,255,255,255, 75,255, 77, 78,255, //0340 to 034F
255,255,255, 83,255, 85, 86,255,255, 89, 90,255, 92,255,255,255, //0350 to 035F
255,255,255, 99,255,101,102,255,255,105,106,255,108,255,255,255, //0360 to 036F
255,113,114,255,116,255,255,255,120,255,255,255,255,255,255,255, //0370 to 037F
0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0380 to 038F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //0390 to 039F
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //03A0 to 03AF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //03B0 to 03BF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //03C0 to 03CF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //03D0 to 03DF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //03E0 to 03EF
255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, //03F0 to 03FF
};
//DSC decoder states
#define STATE_PHASEDET 0
#define STATE_DECODEDX 1
#define STATE_DECODERX 2
#define STATE_ECCDX 3
#define STATE_ECCRX 4
#define MSG_TIMEOUT 70 //number of RX/DX characters to read before giving up
/////////////////////////////////////////////////////////////////////////////////
// Construct FSK demod object
/////////////////////////////////////////////////////////////////////////////////
CFskDemod::CFskDemod(TYPEREAL samplerate) : m_SampleRate(samplerate)
{
qDebug()<<"FSK Rate = "<<m_SampleRate;
TYPEREAL PhzInc = K_2PI*SHIFT_FREQ/(2.0*m_SampleRate); // FreqShift/2
m_MarkOsc1.re = 1.0; //initialize oscillator unit vectors that will get rotated
m_MarkOsc1.im = 0.0;
m_SpaceOsc1.re = 1.0;
m_SpaceOsc1.im = 0.0;
m_MarkOscCos = cos(-PhzInc);
m_MarkOscSin = sin(-PhzInc);
m_SpaceOscCos = cos(PhzInc);
m_SpaceOscSin = sin(PhzInc);
//init integrators
m_MarkIntegratorIndex = 0;
m_SpaceIntegratorIndex = 0;
for(int i=0; i<INTEGRATOR_BUF_LEN; i++)
{
m_MarkIntegrationBuf[i].re = 0.0;
m_MarkIntegrationBuf[i].im = 0.0;
m_SpaceIntegrationBuf[i].re = 0.0;
m_SpaceIntegrationBuf[i].im = 0.0;
}
//calculate ATC dual time constant filter values.
m_AttackAlpha = (1.0-exp(-1.0/(m_SampleRate*ATTACK_TIMECONST)) );
m_DecayAlpha = (1.0-exp(-1.0/(m_SampleRate*DECAY_TIMECONST)) );
m_MarkAve = 0.0;
m_SpaceAve = 0.0;
//Init LP filter for complex input. abt +-250Hz
m_Fir.InitConstFir(FSK1_LENGTH, FSK1_COEF, FSK1_COEF, m_SampleRate);
//Init Post data IIR LP filter
m_OutIir.InitLP(150.0, 0.707, m_SampleRate);
//create Hi-Q resonator at the bit rate to recover bit sync position Q==200
m_BitSyncFilter.InitBP(100.0, 200.0, m_SampleRate);
//init some decoder variables
m_RxShiftReg = 0;
m_RxBitPos = 0;
m_RxDecodeState = STATE_PHASEDET;
m_RxDecodeTimer = 0;
for(int i=0; i<16; i++)
m_ShiftReg[i] = 0;
//just for testing, create 1700Hz osc for shifting baseband IQ into audio range for soundcard
m_AudioShiftOsc1.re = 1.0; //initialize interocitor unit vectors that will get rotated
m_AudioShiftOsc1.im = 0.0;
m_AudioShiftOscCos = cos(K_2PI*1700.0/m_SampleRate);
m_AudioShiftOscSin = sin(K_2PI*1700.0/m_SampleRate);
}
/////////////////////////////////////////////////////////////////////////////////
// FSK demod Process 'InLength' samples of 'pInData' into 'pOutData'
/////////////////////////////////////////////////////////////////////////////////
int CFskDemod::ProcessData(int InLength, TYPECPX* pInData, TYPECPX* pOutData)
{
TYPECPX cxIn;
TYPECPX cxtmp;
TYPEREAL Mpwr;
TYPEREAL Spwr;
TYPEREAL OscGn;
TYPECPX Osc;
//lowpass Filter +/-250Hz
m_Fir.ProcessFilter(InLength, pInData, pInData);
//g_pTestBench->DisplayData(InLength, pInData, m_SampleRate, PROFILE_2);
for(int i=0; i<InLength; i++)
{
cxIn = pInData[i]; //make copy of complex I/Q input sample
//First Process MARK('1") signal path
//create -85Hz Mark correlator Osc samples using quadrature oscillator
Osc.re = m_MarkOsc1.re * m_MarkOscCos - m_MarkOsc1.im * m_MarkOscSin;
Osc.im = m_MarkOsc1.im * m_MarkOscCos + m_MarkOsc1.re * m_MarkOscSin;
OscGn = 1.95 - (m_MarkOsc1.re*m_MarkOsc1.re + m_MarkOsc1.im*m_MarkOsc1.im);
m_MarkOsc1.re = OscGn * Osc.re; //keeps amplitude bounded
m_MarkOsc1.im = OscGn * Osc.im;
//
//Cpx multiply Input sample by Mark shift frequency(bring -85Hz to 0Hz)
cxtmp.re = ((cxIn.re * Osc.re) + (cxIn.im * Osc.im));
cxtmp.im = ((cxIn.im * Osc.re) - (cxIn.re * Osc.im));
//place shifted Mark I/Q sample in Circular Integrator Buffer
//that is one symbol time deep
m_MarkIntegrationBuf[m_MarkIntegratorIndex++] = cxtmp;
if(m_MarkIntegratorIndex >= INTEGRATOR_BUF_LEN)
m_MarkIntegratorIndex = 0;
//integrate over the last 'INTEGRATOR_BUF_LEN' samples
cxtmp.re = 0.0; cxtmp.im = 0.0;
for(int j=0; j<INTEGRATOR_BUF_LEN; j++)
{
cxtmp.re += m_MarkIntegrationBuf[j].re;
cxtmp.im += m_MarkIntegrationBuf[j].im;
}
//calc correlation energy in Mark signal
Mpwr = sqrt(cxtmp.re*cxtmp.re + cxtmp.im*cxtmp.im);
//create average Mark energy with dual time constant agc for ATC
if(Mpwr>m_MarkAve) //if magnitude is rising (use m_AttackAlpha time constant)
m_MarkAve = (1.0-m_AttackAlpha)*m_MarkAve + m_AttackAlpha*Mpwr;
else //else magnitude is falling (use m_DecayAlpha time constant)
m_MarkAve = (1.0-m_DecayAlpha)*m_MarkAve + m_DecayAlpha*Mpwr;
//Now Process SPACE('0') signal path
//create +85Hz Space correlator Osc samples using quadrature oscillator
Osc.re = m_SpaceOsc1.re * m_SpaceOscCos - m_SpaceOsc1.im * m_SpaceOscSin;
Osc.im = m_SpaceOsc1.im * m_SpaceOscCos + m_SpaceOsc1.re * m_SpaceOscSin;
OscGn = 1.95 - (m_SpaceOsc1.re*m_SpaceOsc1.re + m_SpaceOsc1.im*m_SpaceOsc1.im);
m_SpaceOsc1.re = OscGn * Osc.re; //keeps amplitude bounded
m_SpaceOsc1.im = OscGn * Osc.im;
//Cpx multiply Input sample by Space shift frequency(bring +85Hz to 0Hz)
cxtmp.re = ((cxIn.re * Osc.re) + (cxIn.im * Osc.im));
cxtmp.im = ((cxIn.im * Osc.re) - (cxIn.re * Osc.im));
//place shifted Mark I/Q sample in Circular Integrator Buffer
//that is one symbol time deep
m_SpaceIntegrationBuf[m_SpaceIntegratorIndex++] = cxtmp;
if(m_SpaceIntegratorIndex>=INTEGRATOR_BUF_LEN)
m_SpaceIntegratorIndex = 0;
//integrate over the last 'INTEGRATOR_BUF_LEN' samples
cxtmp.re = 0.0; cxtmp.im = 0.0;
for(int j=0; j<INTEGRATOR_BUF_LEN; j++)
{
cxtmp.re += m_SpaceIntegrationBuf[j].re;
cxtmp.im += m_SpaceIntegrationBuf[j].im;
}
//calc correlation energy in Space signal
Spwr = sqrt(cxtmp.re*cxtmp.re + cxtmp.im*cxtmp.im);
//create average Space energy with dual time constant agc for ATC
if(Spwr>m_SpaceAve) //if magnitude is rising (use m_AttackAlpha time constant)
m_SpaceAve = (1.0-m_AttackAlpha)*m_SpaceAve + m_AttackAlpha*Spwr;
else //else magnitude is falling (use m_DecayAlpha time constant)
m_SpaceAve = (1.0-m_DecayAlpha)*m_SpaceAve + m_DecayAlpha*Spwr;
//calculate mark-space difference and shift by ATC threshold value
//
m_Outbuf[i] = (Mpwr - Spwr + m_SpaceAve/2.0 - m_MarkAve/2.0);
//perform post filter 1 sample at a time in this loop(should be done before ATC?)
m_OutIir.ProcessFilter(1, &m_Outbuf[i], &m_Outbuf[i]);
//create absolute value version of output to use in bit sync algorithm
m_SyncSignal[i] = fabs(m_Outbuf[i]);
}
//now Create Bit sync signal with High Q Resonator IIR BP Filter
//using squared magnitude of data. Positive peak of this signal
//correspondes to the maximum bit energy position so good place to sample
m_BitSyncFilter.ProcessFilter(InLength, m_SyncSignal, m_SyncSignal);
for(int i=0; i<InLength; i++)
{
//the best bit sync position is at the positive peak of the m_SyncSignal waveform
TYPEREAL CurrentSlope = m_SyncSignal[i] - m_LastSync; //current slope
//see if at the top peak of the sync waveform(slope changes from pos to neg)
if( (CurrentSlope < 0.0) && (m_LastSyncSlope >= 0.0) )
{ //are at sample time so use previous bit time value since we are one sample behind in sync position
ProcessNewDataBit( (m_LastData>0) ); //go process new DSC bit
}
m_LastSync = m_SyncSignal[i]; //save previous states
m_LastSyncSlope = CurrentSlope;
m_LastData = m_Outbuf[i];
pOutData[i].im = m_Outbuf[i]/10.0; //use pInData as debug output buf for display
pOutData[i].re = m_SyncSignal[i]/3.0; //use pInData as debug output buf for display
}
//g_pTestBench->DisplayData(InLength, pOutData, m_SampleRate, PROFILE_3);
//for testing, create 1700Hz shifted copy of IQ data to get sent to soundcard
for(int i=0; i<InLength; i++)
{
cxIn = pInData[i]; //make copy of complex I/Q input sample
//create 1700Hz Osc samples using quadrature oscillator
Osc.re = m_AudioShiftOsc1.re * m_AudioShiftOscCos - m_AudioShiftOsc1.im * m_AudioShiftOscSin;
Osc.im = m_AudioShiftOsc1.im * m_AudioShiftOscCos + m_AudioShiftOsc1.re * m_AudioShiftOscSin;
OscGn = 1.95 - (m_AudioShiftOsc1.re*m_AudioShiftOsc1.re + m_AudioShiftOsc1.im*m_AudioShiftOsc1.im);
m_AudioShiftOsc1.re = OscGn * Osc.re; //keeps amplitude bounded
m_AudioShiftOsc1.im = OscGn * Osc.im;
//
//Cpx multiply Input sample by audio frequency(bring baseband up to 1700Hz)
pOutData[i].re = ( (cxIn.re * Osc.re) + (cxIn.im * Osc.im) );
pOutData[i].im = ( (cxIn.im * Osc.re) - (cxIn.re * Osc.im) );
}
return InLength;
}
/////////////////////////////////////////////////////////////////////////////////
// FSK demod Process 'InLength' samples of 'pInData' into 'pOutData'
/////////////////////////////////////////////////////////////////////////////////
int CFskDemod::ProcessData(int InLength, TYPECPX* pInData, TYPEREAL* pOutData)
{
TYPECPX cxIn;
TYPECPX cxtmp;
TYPEREAL Mpwr;
TYPEREAL Spwr;
TYPEREAL OscGn;
TYPECPX Osc;
//lowpass Filter +/-250Hz
m_Fir.ProcessFilter(InLength, pInData, pInData);
//g_pTestBench->DisplayData(InLength, pInData, m_SampleRate, PROFILE_2);
for(int i=0; i<InLength; i++)
{
cxIn = pInData[i]; //make copy of complex I/Q input sample
//First Process MARK('1") signal path
//create -85Hz Mark correlator Osc samples using quadrature oscillator
Osc.re = m_MarkOsc1.re * m_MarkOscCos - m_MarkOsc1.im * m_MarkOscSin;
Osc.im = m_MarkOsc1.im * m_MarkOscCos + m_MarkOsc1.re * m_MarkOscSin;
OscGn = 1.95 - (m_MarkOsc1.re*m_MarkOsc1.re + m_MarkOsc1.im*m_MarkOsc1.im);
m_MarkOsc1.re = OscGn * Osc.re; //keeps amplitude bounded
m_MarkOsc1.im = OscGn * Osc.im;
//
//Cpx multiply Input sample by Mark shift frequency(bring -85Hz to 0Hz)
cxtmp.re = ((cxIn.re * Osc.re) + (cxIn.im * Osc.im));
cxtmp.im = ((cxIn.im * Osc.re) - (cxIn.re * Osc.im));
//place shifted Mark I/Q sample in Circular Integrator Buffer
//that is one symbol time deep
m_MarkIntegrationBuf[m_MarkIntegratorIndex++] = cxtmp;
if(m_MarkIntegratorIndex >= INTEGRATOR_BUF_LEN)
m_MarkIntegratorIndex = 0;
//integrate over the last 'INTEGRATOR_BUF_LEN' samples
cxtmp.re = 0.0; cxtmp.im = 0.0;
for(int j=0; j<INTEGRATOR_BUF_LEN; j++)
{
cxtmp.re += m_MarkIntegrationBuf[j].re;
cxtmp.im += m_MarkIntegrationBuf[j].im;
}
//calc correlation energy in Mark signal
Mpwr = sqrt(cxtmp.re*cxtmp.re + cxtmp.im*cxtmp.im);
//create average Mark energy with dual time constant agc for ATC
if(Mpwr>m_MarkAve) //if magnitude is rising (use m_AttackAlpha time constant)
m_MarkAve = (1.0-m_AttackAlpha)*m_MarkAve + m_AttackAlpha*Mpwr;
else //else magnitude is falling (use m_DecayAlpha time constant)
m_MarkAve = (1.0-m_DecayAlpha)*m_MarkAve + m_DecayAlpha*Mpwr;
//Now Process SPACE('0') signal path
//create +85Hz Space correlator Osc samples using quadrature oscillator
Osc.re = m_SpaceOsc1.re * m_SpaceOscCos - m_SpaceOsc1.im * m_SpaceOscSin;
Osc.im = m_SpaceOsc1.im * m_SpaceOscCos + m_SpaceOsc1.re * m_SpaceOscSin;
OscGn = 1.95 - (m_SpaceOsc1.re*m_SpaceOsc1.re + m_SpaceOsc1.im*m_SpaceOsc1.im);
m_SpaceOsc1.re = OscGn * Osc.re; //keeps amplitude bounded
m_SpaceOsc1.im = OscGn * Osc.im;
//Cpx multiply Input sample by Space shift frequency(bring +85Hz to 0Hz)
cxtmp.re = ((cxIn.re * Osc.re) + (cxIn.im * Osc.im));
cxtmp.im = ((cxIn.im * Osc.re) - (cxIn.re * Osc.im));
//place shifted Mark I/Q sample in Circular Integrator Buffer
//that is one symbol time deep
m_SpaceIntegrationBuf[m_SpaceIntegratorIndex++] = cxtmp;
if(m_SpaceIntegratorIndex>=INTEGRATOR_BUF_LEN)
m_SpaceIntegratorIndex = 0;
//integrate over the last 'INTEGRATOR_BUF_LEN' samples
cxtmp.re = 0.0; cxtmp.im = 0.0;
for(int j=0; j<INTEGRATOR_BUF_LEN; j++)
{
cxtmp.re += m_SpaceIntegrationBuf[j].re;
cxtmp.im += m_SpaceIntegrationBuf[j].im;
}
//calc correlation energy in Space signal
Spwr = sqrt(cxtmp.re*cxtmp.re + cxtmp.im*cxtmp.im);
//create average Space energy with dual time constant agc for ATC
if(Spwr>m_SpaceAve) //if magnitude is rising (use m_AttackAlpha time constant)
m_SpaceAve = (1.0-m_AttackAlpha)*m_SpaceAve + m_AttackAlpha*Spwr;
else //else magnitude is falling (use m_DecayAlpha time constant)
m_SpaceAve = (1.0-m_DecayAlpha)*m_SpaceAve + m_DecayAlpha*Spwr;
//calculate mark-space difference and shift by ATC threshold value
//
m_Outbuf[i] = (Mpwr - Spwr + m_SpaceAve/2.0 - m_MarkAve/2.0);
//perform post filter 1 sample at a time in this loop(should be done before ATC?)
m_OutIir.ProcessFilter(1, &m_Outbuf[i], &m_Outbuf[i]);
//create absolute value version of output to use in bit sync algorithm
m_SyncSignal[i] = fabs(m_Outbuf[i]);
}
//now Create Bit sync signal with High Q Resonator IIR BP Filter
//using squared magnitude of data. Positive peak of this signal
//correspondes to the maximum bit energy position so good place to sample
m_BitSyncFilter.ProcessFilter(InLength, m_SyncSignal, m_SyncSignal);
for(int i=0; i<InLength; i++)
{
//the best bit sync position is at the positive peak of the m_SyncSignal waveform
TYPEREAL CurrentSlope = m_SyncSignal[i] - m_LastSync; //current slope
//see if at the top peak of the sync waveform(slope changes from pos to neg)
if( (CurrentSlope < 0.0) && (m_LastSyncSlope >= 0.0) )
{ //are at sample time so use previous bit time value since we are one sample behind in sync position
ProcessNewDataBit( (m_LastData>0) ); //go process new DSC bit
}
m_LastSync = m_SyncSignal[i]; //save previous states
m_LastSyncSlope = CurrentSlope;
m_LastData = m_Outbuf[i];
}
//g_pTestBench->DisplayData(InLength, pOutData, m_SampleRate, PROFILE_3);
for(int i=0; i<InLength; i++)
{
pOutData[i] = pInData[i].re;
}
return InLength;
}
/////////////////////////////////////////////////////////////////////////////////
// DSC state decoder to find bit phase position and get raw characters.
/////////////////////////////////////////////////////////////////////////////////
void CFskDemod::ProcessNewDataBit(bool Bit)
{
quint8 ch;
int i;
int dxphzcnt = 0;
int rxphzcnt = 0;
//shift in new data bit into 160 bit shift register consisting of 16 ten bit shift registers
for(i=0; i<16; i++)
{
m_ShiftReg[i] >>= 1;
if(i<15)
{
if(m_ShiftReg[i+1] & 1) //shift in bit from upper register
m_ShiftReg[i] |= 0x0200;
}
else
{
if(Bit)
m_ShiftReg[15] |= 0x0200;
}
}
// go through all Rx Phase positions and count matches to Rx phase characters
//this will detect a phase sequence even if already decoding a msg.
if(111 == CHARTBL[ m_ShiftReg[1] ]) rxphzcnt++;
if(110 == CHARTBL[ m_ShiftReg[3] ]) rxphzcnt++;
if(109 == CHARTBL[ m_ShiftReg[5] ]) rxphzcnt++;
if(108 == CHARTBL[ m_ShiftReg[7] ]) rxphzcnt++;
if(107 == CHARTBL[ m_ShiftReg[9] ]) rxphzcnt++;
if(106 == CHARTBL[ m_ShiftReg[11] ]) rxphzcnt++;
if(105 == CHARTBL[ m_ShiftReg[13] ]) rxphzcnt++;
if(104 == CHARTBL[ m_ShiftReg[15] ]) rxphzcnt++;
if(125 == CHARTBL[ m_ShiftReg[0] ]) dxphzcnt++;
if(125 == CHARTBL[ m_ShiftReg[2] ]) dxphzcnt++;
if(125 == CHARTBL[ m_ShiftReg[4] ]) dxphzcnt++;
if(125 == CHARTBL[ m_ShiftReg[6] ]) dxphzcnt++;
if(125 == CHARTBL[ m_ShiftReg[8] ]) dxphzcnt++;
if(125 == CHARTBL[ m_ShiftReg[10] ]) dxphzcnt++;
if( (rxphzcnt>=3) || ((rxphzcnt==2)&&(dxphzcnt>=1)) || ((rxphzcnt==1)&&(dxphzcnt>=2)))
{
//qDebug()<<"Got Phase Sequence pattern";
m_DxBufIndex = 0;
m_RxBufIndex = 0;
m_DxBuf[m_DxBufIndex++] = CHARTBL[ m_ShiftReg[12] ]; //read in first A Dx field
m_DxBuf[m_DxBufIndex++] = CHARTBL[ m_ShiftReg[14] ]; //read in second A Dx field
m_RxDecodeState = STATE_DECODEDX;
m_RxBitPos = -10; //set sync bit position so shifts in next full symbol before executing decode state machine
m_Ecc = 0;
m_RxDecodeTimer = MSG_TIMEOUT;
}
if(0==m_RxBitPos)
{
if(m_RxDecodeTimer > 0) //dec msg timeout
{
m_RxDecodeTimer--;
}
else
{
m_RxDecodeState = STATE_PHASEDET;
}
switch( m_RxDecodeState)
{
case STATE_PHASEDET: //looking for initial phazing position so do nothing
break;
case STATE_DECODEDX: //here to store next Dx character into
ch = CHARTBL[ m_ShiftReg[15] ];
m_DxBuf[m_DxBufIndex++] = ch;
if(m_DxBufIndex>2) //only need to save last 3 Dx characters
m_DxBufIndex = 0;
m_RxDecodeState = STATE_DECODERX;
break;
case STATE_DECODERX:
ch = CHARTBL[ m_ShiftReg[15] ];
if(ch!=255) //if Rx char is ok use it
m_RxBuf[m_RxBufIndex] = ch;
else //else use Dx char
m_RxBuf[m_RxBufIndex] = m_DxBuf[m_DxBufIndex];
if(1==m_RxBufIndex) //if second 'A' position is error then use first char 'A'
{
if(255 == m_RxBuf[1] )
m_RxBuf[1] = m_RxBuf[0];
m_Ecc = m_RxBuf[1]; //init ecc value
}
else
m_Ecc ^= m_RxBuf[m_RxBufIndex];
if( (117==m_RxBuf[m_RxBufIndex]) || (122==m_RxBuf[m_RxBufIndex]) || (127==m_RxBuf[m_RxBufIndex]) )
{
m_RxDecodeState = STATE_ECCDX;
}
else
{
m_RxDecodeState = STATE_DECODEDX;
}
m_RxBufIndex++;
break;
case STATE_ECCDX: //here to store next Dx character
ch = CHARTBL[ m_ShiftReg[15] ];
m_DxBuf[m_DxBufIndex++] = ch;
if(m_DxBufIndex>2) //only need to save last 3 Dx characters
m_DxBufIndex = 0;
m_RxDecodeState = STATE_ECCRX;
break;
case STATE_ECCRX:
ch = CHARTBL[ m_ShiftReg[15] ];
if(ch!=255) //if ECC char is ok use it
m_RxBuf[m_RxBufIndex] = ch;
else //else use Dx ECC
m_RxBuf[m_RxBufIndex] = m_DxBuf[m_DxBufIndex];
if(m_Ecc == m_RxBuf[m_RxBufIndex])
{ //here if EEC correct MSG received in m_RxBuf[]
m_Str1.sprintf("*** Valid Msg ");
for(i=1; i<=m_RxBufIndex; i++)
{
m_Str1 += m_Str2.sprintf("% d",m_RxBuf[i]);
}
g_pTestBench->SendDebugTxt(m_Str1);
qDebug()<<"Got Valid Msg ecc = "<<m_Ecc << m_Str1;
emit g_pChatDialog->SendChatStr(m_Str1);
}
else
{
rxphzcnt = 0; //use to count up all error characters
for(i=1; i<=m_RxBufIndex; i++)
{
if(255==m_RxBuf[i])
rxphzcnt++;
}
m_Str1.sprintf("??? Got Bad Msg Cnt=%d",rxphzcnt);
qDebug()<<"Got BAD Msg ecc = "<<m_Ecc;
emit g_pChatDialog->SendChatStr(m_Str1);
g_pTestBench->SendDebugTxt(m_Str1);
}
m_RxDecodeState = STATE_PHASEDET;
//create/Reset Hi-Q resonator at the bit rate to recover bit sync position Q==200
m_BitSyncFilter.InitBP(100.0, 200.0, m_SampleRate);
break;
}
}
//inc current Bit position modulo 10
if( ++m_RxBitPos >= 10)
m_RxBitPos = 0;
}
|
