1 /*
2 * mt63base.cxx -- MT63 transmitter and receiver in C++ for LINUX
3 *
4 * Copyright (C) 1999-2004 Pawel Jalocha, SP9VRC
5 * Copyright (c) 2007-2011 Dave Freese, W1HKJ
6 *
7 * base class for use by fldigi
8 * modified from original
9 * excluded CW_ID which is a part of the base modem class for fldigi
10 * changed all floats to double and removed all float functions/methods
11 * changed from int* to double* for all sound card buffer transfers
12 *
13 * Modified base class for rx and tx to allow variable center frequency
14 * for baseband signal
15 *
16 * based on mt63 code by Pawel Jalocha
17 *
18 * This file is part of fldigi.
19 *
20 * Fldigi is free software: you can redistribute it and/or modify
21 * it under the terms of the GNU General Public License as published by
22 * the Free Software Foundation, either version 3 of the License, or
23 * (at your option) any later version.
24 *
25 * Fldigi is distributed in the hope that it will be useful,
26 * but WITHOUT ANY WARRANTY; without even the implied warranty of
27 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
28 * GNU General Public License for more details.
29 *
30 * You should have received a copy of the GNU General Public License
31 * along with fldigi. If not, see <http://www.gnu.org/licenses/>.
32 *
33 */
34
35 #include <config.h>
36
37 #include <stdio.h> // only for control printf's
38 // #include <alloc.h>
39 #include <iostream>
40
41 #include "dsp.h"
42
43 #include "mt63base.h"
44
45 #include "symbol.dat" // symbol shape
46 #include "mt63intl.dat" // interleave patterns
47
48 // W1HKJ
49 // fixed filter shapes replaced by maximally flat blackman3 filters
50 // that are generated as required as signal center frequency is changed
51
52 //#include "alias_k5.dat" // anti-alias filter shapes
53 //#include "alias_1k.dat" // for 500, 1000 and 2000 Hz modes
54 //#include "alias_2k.dat"
55
56
57 // ==========================================================================
58 // MT63 transmitter code
59
MT63tx()60 MT63tx::MT63tx()
61 {
62 TxVect = NULL;
63 dspPhaseCorr = NULL;
64 }
65
~MT63tx()66 MT63tx::~MT63tx()
67 {
68 free(TxVect);
69 free(dspPhaseCorr);
70 }
71
Free(void)72 void MT63tx::Free(void)
73 {
74 free(TxVect);
75 TxVect = NULL;
76 free(dspPhaseCorr);
77 dspPhaseCorr = NULL;
78 Encoder.Free();
79 FFT.Free();
80 Window.Free();
81 Comb.Free();
82 WindowBuff.Free();
83 }
84
85 // W1HKJ
86 // added freq paramter to Preset
Preset(double freq,int BandWidth,int LongInterleave)87 int MT63tx::Preset(double freq, int BandWidth, int LongInterleave)
88 {
89 int i, p, step, incr, mask;
90
91 // W1HKJ
92 // values used to computer the blackman3 passband filter shape
93 double hbw = 1.5*BandWidth / 2;
94 double omega_low = (freq - hbw);
95 double omega_high = (freq + hbw);
96 if (omega_low < 100) omega_low = 100;
97 if (omega_high > 4000) omega_high = 4000;
98 omega_low *= (M_PI / 4000);
99 omega_high *= (M_PI / 4000);
100
101 mask = FFT.Size - 1;
102 DataCarriers = 64;
103
104 switch(BandWidth) {
105 case 500:
106 FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 256 / 500 + .5);
107 AliasFilterLen = 128;
108 DecimateRatio = 8;
109 break;
110 case 1000:
111 FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 128 / 500 + 0.5);
112 AliasFilterLen = 64;
113 DecimateRatio = 4;
114 break;
115 case 2000:
116 FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 64 / 500 + 0.5);
117 AliasFilterLen = 64;
118 DecimateRatio = 2;
119 break;
120 default:
121 return -1;
122 }
123
124 WindowLen = SymbolLen;
125 TxWindow = SymbolShape;
126 TxAmpl = 4.0 / DataCarriers; // for maximum undistorted output
127 CarrMarkCode = 0x16918BBEL;
128 CarrMarkAmpl = 0;
129
130 if (LongInterleave) {
131 DataInterleave = 64;
132 InterleavePattern = LongIntlvPatt;
133 }
134 else {
135 DataInterleave = 32;
136 InterleavePattern = ShortIntlvPatt;
137 }
138
139 if (dspRedspAllocArray(&TxVect, DataCarriers))
140 goto Error;
141 if (dspRedspAllocArray(&dspPhaseCorr, DataCarriers))
142 goto Error;
143 if (WindowBuff.EnsureSpace(2 * WindowLen))
144 goto Error;
145 WindowBuff.Len = 2 * WindowLen;
146
147 if (Encoder.Preset(DataCarriers, DataInterleave, InterleavePattern, 1))
148 goto Error;
149 if (FFT.Preset(WindowLen))
150 goto Error;
151 if (Window.Preset(WindowLen, SymbolSepar / 2, TxWindow))
152 goto Error;
153
154 // W1HKJ
155 // Preset the combining instance, NULL pointers in lieu of fixed filter shapes
156 // blackman3 filter provides flat passband and sufficient out-of-band rejection
157 // to insure that all unwanted FFT components (periodic signal) are suppressed
158 // by 70 dB or more
159 if ( Comb.Preset( AliasFilterLen, NULL, NULL, DecimateRatio ) )
160 goto Error;
161 // compute new combining filter shape
162 Comb.ComputeShape(omega_low, omega_high, dspWindowBlackman3);
163
164 // Preset the initial dspPhase for each data carrier.
165 // Here we only compute indexes to the FFT twiddle factors
166 // so the actual vector is FFT.Twiddle[TxVect[i]]
167
168 for (step = 0, incr = 1, p = 0, i = 0; i < DataCarriers; i++) {
169 TxVect[i] = p;
170 step += incr;
171 p = (p + step) & mask;
172 }
173
174 // compute dspPhase correction between successive FFTs separated by SymbolSepar
175 // Like above we compute indexes to the FFT.Twiddle[]
176
177 incr = (SymbolSepar * DataCarrSepar) & mask;
178 for (p = (SymbolSepar * FirstDataCarr) & mask, i = 0; i < DataCarriers; i++) {
179 dspPhaseCorr[i] = p;
180 p = (p + incr) & mask;
181 }
182 return 0;
183 Error:
184 Free();
185 return -1;
186 }
187
188 // W1HKJ
189 // SendTune and ProcessTxVect are both modified to allow the FirstDataCarr
190 // to be other than WindowLen / 2 as in the original design
191 // The peridocity of the FFT is taken advantage of by computing the positions
192 // of the bit indices modulo FFT.size, i.e. r = FFT.BitRevIdx[c & (FFT.Size - 1)]
193
SendTune(bool twotones)194 int MT63tx::SendTune(bool twotones)
195 {
196 int i, c, r, mask;
197 double Ampl;
198
199 mask = FFT.Size - 1;
200 Ampl = TxAmpl * sqrt(DataCarriers / 2);
201
202 for (i = 0; i < DataCarriers; i++)
203 TxVect[i] = (TxVect[i] + dspPhaseCorr[i]) & mask;
204
205 for (i = 0; i < 2 * WindowLen; i++)
206 WindowBuff.Data[i].im = WindowBuff.Data[i].re = 0.0;
207
208 // W1HKJ
209 // first tone at the lowest most MT63 carrier
210 i = 0;
211 c = FirstDataCarr;
212 r = FFT.BitRevIdx[c & mask];
213 WindowBuff.Data[r].re = Ampl * FFT.Twiddle[TxVect[i]].re;
214 WindowBuff.Data[r].im = (-Ampl) * FFT.Twiddle[TxVect[i]].im;
215
216 // W1HKJ
217 // 2nd tone at the highest most MT63 carrier + 1
218 // MT63 is specified as 500, 1000 and 2000 Hz wide signal format, but in
219 // fact are narrower by one carrier spacing, i.e. 0 to N-1 carriers where
220 // N = 64
221
222 if (twotones) {
223 i = DataCarriers - 1;
224 c = (FirstDataCarr + i * DataCarrSepar);
225 r = WindowLen + FFT.BitRevIdx[c & mask];
226 WindowBuff.Data[r].re = Ampl * FFT.Twiddle[TxVect[i]].re;
227 WindowBuff.Data[r].im = (-Ampl) * FFT.Twiddle[TxVect[i]].im;
228 }
229
230 // inverse FFT: WindowBuff is already scrambled
231 FFT.CoreProc(WindowBuff.Data);
232 FFT.CoreProc(WindowBuff.Data + WindowLen);
233
234 // negate the imaginary part for the IFFT
235 for (i = 0; i < 2 * WindowLen; i++)
236 WindowBuff.Data[i].im *= (-1.0);
237
238 // process the FFT values to produce a complex time domain vector
239 Window.Process(&WindowBuff);
240
241 // W1HKJ
242 // convert the complex time domain vector to a real time domain signal
243 // suitably filtered by the anti-alias filter used in the combiner
244 Comb.Process(&Window.Output);
245
246 return 0;
247 }
248
SendChar(char ch)249 int MT63tx::SendChar(char ch)
250 {
251 int i,mask,flip;
252
253 Encoder.Process(ch); // encode and interleave the character
254
255 // print the character and the DataBlock being sent
256 // printf("0x%02x [%c] => ", ch, ch>=' ' ? ch : '.');
257 // for (i=0; i<DataCarriers; i++) printf("%c",'0'+Encoder.Output[i]);
258 // printf("\n");
259
260 // here we encode the Encoder.Output into dspPhase flips
261
262 mask = FFT.Size - 1;
263 flip = FFT.Size / 2;
264 for (i = 0; i < DataCarriers; i++) {
265 // data bit = 1 => only dspPhase correction
266 if (Encoder.Output[i])
267 TxVect[i] = (TxVect[i] + dspPhaseCorr[i]) & mask;
268 // data bit = 0 => dspPhase flip + dspPhase correction
269 else
270 TxVect[i] = (TxVect[i] + dspPhaseCorr[i] + flip) & mask;
271 }
272
273 ProcessTxVect();
274 return 0;
275 }
276
SendJam(void)277 int MT63tx::SendJam(void)
278 {
279 int i,mask,left,right;
280 int j = 0;
281
282 mask = FFT.Size-1;
283 left = FFT.Size / 4;
284 right = 3 * (FFT.Size / 4);
285 for (i = 0; i < DataCarriers; i++) {
286 j = i & mask;
287 if (rand() & 0x100) // turn left 90 degrees
288 TxVect[j] = (TxVect[j] + dspPhaseCorr[j] + left) & mask;
289 else // turn right 90 degrees
290 TxVect[j] = (TxVect[j] + dspPhaseCorr[j] + right) & mask;
291 }
292
293 ProcessTxVect();
294 return 0;
295 }
296
297 // W1HKJ
298 // principal change from original is modulo arithmetic used to creat
299 // WindowBuff.Data vectors
300
ProcessTxVect(void)301 int MT63tx::ProcessTxVect(void)
302 {
303 int i, c, r, mask;
304
305 mask = FFT.Size - 1;
306
307 for (i = 0; i < 2 * WindowLen; i++)
308 WindowBuff.Data[i].im = WindowBuff.Data[i].re = 0.0;
309
310 for ( i = 0, c = FirstDataCarr; i < DataCarriers; i++, c += DataCarrSepar) {
311 r = FFT.BitRevIdx[c & mask] + WindowLen * (i & 1);
312 WindowBuff.Data[r].re = TxAmpl*FFT.Twiddle[TxVect[i]].re;
313 WindowBuff.Data[r].im = (-TxAmpl)*FFT.Twiddle[TxVect[i]].im;
314 }
315 FFT.CoreProc(WindowBuff.Data);
316 FFT.CoreProc(WindowBuff.Data + WindowLen);
317
318 // negate the imaginary part for the IFFT
319 for (i = 0; i < 2 * WindowLen; i++)
320 WindowBuff.Data[i].im *= (-1.0);
321
322 Window.Process(&WindowBuff);
323
324 // W1HKJ
325 // audio output to be sent out is in Comb.Output
326 Comb.Process(&Window.Output);
327
328 return 0;
329 }
330
SendSilence(void)331 int MT63tx::SendSilence(void)
332 {
333 Window.ProcessSilence(2);
334 Comb.Process(&Window.Output);
335 return 0;
336 }
337
338 // ==========================================================================
339 // Character encoder and block interleave for the MT63 modem
340
MT63encoder()341 MT63encoder::MT63encoder()
342 {
343 IntlvPipe = NULL;
344 WalshBuff = NULL;
345 Output = NULL;
346 IntlvPatt=NULL;
347 }
348
~MT63encoder()349 MT63encoder::~MT63encoder()
350 {
351 free(IntlvPipe);
352 free(WalshBuff);
353 free(Output);
354 free(IntlvPatt);
355 }
356
Free()357 void MT63encoder::Free()
358 {
359 free(IntlvPipe);
360 free(WalshBuff);
361 free(Output);
362 free(IntlvPatt);
363 IntlvPipe = NULL;
364 WalshBuff = NULL;
365 Output = NULL;
366 IntlvPatt = NULL;
367 }
368
Preset(int Carriers,int Intlv,int * Pattern,int PreFill)369 int MT63encoder::Preset(int Carriers, int Intlv, int *Pattern, int PreFill)
370 {
371 int i, p;
372 if (!dspPowerOf2(Carriers)) goto Error;
373
374 DataCarriers = Carriers;
375 IntlvLen = Intlv;
376 IntlvSize = IntlvLen * DataCarriers;
377 if (IntlvLen) {
378 if (dspRedspAllocArray(&IntlvPipe, IntlvSize)) goto Error;
379 if (PreFill)
380 for (i = 0; i < IntlvSize; i++)
381 IntlvPipe[i] = rand() & 1;
382 else
383 dspClearArray(IntlvPipe,IntlvSize);
384 if (dspRedspAllocArray(&IntlvPatt, DataCarriers)) goto Error;
385 IntlvPtr = 0;
386 }
387 if (dspRedspAllocArray(&WalshBuff, DataCarriers)) goto Error;
388 if (dspRedspAllocArray(&Output, DataCarriers)) goto Error;
389 CodeMask = 2 * DataCarriers - 1;
390
391 for (p = 0, i = 0; i < DataCarriers; i++) {
392 IntlvPatt[i] = p * DataCarriers;
393 p += Pattern[i];
394 if (p >= IntlvLen) p -= IntlvLen;
395 }
396 return 0;
397
398 Error:
399 Free();
400 return -1;
401 }
402
Process(char code)403 int MT63encoder::Process(char code) // encode an ASCII character "code"
404 {
405 int i, k;
406 code &= CodeMask;
407 for (i = 0; i < DataCarriers; i++)
408 WalshBuff[i] = 0;
409 if (code < DataCarriers)
410 WalshBuff[(int)code] = 1.0;
411 else WalshBuff[code-DataCarriers] = (-1.0);
412
413 dspWalshInvTrans(WalshBuff, DataCarriers);
414
415 if (IntlvLen) {
416 for (i = 0; i < DataCarriers; i++)
417 IntlvPipe[IntlvPtr + i] = (WalshBuff[i] < 0.0);
418 for (i = 0; i < DataCarriers; i++) {
419 k = IntlvPtr + IntlvPatt[i];
420 if (k >= IntlvSize)
421 k -= IntlvSize;
422 Output[i] = IntlvPipe[k+i];
423 }
424 IntlvPtr += DataCarriers;
425 if (IntlvPtr >= IntlvSize)
426 IntlvPtr -= IntlvSize;
427 } else
428 for (i = 0; i < DataCarriers; i++)
429 Output[i] = (WalshBuff[i] < 0.0);
430
431 return 0;
432 }
433
434 // After encoding the "Output" array contains the bits to be transmitted
435
436 // ==========================================================================
437 // MT63 decoder and deinterleaver
438
MT63decoder()439 MT63decoder::MT63decoder()
440 {
441 IntlvPipe = NULL;
442 IntlvPatt = NULL;
443 WalshBuff = NULL;
444 DecodeSnrMid = NULL;
445 DecodeSnrOut = NULL;
446 DecodePipe = NULL;
447 }
448
~MT63decoder()449 MT63decoder::~MT63decoder()
450 {
451 free(IntlvPipe);
452 free(IntlvPatt);
453 free(WalshBuff);
454 free(DecodeSnrMid);
455 free(DecodeSnrOut);
456 free(DecodePipe);
457 }
458
Free()459 void MT63decoder::Free()
460 {
461 free(IntlvPipe);
462 IntlvPipe = NULL;
463 free(IntlvPatt);
464 IntlvPatt = NULL;
465 free(WalshBuff);
466 WalshBuff = NULL;
467 free(DecodeSnrMid);
468 free(DecodeSnrOut);
469 DecodeSnrMid = NULL;
470 DecodeSnrOut = NULL;
471 free(DecodePipe);
472 DecodePipe = NULL;
473 }
474
Preset(int Carriers,int Intlv,int * Pattern,int Margin,int Integ)475 int MT63decoder::Preset(int Carriers, int Intlv, int *Pattern, int Margin, int Integ)
476 {
477 int i,p;
478
479 if (!dspPowerOf2(Carriers)) goto Error;
480 DataCarriers = Carriers;
481 ScanLen = 2 * Margin + 1;
482 ScanSize = DataCarriers + 2 * Margin;
483
484 dspLowPass2Coeff(Integ,W1,W2,W5);
485 DecodeLen = Integ / 2;
486 DecodeSize = DecodeLen * ScanLen;
487 if (dspRedspAllocArray(&DecodePipe, DecodeSize)) goto Error;
488 dspClearArray(DecodePipe, DecodeSize);
489 DecodePtr = 0;
490
491 IntlvLen = Intlv; // printf("%d:",IntlvLen);
492 if (dspRedspAllocArray(&IntlvPatt, DataCarriers)) goto Error;
493 for (p = 0, i = 0; i < DataCarriers; i++) {
494 IntlvPatt[i] = p * ScanSize; // printf(" %2d",p);
495 p += Pattern[i];
496 if (p >= IntlvLen) p -= IntlvLen;
497 }
498 // printf("\n");
499
500 IntlvSize = (IntlvLen + 1) * ScanSize;
501 if (dspRedspAllocArray(&IntlvPipe, IntlvSize)) goto Error;
502 dspClearArray(IntlvPipe, IntlvSize);
503 IntlvPtr = 0;
504
505 if (dspRedspAllocArray(&WalshBuff, DataCarriers)) goto Error;
506
507 if (dspRedspAllocArray(&DecodeSnrMid, ScanLen)) goto Error;
508 if (dspRedspAllocArray(&DecodeSnrOut, ScanLen)) goto Error;
509 dspClearArray(DecodeSnrMid, ScanLen);
510 dspClearArray(DecodeSnrOut, ScanLen);
511
512 SignalToNoise = 0.0;
513 CarrOfs = 0;
514
515 return 0;
516 Error:
517 Free();
518 return -1;
519 }
520
Process(double * data)521 int MT63decoder::Process(double *data)
522 {
523 int s, i, k;
524 double Min, Max, Sig, Noise, SNR;
525 int MinPos,MaxPos,code;
526
527 dspCopyArray(IntlvPipe + IntlvPtr, data, ScanSize);
528
529 // printf("Decoder [%d/%d/%d]: \n",IntlvPtr,IntlvSize,ScanSize);
530 for (s = 0; s < ScanLen; s++) {
531 // printf(" %2d:",s);
532 for (i = 0; i < DataCarriers; i++) {
533 k = IntlvPtr - ScanSize - IntlvPatt[i];
534 if (k < 0) k += IntlvSize;
535 if ((s & 1) && (i & 1)) {
536 k += ScanSize;
537 if (k >= IntlvSize) k-=IntlvSize;
538 }
539 WalshBuff[i] = IntlvPipe[k + s + i];
540 // printf(" %4d",k/ScanSize);
541 }
542 // printf("\n");
543 dspWalshTrans(WalshBuff, DataCarriers);
544 Min = dspFindMin(WalshBuff, DataCarriers, MinPos);
545 Max = dspFindMax(WalshBuff, DataCarriers, MaxPos);
546 if (fabs(Max) > fabs(Min)) {
547 code = MaxPos + DataCarriers;
548 Sig = fabs(Max);
549 WalshBuff[MaxPos] = 0.0;
550 } else {
551 code = MinPos;
552 Sig = fabs(Min);
553 WalshBuff[MinPos] = 0.0;
554 }
555 Noise = dspRMS(WalshBuff, DataCarriers);
556 if (Noise > 0.0)
557 SNR = Sig/Noise;
558 else SNR = 0.0;
559 dspLowPass2(SNR, DecodeSnrMid[s], DecodeSnrOut[s], W1, W2, W5);
560 // printf("%2d: %02x => %c, %5.2f/%5.2f=>%5.2f <%5.2f>\n",
561 // s,code,code<' ' ? '.' : (char)code,
562 // Sig,Noise,SNR,DecodeSnrOut[s]);
563 DecodePipe[DecodePtr+s]=code;
564 }
565 IntlvPtr += ScanSize;
566 if (IntlvPtr >= IntlvSize) IntlvPtr = 0;
567 DecodePtr += ScanLen;
568 if (DecodePtr >= DecodeSize) DecodePtr = 0;
569 Max = dspFindMax(DecodeSnrOut, ScanLen, MaxPos);
570 Output = DecodePipe[DecodePtr + MaxPos];
571 SignalToNoise = Max;
572 CarrOfs = MaxPos - (ScanLen - 1) / 2;
573 /*
574 code=Output;
575 if ((code>=' ')||(code=='\n')||(code=='\r')) printf("%c",code);
576 else if (code!='\0') printf("<%02X>",code);
577 */
578 return 0;
579 }
580
581 // ==========================================================================
582 // MT63 receiver code
583
MT63rx()584 MT63rx::MT63rx()
585 {
586 int s;
587
588 FFTbuff = NULL;
589 FFTbuff2 = NULL;
590
591 for (s = 0; s < 4; s++)
592 SyncPipe[s] = NULL;
593 SyncPhCorr = NULL;
594 for (s = 0; s < 4; s++) {
595 CorrelMid[s] = NULL;
596 CorrelOut[s] = NULL;
597 }
598 dspPowerMid = NULL;
599 dspPowerOut = NULL;
600 for (s = 0; s < 4; s++)
601 CorrelNorm[s] = NULL;
602 for (s = 0; s < 4; s++)
603 CorrelAver[s] = NULL;
604 SymbFit = NULL;
605 SymbPipe = NULL;
606 FreqPipe = NULL;
607
608 RefDataSlice = NULL;
609
610 DataPipeLen = 0;
611 DataPipe = NULL;
612 DataPwrMid = NULL;
613 DataPwrOut = NULL;
614 DataSqrMid = NULL;
615 DataSqrOut = NULL;
616
617 DataVect = NULL;
618
619 DatadspPhase = NULL;
620 DatadspPhase2 = NULL;
621
622 SpectradspPower = NULL;
623 }
624
~MT63rx()625 MT63rx::~MT63rx()
626 {
627 int s;
628
629 free(FFTbuff);
630 free(FFTbuff2);
631
632 for (s = 0; s < 4; s++)
633 free(SyncPipe[s]);
634 free(SyncPhCorr);
635 for (s = 0; s < 4; s++) {
636 free(CorrelMid[s]);
637 free(CorrelOut[s]);
638 }
639 free(dspPowerMid);
640 free(dspPowerOut);
641 for (s = 0; s < 4; s++)
642 free(CorrelNorm[s]);
643 for (s = 0; s < 4; s++)
644 free(CorrelAver[s]);
645 free(SymbFit);
646 free(SymbPipe);
647 free(FreqPipe);
648
649 free(RefDataSlice);
650
651 dspFreeArray2D(DataPipe, DataPipeLen);
652 // for (s=0; s<DataPipeLen; s++) free(DataPipe[s]); free(DataPipe);
653 free(DataPwrMid);
654 free(DataPwrOut);
655 free(DataSqrMid);
656 free(DataSqrOut);
657
658 free(DataVect);
659
660 free(DatadspPhase);
661 free(DatadspPhase2);
662
663 free(SpectradspPower);
664 }
665
Free(void)666 void MT63rx::Free(void)
667 {
668 int s;
669 FFT.Free();
670 InpSplit.Free();
671 TestOfs.Free();
672 ProcLine.Free();
673
674 free(FFTbuff);
675 FFTbuff = NULL;
676 free(FFTbuff2);
677 FFTbuff2 = NULL;
678
679 for (s = 0; s < 4; s++) {
680 free(SyncPipe[s]);
681 SyncPipe[s] = NULL;
682 }
683 free(SyncPhCorr);
684 SyncPhCorr = NULL;
685 for (s = 0; s < 4; s++) {
686 free(CorrelMid[s]);
687 CorrelMid[s] = NULL;
688 free(CorrelOut[s]);
689 CorrelOut[s] = NULL;
690 }
691 free(dspPowerMid);
692 dspPowerMid = NULL;
693 free(dspPowerOut);
694 dspPowerOut = NULL;
695 for (s = 0; s < 4; s++) {
696 free(CorrelNorm[s]);
697 CorrelNorm[s] = NULL;
698 }
699 for (s = 0; s < 4; s++) {
700 free(CorrelAver[s]);
701 CorrelAver[s] = NULL;
702 }
703 free(SymbFit);
704 SymbFit = NULL;
705 free(SymbPipe);
706 SymbPipe = NULL;
707 free(FreqPipe);
708 FreqPipe = NULL;
709
710 free(RefDataSlice);
711 RefDataSlice = NULL;
712
713 dspFreeArray2D(DataPipe, DataPipeLen);
714 // for (s=0; s<DataPipeLen; s++) free(DataPipe[s]); free(DataPipe);
715
716 DataPipeLen = 0;
717 DataPipe = NULL;
718
719 free(DataPwrMid);
720 free(DataPwrOut);
721 DataPwrMid = NULL;
722 DataPwrOut = NULL;
723 free(DataSqrMid);
724 free(DataSqrOut);
725 DataSqrMid = NULL;
726 DataSqrOut = NULL;
727
728 free(DataVect);
729 DataVect = NULL;
730
731 free(DatadspPhase);
732 DatadspPhase = NULL;
733 free(DatadspPhase2);
734 DatadspPhase2 = NULL;
735
736 Decoder.Free();
737
738 free(SpectradspPower);
739 SpectradspPower = NULL;
740 }
741
742 // added freq parameter to Preset
Preset(double freq,int BandWidth,int LongInterleave,int Integ,void (* Display)(double * Spectra,int Len))743 int MT63rx::Preset(double freq, int BandWidth, int LongInterleave, int Integ,
744 void (*Display)(double *Spectra, int Len))
745 {
746 int err,s,i,c;
747
748 // W1HKJ
749 // variables used for generating the anti-alias filter
750 double hbw = 1.5*BandWidth / 2;
751 double omega_low = (freq - hbw);
752 double omega_high = (freq + hbw);
753 if (omega_low < 100) omega_low = 100;
754 if (omega_high > 4000) omega_high = 4000;
755 omega_low *= (M_PI / 4000);
756 omega_high *= (M_PI/ 4000);
757
758 switch(BandWidth) {
759 case 500:
760 FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 256 / 500 + 0.5);
761 AliasFilterLen = 128;
762 DecimateRatio = 8;
763 break;
764 case 1000:
765 FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 128 / 500 + 0.5);
766 AliasFilterLen = 64;
767 DecimateRatio = 4;
768 break;
769 case 2000:
770 FirstDataCarr = (int)floor((freq - BandWidth / 2.0) * 64 / 500 + 0.5);
771 AliasFilterLen = 64;
772 DecimateRatio = 2;
773 break;
774 default:
775 return -1;
776 }
777
778 DataCarriers = 64; // 64 carriers
779
780 WindowLen = SymbolLen; // the symbol length
781 RxWindow = SymbolShape; // the symbol shape
782
783 // RxWindow, WindowLen, SymbolSepar, DataCarrSepar are tuned one for another
784 // to minimize inter-symbol interference (ISI) and one should not change
785 // them independently or ISI will increase.
786
787 CarrMarkCode = 0x16918BBEL;
788
789 IntegLen = Integ; // sync. integration period
790 SymbolDiv = 4; // don't change this
791 ScanMargin = 8; // we look 8 data carriers up and down
792 SyncStep = SymbolSepar/SymbolDiv;
793
794 ProcdspDelay = IntegLen * SymbolSepar;
795
796 TrackPipeLen = IntegLen;
797
798 if (LongInterleave) {
799 DataInterleave = 64;
800 InterleavePattern = LongIntlvPatt;
801 } else {
802 DataInterleave = 32;
803 InterleavePattern = ShortIntlvPatt;
804 }
805
806 DataScanMargin = 8;
807
808 err = FFT.Preset(WindowLen);
809 if (err) goto Error;
810
811 if (dspRedspAllocArray(&FFTbuff, WindowLen)) goto Error;
812 if (dspRedspAllocArray(&FFTbuff2, WindowLen)) goto Error;
813 WindowLenMask = WindowLen - 1;
814
815 // W1HKJ
816 // InpSplit is the anti-aliasing filter that converts a real time domain
817 // signal into a complex time domain signal with pre-filtering.
818 // the black3man3 filter provides very sharp skirts with a flat
819 // passband.
820 err = InpSplit.Preset(AliasFilterLen, NULL, NULL, DecimateRatio);
821 if (err) goto Error;
822 err = InpSplit.ComputeShape(omega_low, omega_high, dspWindowBlackman3);
823 if (err) goto Error;
824
825 err = TestOfs.Preset(-0.25 * (2.0 * M_PI / WindowLen)); // for decoder tests only
826 if (err) goto Error;
827
828 err = ProcLine.Preset(ProcdspDelay + WindowLen + SymbolSepar);
829 if (err) goto Error;
830 SyncProcPtr = 0;
831
832 ScanFirst = FirstDataCarr - ScanMargin * DataCarrSepar; // first FFT bin to scan
833 if (ScanFirst < 0) ScanFirst += WindowLen;
834 ScanLen = (DataCarriers + 2 * ScanMargin) * DataCarrSepar; // number of FFT bins to scan
835
836 for (s = 0; s < SymbolDiv; s++) {
837 if (dspRedspAllocArray(&SyncPipe[s], ScanLen)) goto Error;
838 dspClearArray(SyncPipe[s], ScanLen);
839 }
840 SyncPtr = 0;
841
842 if (dspRedspAllocArray(&SyncPhCorr, ScanLen)) goto Error;
843
844 for (c = (ScanFirst * SymbolSepar) & WindowLenMask, i = 0; i < ScanLen; i++) {
845 SyncPhCorr[i].re = FFT.Twiddle[c].re * FFT.Twiddle[c].re -
846 FFT.Twiddle[c].im * FFT.Twiddle[c].im;
847 SyncPhCorr[i].im = 2 * FFT.Twiddle[c].re * FFT.Twiddle[c].im;
848 c = (c + SymbolSepar) & WindowLenMask;
849 }
850
851 for (s = 0; s < SymbolDiv; s++) {
852 if (dspRedspAllocArray(&CorrelMid[s], ScanLen)) goto Error;
853 dspClearArray(CorrelMid[s], ScanLen);
854 if (dspRedspAllocArray(&CorrelOut[s], ScanLen)) goto Error;
855 dspClearArray(CorrelOut[s], ScanLen);
856 }
857 dspLowPass2Coeff(IntegLen, W1, W2, W5);
858
859 if (dspRedspAllocArray(&dspPowerMid, ScanLen)) goto Error;
860 dspClearArray(dspPowerMid, ScanLen);
861 if (dspRedspAllocArray(&dspPowerOut, ScanLen)) goto Error;
862 dspClearArray(dspPowerOut, ScanLen);
863 dspLowPass2Coeff(IntegLen * SymbolDiv, W1p, W2p, W5p);
864
865 for (s = 0; s < SymbolDiv; s++) {
866 if (dspRedspAllocArray(&CorrelNorm[s], ScanLen)) goto Error;
867 }
868
869 FitLen = 2 * ScanMargin * DataCarrSepar;
870
871 for (s = 0; s < SymbolDiv; s++) {
872 if (dspRedspAllocArray(&CorrelAver[s], FitLen)) goto Error;
873 }
874
875 if (dspRedspAllocArray(&SymbFit, FitLen)) goto Error;
876
877 if (dspRedspAllocArray(&SymbPipe, TrackPipeLen)) goto Error;
878 dspClearArray(SymbPipe, TrackPipeLen);
879 if (dspRedspAllocArray(&FreqPipe, TrackPipeLen)) goto Error;
880 dspClearArray(FreqPipe, TrackPipeLen);
881 TrackPipePtr = 0;
882
883 SymbFitPos = ScanMargin * DataCarrSepar;
884 SyncLocked = 0;
885 SyncSymbConf = 0.0;
886 SyncFreqOfs = 0.0;
887 SyncFreqDev = 0.0;
888 SymbPtr = 0;
889 SyncSymbShift = 0.0;
890
891 SyncHoldThres = 1.5 * sqrt(1.0 / (IntegLen * DataCarriers));
892 SyncLockThres = 1.5 * SyncHoldThres;
893
894 DataProcPtr = (-ProcdspDelay);
895
896 DataScanLen = DataCarriers + 2 * DataScanMargin;
897 DataScanFirst = FirstDataCarr - DataScanMargin * DataCarrSepar;
898
899 if (dspRedspAllocArray(&RefDataSlice, DataScanLen)) goto Error;
900 dspClearArray(RefDataSlice, DataScanLen);
901
902 dspFreeArray2D(DataPipe, DataPipeLen);
903 DataPipeLen = IntegLen / 2;
904 dspLowPass2Coeff(IntegLen, dW1, dW2, dW5);
905 if (dspAllocArray2D(&DataPipe, DataPipeLen, DataScanLen)) {
906 DataPipeLen = 0;
907 goto Error;
908 }
909 dspClearArray2D(DataPipe, DataPipeLen, DataScanLen);
910
911 DataPipePtr = 0;
912
913 if (dspRedspAllocArray(&DataPwrMid, DataScanLen)) goto Error;
914 dspClearArray(DataPwrMid, DataScanLen);
915 if (dspRedspAllocArray(&DataPwrOut, DataScanLen)) goto Error;
916 dspClearArray(DataPwrOut, DataScanLen);
917
918 if (dspRedspAllocArray(&DataSqrMid, DataScanLen)) goto Error;
919 dspClearArray(DataSqrMid, DataScanLen);
920 if (dspRedspAllocArray(&DataSqrOut, DataScanLen)) goto Error;
921 dspClearArray(DataSqrOut, DataScanLen);
922
923 if (dspRedspAllocArray(&DataVect, DataScanLen)) goto Error;
924
925 if (dspRedspAllocArray(&DatadspPhase, DataScanLen)) goto Error;
926 if (dspRedspAllocArray(&DatadspPhase2, DataScanLen)) goto Error;
927
928 err = Decoder.Preset(DataCarriers, DataInterleave,
929 InterleavePattern, DataScanMargin, IntegLen);
930 if (err) goto Error;
931
932 SpectraDisplay = Display;
933 if (SpectraDisplay) {
934 if (dspRedspAllocArray(&SpectradspPower, WindowLen))
935 goto Error;
936 }
937 return 0;
938
939 Error:
940 Free();
941 return -1;
942 }
943
Process(double_buff * Input)944 int MT63rx::Process(double_buff *Input)
945 {
946 int s1,s2;
947
948 // TestOfs.Omega+=(-0.005*(2.0*M_PI/512)); // simulate frequency drift
949
950 Output.Len = 0;
951
952 // W1HKJ
953 // convert the real data input into a complex time domain signal,
954 // anti-aliased using the blackman3 filter
955 // subsequent rx signal processing takes advantage of the periodic nature
956 // of the resultant FFT of the anti-aliased input signal. Actual decoding
957 // is at baseband.
958
959 InpSplit.Process(Input);
960
961 ProcLine.Process(&InpSplit.Output);
962 // TestOfs.Process(&InpSplit.Output);
963 // ProcLine.Process(&TestOfs.Output);
964
965 // printf("New input, Len=%d/%d\n",Input->Len,ProcLine.InpLen);
966
967 while((SyncProcPtr+WindowLen) < ProcLine.InpLen) {
968 SyncProcess(ProcLine.InpPtr + SyncProcPtr);
969 // printf("SyncSymbConf=%5.2f, SyncLock=%d, SyncProcPtr=%d, SyncPtr=%d, SymbPtr=%d, SyncSymbShift=%5.1f, SyncFreqOfs=%5.2f =>",
970 // SyncSymbConf,SyncLocked,SyncProcPtr,SyncPtr,SymbPtr,SyncSymbShift,SyncFreqOfs);
971 if (SyncPtr == SymbPtr) {
972 s1 = SyncProcPtr - ProcdspDelay +
973 ((int)SyncSymbShift - SymbPtr * SyncStep);
974 s2 = s1 + SymbolSepar / 2;
975 // printf(" Sample at %d,%d (SyncProcPtr-%d), time diff.=%d\n",s1,s2,SyncProcPtr-s1,s1-DataProcPtr);
976 DataProcess(ProcLine.InpPtr + s1, ProcLine.InpPtr + s2,
977 SyncFreqOfs, s1 - DataProcPtr);
978 DataProcPtr = s1;
979 }
980 // printf("\n");
981 SyncProcPtr += SyncStep;
982 }
983 SyncProcPtr -= ProcLine.InpLen;
984 DataProcPtr -= ProcLine.InpLen;
985 return 0;
986 }
987
DoCorrelSum(dspCmpx * Correl1,dspCmpx * Correl2,dspCmpx * Aver)988 void MT63rx::DoCorrelSum(dspCmpx *Correl1, dspCmpx *Correl2, dspCmpx *Aver)
989 {
990 dspCmpx sx;
991 int i, s, d;
992
993 s = 2 * DataCarrSepar;
994 d = DataCarriers * DataCarrSepar;
995 sx.re = sx.im = 0.0;
996 for (i = 0; i < d; i+=s) {
997 sx.re += Correl1[i].re;
998 sx.im += Correl1[i].im;
999 sx.re += Correl2[i].re;
1000 sx.im += Correl2[i].im;
1001 }
1002 Aver[0].re = sx.re / DataCarriers;
1003 Aver[0].im = sx.im / DataCarriers;
1004 for (i = 0; i < (FitLen-s); ) {
1005 sx.re -= Correl1[i].re;
1006 sx.im -= Correl1[i].im;
1007 sx.re -= Correl2[i].re;
1008 sx.im -= Correl2[i].im;
1009 sx.re += Correl1[i+d].re;
1010 sx.im -= Correl1[i+d].im;
1011 sx.re += Correl2[i+d].re;
1012 sx.im -= Correl2[i+d].im;
1013 i += s;
1014 Aver[i].re = sx.re / DataCarriers;
1015 Aver[i].im = sx.im / DataCarriers; }
1016 }
1017
SyncProcess(dspCmpx * Slice)1018 void MT63rx::SyncProcess(dspCmpx *Slice)
1019 {
1020 int i, j, k, r, s, s2;
1021 double pI, pQ;
1022 dspCmpx Correl;
1023 dspCmpx *PrevSlice;
1024 double I, Q;
1025 double dI, dQ;
1026 double P,A;
1027 double w0, w1;
1028 double Fl, F0, Fu;
1029 dspCmpx SymbTime;
1030 double SymbConf, SymbShift, FreqOfs;
1031 double dspRMS;
1032 // int Loops;
1033 int Incl;
1034
1035 SyncPtr = (SyncPtr + 1) & (SymbolDiv - 1); // increment the correlators pointer
1036
1037 for (i = 0; i < WindowLen; i++) {
1038 r = FFT.BitRevIdx[i];
1039 FFTbuff[r].re = Slice[i].re * RxWindow[i];
1040 FFTbuff[r].im = Slice[i].im * RxWindow[i];
1041 }
1042 FFT.CoreProc(FFTbuff);
1043
1044 if (SpectraDisplay) {
1045 for ( i = 0,
1046 j = FirstDataCarr + (DataCarriers / 2) * DataCarrSepar -
1047 WindowLen / 2;
1048 (i < WindowLen) && ( j <WindowLen); i++,j++)
1049 SpectradspPower[i] = dspPower(FFTbuff[j]);
1050 for (j = 0; (i < WindowLen) && (j < WindowLen); i++,j++)
1051 SpectradspPower[i] = dspPower(FFTbuff[j]);
1052 (*SpectraDisplay)(SpectradspPower, WindowLen);
1053 }
1054
1055 // EnvSync.Process(FFTbuff); // experimental synchronizer
1056
1057 PrevSlice = SyncPipe[SyncPtr];
1058 for (i = 0; i < ScanLen; i++) {
1059 k = (ScanFirst+i) & WindowLenMask;
1060 I = FFTbuff[k].re;
1061 Q = FFTbuff[k].im;
1062 P = I * I + Q * Q;
1063 A = sqrt(P);
1064 if (P > 0.0) {
1065 dI = (I * I - Q * Q) / A;
1066 dQ = (2 * I * Q) / A;
1067 } else {
1068 dI = dQ = 0.0;
1069 }
1070 dspLowPass2(P, dspPowerMid[i], dspPowerOut[i], W1p, W2p, W5p);
1071 pI = PrevSlice[i].re * SyncPhCorr[i].re -
1072 PrevSlice[i].im * SyncPhCorr[i].im;
1073 pQ = PrevSlice[i].re * SyncPhCorr[i].im +
1074 PrevSlice[i].im * SyncPhCorr[i].re;
1075 Correl.re = dQ * pQ + dI * pI;
1076 Correl.im = dQ * pI - dI * pQ;
1077 dspLowPass2(&Correl, CorrelMid[SyncPtr] + i,
1078 CorrelOut[SyncPtr] + i, W1, W2, W5);
1079 PrevSlice[i].re = dI;
1080 PrevSlice[i].im = dQ;
1081 }
1082
1083 if (SyncPtr == (SymbPtr^2)) {
1084 for (s = 0; s < SymbolDiv; s++) { // normalize the correlations
1085 for (i = 0; i < ScanLen; i++) {
1086 if (dspPowerOut[i] > 0.0) {
1087 CorrelNorm[s][i].re = CorrelOut[s][i].re / dspPowerOut[i];
1088 CorrelNorm[s][i].im = CorrelOut[s][i].im / dspPowerOut[i];
1089 } else
1090 CorrelNorm[s][i].im = CorrelNorm[s][i].re = 0.0;
1091 }
1092 }
1093
1094 /*
1095 // another way to normalize - a better one ?
1096 for (i=0; i<ScanLen; i++)
1097 { for (P=0.0,s=0; s<SymbolDiv; s++)
1098 P+=dspPower(CorrelOut[s][i]);
1099 if (P>0.0)
1100 { for (s=0; s<SymbolDiv; s++)
1101 { CorrelNorm[s][i].re=CorrelOut[s][i].re/P;
1102 CorrelNorm[s][i].im=CorrelOut[s][i].im/P; }
1103 } else
1104 { for (s=0; s<SymbolDiv; s++)
1105 CorrelNorm[s][i].re=CorrelNorm[s][i].im=0.0; }
1106 }
1107 */
1108 // make a sum for each possible carrier positions
1109 for (s = 0; s < SymbolDiv; s++) {
1110 s2 = (s + SymbolDiv / 2) & (SymbolDiv - 1);
1111 for (k = 0; k < 2 * DataCarrSepar; k++)
1112 DoCorrelSum( CorrelNorm[s] + k,
1113 CorrelNorm[s2] + k + DataCarrSepar,
1114 CorrelAver[s] + k);
1115 }
1116 // symbol-shift dspPhase fitting
1117 for (i = 0; i < FitLen; i++) {
1118 SymbFit[i].re = dspAmpl(CorrelAver[0][i]) -
1119 dspAmpl(CorrelAver[2][i]);
1120 SymbFit[i].im = dspAmpl(CorrelAver[1][i]) -
1121 dspAmpl(CorrelAver[3][i]);
1122 }
1123
1124 // P=dspFindMaxdspPower(SymbFit+30,4,j); j+=30;
1125 P = dspFindMaxdspPower(SymbFit + 2, FitLen- 4 , j);
1126 j += 2;
1127 // printf("[%2d,%2d]",j,SymbFitPos);
1128 k = (j - SymbFitPos) / DataCarrSepar;
1129 if (k > 1)
1130 j -= (k - 1) * DataCarrSepar;
1131 else if (k < (-1))
1132 j -= (k + 1) * DataCarrSepar;
1133 SymbFitPos = j;
1134 // printf(" => %2d",j);
1135 if (P > 0.0) {
1136 SymbConf = dspAmpl(SymbFit[j]) +
1137 0.5 * (dspAmpl(SymbFit[j + 1]) + dspAmpl(SymbFit[j - 1]));
1138 SymbConf *= 0.5;
1139 I = SymbFit[j].re + 0.5 * (SymbFit[j - 1].re + SymbFit[j + 1].re);
1140 Q = SymbFit[j].im + 0.5 * (SymbFit[j - 1].im + SymbFit[j + 1].im);
1141 SymbTime.re = I;
1142 SymbTime.im = Q;
1143 SymbShift = (dspPhase(SymbTime) / (2 * M_PI)) * SymbolDiv;
1144 if (SymbShift < 0)
1145 SymbShift += SymbolDiv;
1146 // for (i=j-1; i<=j+1; i++) printf(" [%+5.2f,%+5.2f]",SymbFit[i].re,SymbFit[i].im);
1147 // make first estimation of FreqOfs
1148 // printf(" -> [%+5.2f,%+5.2f] =>",I,Q);
1149 // for (i=j-2; i<=j+2; i++) printf(" %+6.3f",I*SymbFit[i].re+Q*SymbFit[i].im);
1150 pI = dspScalProd(I, Q, SymbFit[j])
1151 + 0.7 * dspScalProd(I, Q, SymbFit[j - 1])
1152 + 0.7 * dspScalProd(I, Q, SymbFit[j + 1]);
1153 pQ = 0.7 * dspScalProd(I, Q, SymbFit[j + 1])
1154 - 0.7 * dspScalProd(I, Q, SymbFit[j - 1])
1155 + 0.5 * dspScalProd(I, Q, SymbFit[j + 2])
1156 - 0.5 * dspScalProd(I, Q, SymbFit[j - 2]);
1157 FreqOfs = j + dspPhase(pI, pQ) / (2.0 * M_PI / 8);
1158 /* SYNC TEST */
1159 // refine the FreqOfs
1160 i = (int)floor(FreqOfs + 0.5);
1161 s = (int)floor(SymbShift);
1162 s2 = (s + 1) & (SymbolDiv - 1);
1163 // printf(" [%5.2f,%2d,%d,%d] ",FreqOfs,i,s,s2);
1164 w0 = (s + 1 - SymbShift);
1165 w1 = (SymbShift - s);
1166 // printf(" [%4.2f,%4.2f] ",w0,w1);
1167 A = (0.5 * WindowLen) / SymbolSepar;
1168 I = w0 * CorrelAver[s][i].re + w1 * CorrelAver[s2][i].re;
1169 Q = w0 * CorrelAver[s][i].im + w1 * CorrelAver[s2][i].im;
1170 // printf(" [%5.2f,%2d] -> [%+5.2f,%+5.2f]",FreqOfs,i,I,Q);
1171 // FreqOfs=i+dspPhase(I,Q)/(2.0*M_PI)*0.5*A;
1172 // printf(" => %5.2f",FreqOfs);
1173 F0 = i + dspPhase(I, Q) / (2.0 * M_PI) * A - FreqOfs;
1174 Fl = F0 - A;
1175 Fu = F0 + A;
1176 if (fabs(Fl) < fabs(F0))
1177 FreqOfs += (fabs(Fu) < fabs(Fl)) ? Fu : Fl;
1178 else
1179 FreqOfs += (fabs(Fu) < fabs(F0)) ? Fu : F0;
1180 // printf(" => (%5.2f,%5.2f,%5.2f) => %5.2f",Fl,F0,Fu,FreqOfs);
1181
1182 } else {
1183 SymbTime.re = SymbTime.im = 0.0;
1184 SymbConf = 0.0;
1185 SymbShift = 0.0;
1186 FreqOfs = 0.0;
1187 }
1188
1189 // here we have FreqOfs and SymbTime.re/im
1190
1191 // printf("FreqOfs=%5.2f",FreqOfs);
1192
1193 if (SyncLocked) { // flip the SymbTime if it doesn't agree with the dspAverage
1194 if (dspScalProd(SymbTime, AverSymb) < 0.0) {
1195 SymbTime.re = (-SymbTime.re);
1196 SymbTime.im = (-SymbTime.im);
1197 FreqOfs -= DataCarrSepar;
1198 }
1199 // reduce the freq. offset towards the dspAverage offset
1200 A = 2 * DataCarrSepar;
1201 k = (int)floor((FreqOfs - AverFreq) / A + 0.5);
1202 FreqOfs -= k * A;
1203 /* SYNC TEST */
1204 A = (0.5 * WindowLen) / SymbolSepar;
1205 F0 = FreqOfs - AverFreq; // correct freq. auto-correlator wrap
1206 Fl = F0 - A;
1207 Fu = F0 + A;
1208 if (fabs(Fl) < fabs(F0))
1209 FreqOfs += (fabs(Fu) < fabs(Fl)) ? A : -A;
1210 else
1211 FreqOfs += (fabs(Fu) < fabs(F0)) ? A : 0.0;
1212 // printf(" => (%5.2f,%5.2f,%5.2f) => %5.2f",Fl,F0,Fu,FreqOfs);
1213
1214 } else { // of if (SyncLocked)
1215 // flip SymbTime if it doesn't agree with the previous
1216 if (dspScalProd(SymbTime, SymbPipe[TrackPipePtr]) < 0.0) {
1217 SymbTime.re = (-SymbTime.re);
1218 SymbTime.im = (-SymbTime.im);
1219 FreqOfs -= DataCarrSepar;
1220 }
1221 // reduce the FreqOfs towards zero
1222 A = 2 * DataCarrSepar;
1223 k = (int)floor(FreqOfs / A + 0.5);
1224 FreqOfs -= k * A;
1225 /* SYNC TEST */
1226 F0 = FreqOfs - FreqPipe[TrackPipePtr];
1227 Fl = F0 - A;
1228 Fu = F0 + A;
1229 if (fabs(Fl) < fabs(F0))
1230 FreqOfs += (fabs(Fu) < fabs(Fl)) ? A : -A;
1231 else
1232 FreqOfs += (fabs(Fu) < fabs(F0)) ? A : 0.0;
1233 }
1234
1235 // printf(" => [%+5.2f,%+5.2f], %5.2f",SymbTime.re,SymbTime.im,FreqOfs);
1236
1237 TrackPipePtr += 1;
1238 if (TrackPipePtr >= TrackPipeLen)
1239 TrackPipePtr -= TrackPipeLen;
1240 SymbPipe[TrackPipePtr] = SymbTime; // put SymbTime and FreqOfs into pipes
1241 FreqPipe[TrackPipePtr] = FreqOfs; // for averaging
1242
1243 // find dspAverage symbol time
1244 // Loops =
1245 dspSelFitAver( SymbPipe,
1246 TrackPipeLen,
1247 (double)3.0,
1248 4,
1249 AverSymb,
1250 dspRMS,
1251 Incl);
1252 // printf(" AverSymb=[%+5.2f,%+5.2f], dspRMS=%5.3f/%2d",
1253 // AverSymb.re,AverSymb.im,dspRMS,Incl);
1254 // find dspAverage freq. offset
1255 // Loops =
1256 dspSelFitAver( FreqPipe,
1257 TrackPipeLen,
1258 (double)2.5,
1259 4,
1260 AverFreq,
1261 dspRMS,
1262 Incl);
1263 SyncFreqDev = dspRMS;
1264 // printf(" AverFreq=%+5.2f, dspRMS=%5.3f/%2d",AverFreq,dspRMS,Incl);
1265
1266 SymbConf = dspAmpl(AverSymb);
1267 SyncSymbConf = SymbConf;
1268 SyncFreqOfs = AverFreq;
1269 if (SymbConf > 0.0) {
1270 SymbShift = dspPhase(AverSymb) / (2 * M_PI) * SymbolSepar;
1271 if (SymbShift < 0.0)
1272 SymbShift += SymbolSepar;
1273 SymbPtr = (int)floor((dspPhase(AverSymb) / (2 * M_PI)) * SymbolDiv);
1274 if (SymbPtr < 0)
1275 SymbPtr += SymbolDiv;
1276 SyncSymbShift = SymbShift;
1277 }
1278
1279 if (SyncLocked) {
1280 if ((SyncSymbConf < SyncHoldThres) || (SyncFreqDev > 0.250))
1281 SyncLocked = 0;
1282 } else {
1283 if ((SyncSymbConf > SyncLockThres) && (SyncFreqDev < 0.125))
1284 SyncLocked = 1;
1285 }
1286
1287 SyncSymbConf *= 0.5;
1288
1289 // printf(" => SyncLocked=%d, SyncSymbShift=%5.1f, SymbPtr=%d",
1290 // SyncLocked,SyncSymbShift,SymbPtr);
1291
1292 // printf("\n");
1293
1294 } // enf of if (SyncPtr==(SymbPtr^2))
1295
1296 }
1297
DataProcess(dspCmpx * EvenSlice,dspCmpx * OddSlice,double FreqOfs,int TimeDist)1298 void MT63rx::DataProcess(dspCmpx *EvenSlice, dspCmpx *OddSlice, double FreqOfs, int TimeDist)
1299 {
1300 int i, c, r;
1301 dspCmpx Freq, Phas;
1302 int incr, p;
1303 double I, Q, P;
1304 dspCmpx Dtmp;
1305 dspCmpx Ftmp;
1306
1307 // double Aver,dspRMS; int Loops,Incl;
1308
1309 // Here we pickup a symbol in the data history. The time/freq. synchronizer
1310 // told us where it is in time and at which frequency offset (FreqOfs)
1311 // TimeDist is the distance in samples from the symbol we analyzed
1312 // in the previous call to this routine
1313
1314 // FreqOfs=0.0; // for DEBUG only !
1315
1316 // printf("DataProcess: FreqOfs=%5.3f, TimeDist=%d, Locked=%d\n",
1317 // FreqOfs,TimeDist,SyncLocked);
1318
1319 P = (-2 * M_PI * FreqOfs) / WindowLen; // make ready for frequency correction
1320 Freq.re = cos(P);
1321 Freq.im = sin(P);
1322 Phas.re = 1.0;
1323 Phas.im = 0.0;
1324 for (i = 0; i < WindowLen; i++) { // prepare slices for the FFT
1325 r = FFT.BitRevIdx[i]; // multiply by window and pre-scramble
1326 // if (i==2*ScanMargin)
1327 // printf("%3d: [%5.2f,%5.2f] [%5.2f,%5.2f]\n",
1328 // i, dspPhase.re,dspPhase.im, EvenSlice[i].re,EvenSlice[i].im);
1329 CdspcmpxMultAxB(I, Q, EvenSlice[i], Phas);
1330 FFTbuff[r].re = I * RxWindow[i];
1331 FFTbuff[r].im = Q * RxWindow[i];
1332 CdspcmpxMultAxB(I, Q, OddSlice[i], Phas);
1333 FFTbuff2[r].re = I * RxWindow[i];
1334 FFTbuff2[r].im = Q * RxWindow[i];
1335 CdspcmpxMultAxB(Dtmp, Phas, Freq);
1336 Phas = Dtmp;
1337 }
1338 FFT.CoreProc(FFTbuff);
1339 FFT.CoreProc(FFTbuff2);
1340 /*
1341 printf("FFTbuff [%3d...]:",FirstDataCarr-16);
1342 for (i=FirstDataCarr-16; i<=FirstDataCarr+32; i++)
1343 printf(" %+3d/%4.2f",i-FirstDataCarr,dspAmpl(FFTbuff[i]));
1344 printf("\n");
1345
1346 printf("FFTbuff2[%3d...]:",FirstDataCarr-16);
1347 for (i=FirstDataCarr-16; i<=FirstDataCarr+32; i++)
1348 printf(" %+3d/%4.2f",i-FirstDataCarr,dspAmpl(FFTbuff2[i]));
1349 printf("\n");
1350 */
1351 // printf(" FreqOfs=%5.2f: ",FreqOfs);
1352
1353 // printf("Symbol vectors:\n");
1354 incr = (TimeDist * DataCarrSepar) & WindowLenMask; // correct FFT dspPhase shift
1355 p = (TimeDist * DataScanFirst) & WindowLenMask; // due to time shift by
1356 for (c = DataScanFirst, i = 0; i < DataScanLen; ) { // TimeDist
1357 // printf("%2d,%3d:",i,c);
1358 // printf(" [%6.3f,%6.3f] [%6.3f,%6.3f]",
1359 // FFTbuff[c].re,FFTbuff[c].im,
1360 // FFTbuff2[c+DataCarrSepar].re,FFTbuff2[c+DataCarrSepar].im);
1361 // printf(" [%6.3f,%6.3f]/[%6.3f,%6.3f]",
1362 // FFTbuff2[c].re,FFTbuff2[c].im,
1363 // FFTbuff[c+DataCarrSepar].re,FFTbuff[c+DataCarrSepar].im);
1364 // printf(" %5.3f/%5.3f",dspAmpl(FFTbuff[c]),dspAmpl(FFTbuff[c+DataCarrSepar]));
1365 // printf(" %5.3f/%5.3f",dspAmpl(FFTbuff2[c+DataCarrSepar]),dspAmpl(FFTbuff2[c]));
1366 // printf("\n");
1367 Phas = FFT.Twiddle[p];
1368 CdspcmpxMultAxB(Dtmp, RefDataSlice[i], Phas);
1369 CdspcmpxMultAxBs(DataVect[i], FFTbuff[c], Dtmp);
1370 // printf("%3d,%2d: [%8.5f,%8.5f] / %8.5f\n",
1371 // c,i,FFTbuff[c].re,FFTbuff[c].im,DataPwrOut[i]);
1372 dspLowPass2( dspPower(FFTbuff[c]),
1373 DataPwrMid[i],
1374 DataPwrOut[i], dW1, dW2, dW5);
1375 RefDataSlice[i++] = FFTbuff[c];
1376 c = (c + DataCarrSepar) & WindowLenMask;
1377 p = (p + incr) & WindowLenMask;
1378
1379 Phas = FFT.Twiddle[p];
1380 CdspcmpxMultAxB(Dtmp, RefDataSlice[i], Phas);
1381 CdspcmpxMultAxBs(DataVect[i], FFTbuff2[c], Dtmp);
1382 // printf("%3d,%2d: [%8.5f,%8.5f] / %8.5f\n",
1383 // c,i,FFTbuff2[c].re,FFTbuff2[c].im,DataPwrOut[i]);
1384 dspLowPass2( dspPower(FFTbuff2[c]),
1385 DataPwrMid[i],
1386 DataPwrOut[i], dW1, dW2, dW5);
1387 RefDataSlice[i++] = FFTbuff2[c];
1388 c = (c + DataCarrSepar) & WindowLenMask;
1389 p = (p + incr) & WindowLenMask;
1390 }
1391
1392 P = (-TimeDist * 2 * M_PI * FreqOfs) / WindowLen;
1393 Freq.re = cos(P);
1394 Freq.im = sin(P);
1395 for (i = 0; i < DataScanLen; i++) {
1396 CdspcmpxMultAxB(Ftmp, DataVect[i], Freq);
1397 // dspLowPass2(dspPower(Ftmp),DataPwrMid[i],DataPwrOut[i],dW1,dW2,dW5);
1398 // CdspcmpxMultAxB(Dtmp,Ftmp,Ftmp);
1399 // Dtmp.re=Ftmp.re*Ftmp.re-Ftmp.im*Ftmp.im; Dtmp.im=2*Ftmp.re*Ftmp.im;
1400 // dspLowPass2(&Dtmp,DataSqrMid+i,DataSqrOut+i,dW1,dW2,dW5);
1401 DataVect[i] = DataPipe[DataPipePtr][i];
1402 DataPipe[DataPipePtr][i] = Ftmp;
1403 }
1404 DataPipePtr += 1;
1405 if (DataPipePtr >= DataPipeLen)
1406 DataPipePtr = 0;
1407
1408 for (i = 0; i < DataScanLen; i++) {
1409 if (DataPwrOut[i] > 0.0) {
1410 P = DataVect[i].re / DataPwrOut[i];
1411 if (P > 1.0)
1412 P = 1.0;
1413 else if (P < (-1.0))
1414 P = (-1.0);
1415 DatadspPhase[i] = P;
1416 } else
1417 DatadspPhase[i] = 0.0;
1418 }
1419 Decoder.Process(DatadspPhase);
1420 Output.EnsureSpace(Output.Len + 1);
1421 Output.Data[Output.Len] = Decoder.Output;
1422 Output.Len += 1;
1423 /*
1424 printf("Demodulator output vectors:\n");
1425 for (i=0; i<DataScanLen; i++)
1426 { printf("%2d: [%8.5f,%8.5f] / %8.5f => %8.5f\n",
1427 i,DataVect[i].re,DataVect[i].im,DataPwrOut[i], DatadspPhase[i]);
1428 }
1429 */
1430 /*
1431 for (i=0; i<DataScanLen; i++)
1432 { // printf("%2d: [%8.5f,%8.5f]\n",i,DataVect[i].re,DataVect[i].im);
1433 if (dspPower(DataVect[i])>0.0) P=dspPhase(DataVect[i]); else P=0.0;
1434 DatadspPhase[i]=P;
1435 P*=2; if (P>M_PI) P-=2*M_PI; else if (P<(-M_PI)) P+=2*M_PI;
1436 DatadspPhase2[i]=P;
1437 printf("%2d: %6.3f [%6.3f,%6.3f] [%8.5f,%8.5f], %5.2f, %5.2f",
1438 i, DataPwrOut[i], DataSqrOut[i].re,DataSqrOut[i].im,
1439 DataVect[i].re,DataVect[i].im, DatadspPhase[i],DatadspPhase2[i]);
1440 if (DataPwrOut[i]>0.0)
1441 printf(" %6.3f",dspAmpl(DataSqrOut[i])/DataPwrOut[i]);
1442 printf("\n");
1443 }
1444 Loops=dspSelFitAver(DatadspPhase2,DataScanLen,(double)2.5,4,Aver,dspRMS,Incl);
1445 printf("Aver=%5.2f, dspRMS=%5.2f, Incl=%d\n",Aver,dspRMS,Incl);
1446 */
1447 }
1448
SYNC_LockStatus(void)1449 int MT63rx::SYNC_LockStatus(void) {
1450 return SyncLocked;
1451 }
1452
SYNC_Confidence(void)1453 double MT63rx::SYNC_Confidence(void) {
1454 return SyncSymbConf <= 1.0 ? SyncSymbConf : 1.0;
1455 }
1456
SYNC_FreqOffset(void)1457 double MT63rx::SYNC_FreqOffset(void) {
1458 return SyncFreqOfs / DataCarrSepar;
1459 }
1460
SYNC_FreqDevdspRMS(void)1461 double MT63rx::SYNC_FreqDevdspRMS(void) {
1462 return SyncFreqDev / DataCarrSepar;
1463 }
1464
SYNC_TimeOffset(void)1465 double MT63rx::SYNC_TimeOffset(void) {
1466 return SyncSymbShift / SymbolSepar;
1467 }
1468
FEC_SNR(void)1469 double MT63rx::FEC_SNR(void) {
1470 return Decoder.SignalToNoise;
1471 }
1472
FEC_CarrOffset(void)1473 int MT63rx::FEC_CarrOffset(void) {
1474 return Decoder.CarrOfs;
1475 }
1476
TotalFreqOffset(void)1477 double MT63rx::TotalFreqOffset(void) {
1478 return ( SyncFreqOfs + DataCarrSepar * Decoder.CarrOfs) *
1479 (8000.0 / DecimateRatio) / WindowLen;
1480 }
1481
1482