1 // MFSK trasnmitter and receiver code, Pawel Jalocha, December 2004
2
3 #ifndef __PJ_MFSK_H__
4 #define __PJ_MFSK_H__
5
6 // =====================================================================
7
8 #include <stdlib.h>
9 #include <stdint.h>
10 #include <math.h>
11
12 #include "pj_struc.h"
13 #include "pj_fht.h"
14 #include "pj_cmpx.h"
15 #include "pj_fft.h"
16 #include "pj_gray.h"
17 #include "pj_lowpass3.h"
18 #include "pj_fifo.h"
19
20 // =====================================================================
21
Exp2(uint32_t X)22 static inline size_t Exp2(uint32_t X) {
23 return (uint32_t)1 << X;
24 }
25
Log2(uint32_t X)26 static inline size_t Log2(uint32_t X) {
27 uint32_t Y;
28 for ( Y = 0; X > 1; X >>= 1)
29 Y += 1;
30 return Y;
31 }
32
33 // =====================================================================
34
35 // the symbol shape described in frequency domain
36 static const double MFSK_SymbolFreqShape[] =
37 { 1.0, 1.0 } ; // use raised cosine shape - experimental
38 // from gMFSK
39 // { +1.0000000000,
40 // +1.1913785723,
41 // -0.0793018558,
42 // -0.2171442026,
43 // -0.0014526076
44 //};
45 // from DM780
46 //{
47 // +1.0000000000,
48 // +2.1373197349,
49 // +1.1207588117,
50 // -0.0165609232
51 //};
52
53 static const size_t MFSK_SymbolFreqShapeLen =
54 sizeof(MFSK_SymbolFreqShape) / sizeof(double);
55
56 // =====================================================================
57
58 template <class Type=float>
59 class MFSK_Modulator
60 {
61 public:
62 // parameters to be set before calling Preset()
63 size_t SymbolLen; // length of the symbol, must be a power of 2
64 size_t FirstCarrier; // first carrier in terms of FFT freq. bins
65 size_t BitsPerSymbol; // bits per symbol => number of carriers/tones
66 int UseGrayCode;
67
68 static const size_t CarrierSepar = 2;
69
70 // parameters that are calculated by Preset()
71 size_t Carriers; // number of tones/carriers
72 size_t SymbolSepar; // time distance between symbols
73 int SymbolPhase; // the phase of the tone being transmitted
74 int Reverse; // send carriers in reverse order?
75
76 private:
77 Type *CosineTable; // Cosine table for fast cos/sin calculation
78 Type *SymbolShape; // the shape of the symbol
79 Type *OutTap; // output tap (buffer)
80 size_t TapPtr;
81 size_t WrapMask;
82
83 public:
84
MFSK_Modulator()85 MFSK_Modulator() {
86 Init();
87 Default();
88 }
89
~MFSK_Modulator()90 ~MFSK_Modulator() {
91 Free();
92 }
93
Init(void)94 void Init(void) {
95 CosineTable = 0;
96 SymbolShape = 0;
97 OutTap = 0;
98 }
99
Free(void)100 void Free(void) {
101 delete [] CosineTable;
102 CosineTable = 0;
103 delete [] SymbolShape;
104 SymbolShape = 0;
105 delete [] OutTap;
106 OutTap = 0;
107 }
108
Default(void)109 void Default(void) {
110 SymbolLen = 512;
111 FirstCarrier = 32;
112 BitsPerSymbol = 5;
113 Reverse = 0;
114 UseGrayCode = 1;
115 }
116
Preset(void)117 int Preset(void) {
118 size_t Idx;
119
120 Carriers = Exp2(BitsPerSymbol);
121 SymbolSepar = SymbolLen / 2;
122
123 if (ReallocArray(&CosineTable, SymbolLen) < 0) goto Error;
124 for (Idx = 0; Idx < SymbolLen; Idx++)
125 CosineTable[Idx] = cos((2*M_PI*Idx) / SymbolLen);
126
127 if (ReallocArray(&SymbolShape, SymbolLen) < 0) goto Error;
128
129 { size_t Time;
130 double Ampl = MFSK_SymbolFreqShape[0];
131 for (Time = 0; Time < SymbolLen; Time++)
132 SymbolShape[Time] = Ampl;
133 }
134 size_t Freq;
135 for (Freq = 1; Freq < MFSK_SymbolFreqShapeLen; Freq++) {
136 size_t Time;
137 double Ampl = MFSK_SymbolFreqShape[Freq];
138 if (Freq & 1) Ampl = (-Ampl);
139 size_t Phase = 0;
140 for (Time = 0; Time < SymbolLen; Time++) {
141 SymbolShape[Time] += Ampl*CosineTable[Phase];
142 Phase += Freq;
143 if (Phase >= SymbolLen) Phase-=SymbolLen;
144 }
145 }
146 { size_t Time;
147 double Scale = 1.0/4;
148 for (Time = 0; Time < SymbolLen; Time++)
149 SymbolShape[Time] *= Scale;
150 }
151
152 if (ReallocArray(&OutTap, SymbolLen) < 0) goto Error;
153 for (Idx = 0; Idx < SymbolLen; Idx++)
154 OutTap[Idx] = 0;
155 TapPtr = 0;
156
157 WrapMask = SymbolLen-1;
158 SymbolPhase = 0;
159
160 return 0;
161
162 Error:
163 Free();
164 return -1;
165 }
166
Send(uint8_t Symbol)167 void Send(uint8_t Symbol) {
168 if (UseGrayCode) Symbol=GrayCode(Symbol);
169 int SymbolFreq;
170
171 if (Reverse == 1) { int RevFirstCar=FirstCarrier-2; SymbolFreq=RevFirstCar-CarrierSepar*Symbol; }
172 else { SymbolFreq=FirstCarrier+CarrierSepar*Symbol; }
173
174 int TimeShift=SymbolSepar/2-SymbolLen/2;
175 SymbolPhase+=SymbolFreq*TimeShift;
176 SymbolPhase&=WrapMask;
177
178 AddSymbol(SymbolFreq,SymbolPhase);
179
180 TimeShift=SymbolSepar/2+SymbolLen/2;
181 SymbolPhase+=SymbolFreq*TimeShift;
182 SymbolPhase&=WrapMask;
183
184 int PhaseDiffer=SymbolLen/4;
185 if (rand()&1) PhaseDiffer=(-PhaseDiffer);
186 SymbolPhase+=PhaseDiffer;
187 SymbolPhase&=WrapMask;
188 }
189
190 // get output as 16-bit signed data
Output(int16_t * Buffer)191 int Output(int16_t *Buffer) {
192 const Type Scale = 32768.0;
193 const int32_t Limit = 0x7FFF;
194 size_t Idx;
195
196 for (Idx = 0; Idx < SymbolSepar; Idx++) {
197 Type Ampl = OutTap[TapPtr];
198 Ampl *= Scale;
199 int32_t Out = (int32_t)floor(Ampl + 0.5);
200 if (Out > Limit) Out = Limit;
201 else if (Out < (-Limit)) Out = (-Limit);
202 Buffer[Idx] = (int16_t)Out;
203 OutTap[TapPtr] = 0;
204 TapPtr += 1; TapPtr &= WrapMask;
205 }
206 return SymbolSepar;
207 }
208
209 template <class OutType>
Output(OutType * Buffer)210 int Output(OutType *Buffer) {
211 size_t Idx;
212 for (Idx = 0; Idx < SymbolSepar; Idx++) {
213 Buffer[Idx] = OutTap[TapPtr];
214 OutTap[TapPtr] = 0;
215 TapPtr += 1;
216 TapPtr &= WrapMask;
217 }
218 return SymbolSepar;
219 }
220
221 private:
222
AddSymbol(int Freq,int Phase)223 void AddSymbol(int Freq, int Phase) {
224 size_t Time;
225 for (Time = 0; Time < SymbolLen; Time++) {
226 // experimental use with {1.0, 1.0};
227 Type Shape=1.0-CosineTable[Time];
228 OutTap[TapPtr] += CosineTable[Phase] * Shape;
229 // OutTap[TapPtr] += CosineTable[Phase] * SymbolShape[Time];
230
231 Phase += Freq;
232 Phase &= WrapMask;
233 TapPtr += 1;
234 TapPtr &= WrapMask;
235 }
236 }
237
238 } ;
239
240 // =====================================================================
241
242 template <class TapType = float, class OutType = double>
243 class BoxFilter
244 {
245 public:
246 size_t Len;
247 TapType *Tap;
248 size_t Ptr;
249 OutType Output;
250
BoxFilter()251 BoxFilter() {
252 Tap = 0;
253 }
254
~BoxFilter()255 ~BoxFilter() {
256 delete [] Tap;
257 }
258
Free(void)259 void Free(void) {
260 delete [] Tap;
261 Tap = 0;
262 }
263
Preset(void)264 int Preset(void) {
265 if (ReallocArray(&Tap, Len) < 0)
266 return -1;
267 Clear();
268 return 0;
269 }
270
Clear(void)271 void Clear(void) {
272 size_t Idx;
273 for (Idx = 0; Idx < Len; Idx++)
274 Tap[Idx] = 0;
275 Ptr = 0;
276 Output = 0;
277 }
278
279 template <class InpType>
Process(InpType Input)280 void Process(InpType Input) {
281 Output -= Tap[Ptr];
282 Output += Input;
283 Tap[Ptr] = Input;
284 Ptr += 1;
285 if (Ptr >= Len) Ptr -= Len;
286 }
287
288 };
289
290 // =====================================================================
291
292 template <class Type>
293 class MFSK_InputProcessor
294 {
295 public:
296 size_t WindowLen;
297 size_t WrapMask;
298 Type *InpTap;
299 size_t InpTapPtr;
300 Type *OutTap;
301 size_t OutTapPtr;
302 Type *WindowShape;
303 size_t SliceSepar;
304
305 r2FFT< Cmpx<Type> > FFT; // FFT engine
306 Cmpx<Type> *FFT_Buff; // FFT buffer
307
308 size_t SpectraLen;
309 Cmpx<Type> *Spectra[2];
310
311 Type *Output;
312 Type *Energy;
313
314 BoxFilter<Type> Filter;
315
316 public:
MFSK_InputProcessor()317 MFSK_InputProcessor() {
318 Init();
319 Default();
320 }
321
~MFSK_InputProcessor()322 ~MFSK_InputProcessor() {
323 Free();
324 }
325
Init(void)326 void Init(void) {
327 InpTap = 0;
328 OutTap = 0;
329 WindowShape = 0;
330 FFT_Buff = 0;
331 Spectra[0] = 0;
332 Spectra[1] = 0;
333 Output = 0;
334 Energy = 0;
335 }
336
Free(void)337 void Free(void) {
338 delete [] InpTap; InpTap = 0;
339 delete [] OutTap; OutTap = 0;
340 delete [] WindowShape; WindowShape = 0;
341 delete [] FFT_Buff; FFT_Buff = 0;
342 delete [] Spectra[0]; Spectra[0] = 0;
343 delete [] Spectra[1]; Spectra[1] = 0;
344 delete [] Output; Output = 0;
345 delete [] Energy; Energy = 0;
346 FFT.Free();
347 Filter.Free();
348 }
349
Default(void)350 void Default(void) {
351 WindowLen = 8192;
352 }
353
Preset(void)354 int Preset(void) {
355 size_t Idx;
356 WrapMask = WindowLen - 1;
357 Type ShapeScale = 2.0 / WindowLen;
358
359 if (ReallocArray(&InpTap, WindowLen) < 0) goto Error;
360 ClearArray(InpTap, WindowLen);
361 InpTapPtr = 0;
362 if (ReallocArray(&OutTap, WindowLen) < 0) goto Error;
363 ClearArray(OutTap, WindowLen);
364 OutTapPtr = 0;
365
366 if (FFT.Preset(WindowLen) < 0) goto Error;
367 if (ReallocArray(&FFT_Buff, WindowLen) < 0) goto Error;
368 SliceSepar = WindowLen / 2;
369
370 if (ReallocArray(&WindowShape, WindowLen)< 0) goto Error;
371 for (Idx = 0; Idx < WindowLen; Idx++)
372 WindowShape[Idx] = ShapeScale * sqrt(1.0 - FFT.Twiddle[Idx].Re);
373
374 SpectraLen = WindowLen / 2;
375 if (ReallocArray(&Spectra[0], SpectraLen) < 0) goto Error;
376 if (ReallocArray(&Spectra[1], SpectraLen) < 0) goto Error;
377
378 if (ReallocArray(&Output, WindowLen) < 0) goto Error;
379 ClearArray(Output, WindowLen);
380
381 if (ReallocArray(&Energy, SpectraLen) < 0) goto Error;
382
383 Filter.Len = WindowLen / 16;
384 if (Filter.Preset() < 0) goto Error;
385
386 return 0;
387
388 Error:
389 Free();
390 return -1;
391 }
392
Reset(void)393 void Reset(void) {
394 ClearArray(InpTap, WindowLen);
395 InpTapPtr = 0;
396 ClearArray(OutTap, WindowLen);
397 OutTapPtr = 0;
398 }
399
400 void LimitSpectraPeaks( Cmpx<Type> *Spectra,
401 size_t Len = 64,
402 Type Threshold = 4.0) {
403 Filter.Len = Len;
404 Filter.Preset();
405
406 size_t MaxFreq = 3 * (SpectraLen / 4);
407 size_t Freq, Idx;
408
409 for (Freq = 0; Freq < Len; Freq++)
410 Filter.Process(Energy[Freq]);
411
412 for (Idx = Len / 2; Freq < MaxFreq; Freq++,Idx++) {
413 Filter.Process(Energy[Freq]);
414 Type Signal = Energy[Idx];
415 Type Limit = (Filter.Output/Len) * Threshold;
416 if (Signal > Limit) {
417 Spectra[Idx] *= sqrt(Limit / Signal);
418 Energy[Idx] = Limit;
419 }
420 }
421 }
422
423 void LimitOutputPeaks(Type Threshold = 2.5) {
424 size_t Idx;
425 Type RMS = 0;
426 for (Idx = 0; Idx < WindowLen; Idx++) {
427 Type Signal = Output[Idx];
428 RMS += Signal * Signal;
429 }
430 RMS = sqrt(RMS / WindowLen);
431 Type Limit = RMS * Threshold;
432
433 for (Idx = 0; Idx < WindowLen; Idx++) {
434 Type Signal = Output[Idx];
435 if (Signal > Limit)
436 Output[Idx] = Limit;
437 else if (Signal < (-Limit))
438 Output[Idx] = (-Limit);
439 }
440 }
441
442 void AverageEnergy(size_t Len = 32) {
443 Filter.Len = Len;
444 Filter.Preset();
445
446 size_t MaxFreq = 3 * (SpectraLen / 4);
447 Type Scale = 1.0 / Len;
448 size_t Len2 = Len / 2;
449 size_t Idx, Freq;
450
451 for (Freq = 0; Freq < Len; Freq++)
452 Filter.Process(Energy[Freq]);
453
454 for (Idx = 0; Idx < Len2; Idx++)
455 Energy[Idx] = Filter.Output * Scale;
456
457 for ( ; Freq < MaxFreq; Freq++,Idx++) {
458 Filter.Process(Energy[Freq]);
459 Energy[Idx] = Filter.Output * Scale;
460 }
461
462 for ( ; Idx < SpectraLen; Idx++)
463 Energy[Idx] = Filter.Output*Scale;
464 }
465
466 // here we process the spectral data
ProcessSpectra(Cmpx<Type> * Spectra)467 void ProcessSpectra(Cmpx<Type> *Spectra) {
468 size_t Freq;
469 for (Freq = 0; Freq < SpectraLen; Freq++)
470 Energy[Freq] = Spectra[Freq].Energy();
471
472 LimitSpectraPeaks(Spectra, WindowLen / 64, 4.0);
473 LimitSpectraPeaks(Spectra, WindowLen / 64, 4.0);
474 LimitSpectraPeaks(Spectra, WindowLen / 64, 4.0);
475
476 AverageEnergy(WindowLen / 96);
477 AverageEnergy(WindowLen / 64);
478
479 for (Freq = 0; Freq < SpectraLen; Freq++) {
480 Type Corr = Energy[Freq];
481 if (Corr <= 0) continue;
482 Corr = 1.0 / sqrt(Corr);
483 Spectra[Freq] *= Corr;
484 }
485 }
486
487 template <class InpType>
ProcessInpTap(InpType * Input)488 void ProcessInpTap(InpType *Input) {
489 size_t InpIdx;
490 for (InpIdx = 0; InpIdx < SliceSepar; InpIdx++) {
491 InpTap[InpTapPtr] = Input[InpIdx];
492 InpTapPtr += 1;
493 InpTapPtr &= WrapMask;
494 }
495 }
496
ProcessInpTap()497 void ProcessInpTap() {
498 size_t InpIdx;
499 for (InpIdx = 0; InpIdx < SliceSepar; InpIdx++) {
500 InpTap[InpTapPtr] = 0;
501 InpTapPtr += 1;
502 InpTapPtr &= WrapMask;
503 }
504 }
505
ProcessInpWindow_Re(void)506 void ProcessInpWindow_Re(void) {
507 size_t Time;
508 for (Time = 0; Time < WindowLen; Time++) {
509 FFT_Buff[Time].Re = InpTap[InpTapPtr] * WindowShape[Time];
510 InpTapPtr += 1;
511 InpTapPtr &= WrapMask;
512 }
513 }
514
ProcessInpWindow_Im(void)515 void ProcessInpWindow_Im(void) {
516 size_t Time;
517 for (Time = 0; Time < WindowLen; Time++) {
518 FFT_Buff[Time].Im = InpTap[InpTapPtr] * WindowShape[Time];
519 InpTapPtr += 1;
520 InpTapPtr &= WrapMask;
521 }
522 }
523
ProcessOutWindow_Re(void)524 void ProcessOutWindow_Re(void)
525 { size_t Time;
526 for (Time=0; Time<WindowLen; Time++)
527 { OutTap[OutTapPtr] += FFT_Buff[Time].Re*WindowShape[Time];
528 OutTapPtr+=1; OutTapPtr&=WrapMask; }
529 }
530
ProcessOutWindow_Im(void)531 void ProcessOutWindow_Im(void) {
532 size_t Time;
533 for (Time = 0; Time < WindowLen; Time++) {
534 OutTap[OutTapPtr] += FFT_Buff[Time].Im * WindowShape[Time];
535 OutTapPtr += 1;
536 OutTapPtr &= WrapMask;
537 }
538 }
539
ProcessOutTap(Type * Output)540 void ProcessOutTap(Type *Output) {
541 size_t OutIdx;
542 for (OutIdx = 0; OutIdx < SliceSepar; OutIdx++) {
543 Output[OutIdx] = OutTap[OutTapPtr];
544 OutTap[OutTapPtr] = 0;
545 OutTapPtr += 1;
546 OutTapPtr &= WrapMask;
547 }
548 }
549
550 template <class InpType>
Process(InpType * Input)551 int Process(InpType *Input) {
552
553 if (Input) ProcessInpTap(Input);
554 else ProcessInpTap();
555 ProcessInpWindow_Re();
556
557 if (Input) ProcessInpTap(Input+SliceSepar);
558 else ProcessInpTap();
559 ProcessInpWindow_Im();
560
561 FFT.Process(FFT_Buff);
562 FFT.SeparTwoReals(FFT_Buff, Spectra[0], Spectra[1]);
563
564 ProcessSpectra(Spectra[0]);
565 ProcessSpectra(Spectra[1]);
566
567 FFT.JoinTwoReals(Spectra[0], Spectra[1], FFT_Buff);
568 FFT.Process(FFT_Buff);
569
570 ProcessOutWindow_Re();
571 ProcessOutTap(Output);
572 ProcessOutWindow_Im();
573 ProcessOutTap(Output+SliceSepar);
574
575 LimitOutputPeaks(2.5);
576 LimitOutputPeaks(2.5);
577
578 return WindowLen;
579 }
580
581 // get output as 16-bit signed data
GetOutput(int16_t * Buffer)582 int GetOutput(int16_t *Buffer) {
583 const Type Scale = 32768.0;
584 const int32_t Limit = 0x7FFF;
585 size_t Idx;
586
587 for (Idx = 0; Idx < WindowLen; Idx++) {
588 Type Ampl = Output[Idx];
589 Ampl *= Scale;
590 int32_t Out = (int32_t)floor(Ampl + 0.5);
591 if (Out > Limit) Out = Limit;
592 else if (Out < (-Limit)) Out = (-Limit);
593 Buffer[Idx] = (int16_t)Out;
594 }
595 return WindowLen;
596 }
597
598 } ;
599
600 // =====================================================================
601
602 // A circular buffer to store history of data.
603 // Data may come as single numbers or in batches
604 // of fixed size (-> Width)
605 template <class Type>
606 class CircularBuffer
607 {
608 public:
609 size_t Width; // input/output data width (row width)
610 size_t Len; // buffer length (column height)
611 size_t Size; // total size of the storage in the buffer
612 size_t Ptr; // current pointer (counts rows)
613 Type *Data; // allocated storage
614
CircularBuffer()615 CircularBuffer() { Init(); }
~CircularBuffer()616 ~CircularBuffer() { delete [] Data; }
617
Init(void)618 void Init(void)
619 { Data = 0; Size = 0; Width = 1; }
620
Free(void)621 void Free(void)
622 { delete [] Data; Data = 0; Size = 0; }
623
624 // reset: set pointer to the beginning of the buffer
Reset(void)625 void Reset(void) { Ptr = 0; }
626
627 // preset for given length and width
Preset(void)628 int Preset(void) {
629 Size = Width * Len;
630 if (ReallocArray(&Data, Size) < 0) return -1;
631 Reset();
632 return 0;
633 }
634
635 // set all elements to given value
Set(Type & Value)636 void Set(Type &Value) {
637 size_t Idx;
638 for (Idx = 0; Idx < Size; Idx++)
639 Data[Idx]=Value;
640 }
641
642 // set all elements to zero
Clear(void)643 void Clear(void) {
644 Type Zero;
645 Zero = 0;
646 Set(Zero);
647 }
648
649 // increment the pointer (with wrapping around)
650 void IncrPtr(size_t &Ptr, size_t Step=1) {
651 Ptr += Step;
652 if (Ptr >= Len) Ptr -= Len;
653 }
654
655 // decrement the pointer (with wrapping around)
656 void DecrPtr(size_t &Ptr, size_t Step=1) {
657 if (Ptr >= Step) Ptr -= Step;
658 else Ptr += (Len - Step);
659 }
660
661 // synchronize current pointer with another circular buffer
662 template <class SrcType>
663 void operator |= (CircularBuffer<SrcType> Buffer) {
664 Ptr = Buffer.Ptr;
665 }
666
667 // advance (increment) current pointer
668 void operator += (size_t Step) {
669 IncrPtr(Ptr, Step);
670 }
671
672 // decrement current pointer
673 void operator -= (size_t Step) {
674 DecrPtr(Ptr, Step);
675 }
676
677 // index operator to get the absolute data pointer
678 Type *operator [] (size_t Idx) {
679 return Data + (Idx * Width);
680 }
681
682 // get storage pointer corresponding to an absolute pipe pointer
AbsPtr(size_t Ptr)683 Type *AbsPtr(size_t Ptr) {
684 return Data + (Ptr * Width);
685 }
686
687 // get storage pointer corresponding to current pipe pointer
CurrPtr(void)688 Type *CurrPtr(void) {
689 return Data + (Ptr * Width);
690 }
691
692 // get storage pointer corresponding to current pointer +/- offset
OffsetPtr(int Offset)693 Type *OffsetPtr(int Offset) {
694 Offset += Ptr;
695 Offset *= Width;
696 if (Offset < 0) Offset += Size;
697 else if (Offset >= (int)Size) Offset -= Size;
698 return Data + Offset;
699 }
700 };
701
702 // =====================================================================
703
704 template <class Type=float>
705 class MFSK_Demodulator
706 { public:
707
708 // parameters to be set before calling Preset()
709 size_t SymbolLen; // length of the symbol, must be a power of 2
710 size_t FirstCarrier; // first carrier in terms of FFT freq. bins
711 size_t BitsPerSymbol; // bits per symbol => number of carriers/tones
712 int UseGrayCode;
713 size_t DecodeMargin;
714 int EqualizerDepth; // leave this at 0 (disable the equalizer)
715 int Reverse;
716
717 static const size_t CarrierSepar=2;
718 static const size_t SpectraPerSymbol = 2; // FFT slices per symbol
719
720 public:
721
722 size_t Carriers; // number of tones/carriers
723 size_t SymbolSepar; // time distance between symbols
724
725 private:
726
727 size_t DecodeWidth;
728
729 size_t SymbolSepar2;
730
731 size_t WrapMask;
732
733 Type *InpTap; // input buffer
734 size_t InpTapPtr;
735
736 Type *SymbolShape; // the shape of the symbol and the FFT window
737
738 r2FFT< Cmpx<Type> > FFT; // FFT engine
739 Cmpx<Type> *FFT_Buff; // FFT buffer
740
741 size_t SpectraLen; // number of spectra points per FFT
742 Cmpx<Type> *Spectra[SpectraPerSymbol]; // two buffers for FFT spectra
743 Type *Energy[SpectraPerSymbol];
744
745 CircularBuffer<Type> EnergyBuffer;
746 LowPass3_Filter<Type> *AverageEnergy;
747 Type FilterWeight;
748
749 public:
750
MFSK_Demodulator()751 MFSK_Demodulator()
752 { Init();
753 Default(); }
754
~MFSK_Demodulator()755 ~MFSK_Demodulator()
756 { Free(); }
757
Init(void)758 void Init(void)
759 { InpTap=0;
760 SymbolShape=0;
761 FFT_Buff=0;
762 Spectra[0]=0;
763 Spectra[1]=0;
764 Energy[0]=0;
765 Energy[1]=0;
766 AverageEnergy=0; }
767
Free(void)768 void Free(void) {
769 delete [] InpTap; InpTap=0;
770 delete [] SymbolShape; SymbolShape=0;
771 delete [] FFT_Buff; FFT_Buff=0;
772 delete [] Spectra[0]; Spectra[0]=0;
773 delete [] Spectra[1]; Spectra[1]=0;
774 delete [] Energy[0]; Energy[0]=0;
775 delete [] Energy[1]; Energy[1]=0;
776 delete [] AverageEnergy; AverageEnergy=0;
777 FFT.Free();
778 EnergyBuffer.Free(); }
779
Default(void)780 void Default(void)
781 { SymbolLen=512;
782 FirstCarrier=32;
783 BitsPerSymbol=5;
784 UseGrayCode=1;
785 DecodeMargin=32;
786 Reverse=0;
787 EqualizerDepth=0; }
788
Preset(void)789 int Preset(void)
790 {
791
792 Carriers=Exp2(BitsPerSymbol);
793
794 WrapMask=SymbolLen-1;
795
796 Type ShapeScale=1.0/SymbolLen;
797
798 if (ReallocArray(&InpTap,SymbolLen)<0) goto Error;
799 ClearArray(InpTap,SymbolLen);
800 InpTapPtr=0;
801
802 if (FFT.Preset(SymbolLen)<0) goto Error;
803 if (ReallocArray(&FFT_Buff,SymbolLen)<0) goto Error;
804 SymbolSepar=SymbolLen/2;
805 SymbolSepar2=SymbolSepar/2;
806
807 if (ReallocArray(&SymbolShape,SymbolLen)<0) goto Error;
808
809 { size_t Time;
810 double Ampl=MFSK_SymbolFreqShape[0];
811 for (Time=0; Time<SymbolLen; Time++)
812 SymbolShape[Time]=Ampl;
813 }
814 size_t Freq;
815 for (Freq=1; Freq<MFSK_SymbolFreqShapeLen; Freq++)
816 { size_t Time;
817 double Ampl=MFSK_SymbolFreqShape[Freq];
818 if (Freq&1) Ampl=(-Ampl);
819 size_t Phase=0;
820 for (Time=0; Time<SymbolLen; Time++)
821 { SymbolShape[Time]+=Ampl*FFT.Twiddle[Phase].Re;
822 Phase+=Freq; if (Phase>=SymbolLen) Phase-=SymbolLen; }
823 }
824 { size_t Time;
825 for (Time=0; Time<SymbolLen; Time++)
826 SymbolShape[Time]*=ShapeScale;
827 }
828
829 SpectraLen=SymbolLen/2;
830 if (ReallocArray(&Spectra[0],SpectraLen)<0) goto Error;
831 if (ReallocArray(&Spectra[1],SpectraLen)<0) goto Error;
832
833 if (DecodeMargin>FirstCarrier) DecodeMargin=FirstCarrier;
834 DecodeWidth=(Carriers*CarrierSepar-1)+2*DecodeMargin;
835
836 if (ReallocArray(&Energy[0],DecodeWidth)<0) goto Error;
837 if (ReallocArray(&Energy[1],DecodeWidth)<0) goto Error;
838
839 if (EqualizerDepth)
840 { EnergyBuffer.Len=EqualizerDepth;
841 EnergyBuffer.Width=DecodeWidth;
842 if (EnergyBuffer.Preset()<0) goto Error;
843 EnergyBuffer.Clear();
844 if (ReallocArray(&AverageEnergy,DecodeWidth)<0) goto Error;
845 size_t Idx;
846 for (Idx=0; Idx<DecodeWidth; Idx++)
847 AverageEnergy[Idx]=0;
848 FilterWeight=1.0/EqualizerDepth;
849 }
850 else
851 { EnergyBuffer.Free();
852 if (AverageEnergy)
853 { delete [] AverageEnergy; AverageEnergy=0; }
854 }
855
856 return 0;
857
858 Error: Free(); return -1; }
859
860 template <class InpType>
Process(InpType * Input)861 void Process(InpType *Input)
862 { size_t InpIdx,Time;
863
864 for (InpIdx=0; InpIdx<SymbolSepar2; InpIdx++)
865 { InpTap[InpTapPtr]=Input[InpIdx];
866 InpTapPtr+=1; InpTapPtr&=WrapMask; }
867
868 for (Time=0; Time<SymbolLen; Time++)
869 { FFT_Buff[Time].Re=InpTap[InpTapPtr]*SymbolShape[Time];
870 InpTapPtr+=1; InpTapPtr&=WrapMask; }
871
872 for ( ; InpIdx<SymbolSepar ; InpIdx++)
873 { InpTap[InpTapPtr]=Input[InpIdx];
874 InpTapPtr+=1; InpTapPtr&=WrapMask; }
875
876 for (Time=0; Time<SymbolLen; Time++)
877 { FFT_Buff[Time].Im=InpTap[InpTapPtr]*SymbolShape[Time];
878 InpTapPtr+=1; InpTapPtr&=WrapMask; }
879
880 FFT.Process(FFT_Buff);
881 FFT.SeparTwoReals(FFT_Buff, Spectra[0], Spectra[1]);
882
883 if (EqualizerDepth)
884 { size_t Idx,Freq;
885 Type *Data0 = EnergyBuffer.OffsetPtr(0);
886 Type *Data1 = EnergyBuffer.OffsetPtr(1);
887
888 for (Idx=0; Idx<DecodeWidth; Idx++)
889 { Energy[0][Idx]=Data0[Idx];
890 Energy[1][Idx]=Data1[Idx]; }
891
892 if (Reverse==1) {
893 Freq=FirstCarrier;
894 for (Idx=0; Idx<DecodeWidth; Idx++, Freq--)
895 { Type Energy0=Spectra[0][Freq].Energy();
896 Data0[Idx]=Energy0;
897 AverageEnergy[Idx].Process(Energy0,FilterWeight);
898 Type Energy1=Spectra[1][Freq].Energy();
899 Data1[Idx]=Energy1;
900 AverageEnergy[Idx].Process(Energy1,FilterWeight); }
901 } else {
902 Freq=FirstCarrier-DecodeMargin;
903 for (Idx=0; Idx<DecodeWidth; Idx++, Freq++)
904 { Type Energy0=Spectra[0][Freq].Energy();
905 Data0[Idx]=Energy0;
906 AverageEnergy[Idx].Process(Energy0,FilterWeight);
907 Type Energy1=Spectra[1][Freq].Energy();
908 Data1[Idx]=Energy1;
909 AverageEnergy[Idx].Process(Energy1,FilterWeight); }
910 }
911 /*
912 for (Idx=0; Idx<DecodeWidth; Idx++, Freq++)
913 { Type RefEnergy=AverageEnergy[Idx].Output;
914 if (RefEnergy>0)
915 { Energy[0][Idx]/=RefEnergy;
916 Energy[1][Idx]/=RefEnergy; }
917 else
918 { Energy[0][Idx]=0;
919 Energy[1][Idx]=0; }
920 }
921 */
922 EnergyBuffer+=2;
923 }
924 else
925 { size_t Idx;
926 if (Reverse==1) {
927 size_t Freq=FirstCarrier;
928 for (Idx=0; Idx<DecodeWidth; Idx++, Freq--)
929 { Energy[0][Idx]=Spectra[0][Freq].Energy();
930 Energy[1][Idx]=Spectra[1][Freq].Energy(); }
931 } else {
932 size_t Freq=FirstCarrier-DecodeMargin;
933 for (Idx=0; Idx<DecodeWidth; Idx++, Freq++)
934 { Energy[0][Idx]=Spectra[0][Freq].Energy();
935 Energy[1][Idx]=Spectra[1][Freq].Energy(); }
936 }
937 }
938 }
939
940 uint8_t HardDecode(size_t Slice=0, int FreqOffset=0)
941 { size_t Idx;
942 Type Peak=0;
943 uint8_t PeakIdx=0;
944 Type *EnergyPtr=Energy[Slice]+(DecodeMargin+FreqOffset);
945 size_t Freq=0;
946 for (Idx=0; Idx<Carriers; Idx++)
947 { Type Energy=EnergyPtr[Freq];
948 if (Energy>Peak)
949 { Peak=Energy; PeakIdx=Idx; }
950 Freq+=CarrierSepar; }
951
952 if (UseGrayCode) PeakIdx=BinaryCode(PeakIdx);
953 return PeakIdx; }
954
955 template <class SymbType>
956 void SoftDecode(SymbType *Symbol,
957 size_t Slice=0, int FreqOffset=0)
958 { size_t Bit,Idx;
959 for (Bit=0; Bit<BitsPerSymbol; Bit++)
960 Symbol[Bit]=0;
961
962 Type *EnergyPtr=Energy[Slice]+(DecodeMargin+FreqOffset);
963 Type TotalEnergy=0;
964 size_t Freq=0;
965 for (Idx=0; Idx<Carriers; Idx++)
966 { uint8_t SymbIdx=Idx;
967 if (UseGrayCode) SymbIdx=BinaryCode(SymbIdx);
968 Type Energy=EnergyPtr[Freq];
969 Energy*=Energy;
970 TotalEnergy+=Energy;
971 uint8_t Mask=1;
972 for (Bit=0; Bit<BitsPerSymbol; Bit++)
973 { if (SymbIdx&Mask) Symbol[Bit]-=Energy;
974 else Symbol[Bit]+=Energy;
975 Mask<<=1; }
976 Freq+=CarrierSepar; }
977
978 if (TotalEnergy>0)
979 { for (Bit=0; Bit<BitsPerSymbol; Bit++)
980 Symbol[Bit]/=TotalEnergy; }
981
982 }
983
984 template <class SymbType>
985 void SoftDecode_Test(SymbType *Symbol,
986 size_t Slice=0, int FreqOffset=0)
987 { size_t Bit,Idx,Freq;
988
989 Type *EnergyPtr=Energy[Slice]+(DecodeMargin+FreqOffset);
990
991 // printf("SoftDecode:");
992
993 Type TotalEnergy=0;
994 Type PeakEnergy=0;
995 for (Freq=0,Idx=0; Idx<Carriers; Idx++,Freq+=CarrierSepar)
996 { Type Energy=EnergyPtr[Freq];
997 TotalEnergy+=Energy;
998 if (Energy>PeakEnergy) PeakEnergy=Energy; }
999 Type AverageNoise=(TotalEnergy-PeakEnergy)/(Carriers-1)/2;
1000 /*
1001 printf(" PeakEnergy/TotalEnergy=%4.3f",PeakEnergy/TotalEnergy);
1002 printf(" AverageNoise/TotalEnergy=%4.3f\n",AverageNoise/TotalEnergy);
1003 printf("Energy[%d]/Tot =",Carriers);
1004 for (Freq=0,Idx=0; Idx<Carriers; Idx++,Freq+=CarrierSepar)
1005 printf(" %4.3f",EnergyPtr[Freq]/TotalEnergy);
1006 printf("\n");
1007 */
1008 Type SymbolProb[Carriers];
1009
1010 for (Freq=0,Idx=0; Idx<Carriers; Idx++,Freq+=CarrierSepar)
1011 { Type Energy=EnergyPtr[Freq];
1012 Type NoiseEnergy=TotalEnergy-Energy;
1013 SymbolProb[Idx]=exp(-NoiseEnergy/(2*AverageNoise)); }
1014 /*
1015 printf("SymbolProb[%d] =",Carriers);
1016 for (Idx=0; Idx<Carriers; Idx++)
1017 printf(" %4.3f",SymbolProb[Idx]);
1018 printf("\n");
1019 */
1020 Type ProbCorr=0;
1021 for (Idx=0; Idx<Carriers; Idx++)
1022 ProbCorr+=SymbolProb[Idx];
1023 ProbCorr=1.0/ProbCorr;
1024 for (Idx=0; Idx<Carriers; Idx++)
1025 SymbolProb[Idx]*=ProbCorr;
1026 /*
1027 printf("SymbolProb[%d] =",Carriers);
1028 for (Idx=0; Idx<Carriers; Idx++)
1029 printf(" %4.3f",SymbolProb[Idx]);
1030 printf("\n");
1031 */
1032 uint8_t Mask=1;
1033 for (Bit=0; Bit<BitsPerSymbol; Bit++)
1034 { Type Prob0=0;
1035 Type Prob1=0;
1036 for (Idx=0; Idx<Carriers; Idx++)
1037 { uint8_t SymbIdx=Idx;
1038 if (UseGrayCode) SymbIdx=BinaryCode(SymbIdx);
1039 if (SymbIdx&Mask) Prob1+=SymbolProb[Idx];
1040 else Prob0+=SymbolProb[Idx];
1041 }
1042 Symbol[Bit]=log(Prob0/Prob1);
1043 Mask<<=1; }
1044 }
1045
1046 } ;
1047
1048 // =====================================================================
1049
1050 template <class Type>
PrintBinary(Type Number,size_t Bits)1051 void PrintBinary(Type Number, size_t Bits)
1052 { Type Mask=1; Mask<<=(Bits-1);
1053 for ( ; Bits; Bits--)
1054 { printf("%c",Number&Mask ? '1':'0');
1055 Mask>>=1; }
1056 }
1057
1058 // =====================================================================
1059
1060 class MFSK_Encoder
1061 { public:
1062
1063 size_t BitsPerSymbol;
1064 size_t BitsPerCharacter;
1065
1066 public:
1067
1068 size_t Symbols;
1069 size_t SymbolsPerBlock;
1070
1071 bool bContestia;
1072 // bool bRTTYM;
1073
1074 private:
1075
1076 static const uint64_t ScramblingCodeOlivia = 0xE257E6D0291574ECLL;
1077 static const uint64_t ScramblingCodeContestia = 0xEDB88320LL;
1078 // static const uint64_t ScramblingCodeRTTYM = 0xEDB88320LL;
1079 uint64_t ScramblingCode;
1080
1081 int8_t *FHT_Buffer;
1082
1083 public:
1084
1085 uint8_t *OutputBlock;
1086
1087 public:
1088
MFSK_Encoder()1089 MFSK_Encoder() {
1090 Default();
1091 Init();
1092 }
1093
~MFSK_Encoder()1094 ~MFSK_Encoder()
1095 { Free(); }
1096
Default(void)1097 void Default(void) {
1098 bContestia = false;
1099 BitsPerSymbol=5;
1100 BitsPerCharacter=7;
1101 ScramblingCode = ScramblingCodeOlivia;
1102 }
1103
Init(void)1104 void Init(void)
1105 { FHT_Buffer=0;
1106 OutputBlock=0; }
1107
Free(void)1108 void Free(void)
1109 { delete [] FHT_Buffer; FHT_Buffer=0;
1110 delete [] OutputBlock; OutputBlock=0; }
1111
Preset(void)1112 int Preset(void) {
1113 if (bContestia) {
1114 ScramblingCode = ScramblingCodeContestia;
1115 BitsPerCharacter = 6;
1116 // } else if (bRTTYM) {
1117 // ScramblingCode = ScramblingCodeRTTYM;
1118 // BitsPerCharacter = 5;
1119 } else { // standard Olivia
1120 ScramblingCode = ScramblingCodeOlivia;
1121 BitsPerCharacter = 7;
1122 }
1123 Symbols = 1 << BitsPerSymbol;
1124 SymbolsPerBlock = Exp2(BitsPerCharacter-1);
1125 if (ReallocArray(&FHT_Buffer,SymbolsPerBlock)<0) goto Error;
1126 if (ReallocArray(&OutputBlock,SymbolsPerBlock)<0) goto Error;
1127 return 0;
1128 Error:
1129 Free();
1130 return -1;
1131 }
1132
EncodeCharacter(uint8_t Char)1133 void EncodeCharacter(uint8_t Char) {
1134 size_t TimeBit;
1135 uint8_t Mask = (SymbolsPerBlock << 1) - 1;
1136
1137 if (bContestia) {
1138 if (Char >= 'a' && Char <= 'z')
1139 Char += 'A' - 'a';
1140 if (Char == ' ')
1141 Char = 59;
1142 else if (Char == '\r')
1143 Char = 60;
1144 else if (Char == '\n')
1145 Char = 0;
1146 else if (Char >= 33 && Char <= 90)
1147 Char -= 32;
1148 else if (Char == 8)
1149 Char = 61;
1150 else if (Char == 0)
1151 Char = 0;
1152 else
1153 Char = '?' - 32;
1154 // } else if (bRTTYM) {
1155 } else {
1156 Char &= Mask;
1157 }
1158
1159 for (TimeBit = 0; TimeBit < SymbolsPerBlock; TimeBit++)
1160 FHT_Buffer[TimeBit] = 0;
1161 if (Char<SymbolsPerBlock)
1162 FHT_Buffer[Char] = 1;
1163 else
1164 FHT_Buffer[Char-SymbolsPerBlock] = (-1);
1165 IFHT(FHT_Buffer, SymbolsPerBlock);
1166 }
1167
1168 void ScrambleFHT(size_t CodeOffset=0)
1169 { size_t TimeBit;
1170 size_t CodeWrap=(SymbolsPerBlock-1);
1171 size_t CodeBit=CodeOffset&CodeWrap;
1172 for (TimeBit=0; TimeBit<SymbolsPerBlock; TimeBit++)
1173 { uint64_t CodeMask=1; CodeMask<<=CodeBit;
1174 if (ScramblingCode&CodeMask)
1175 FHT_Buffer[TimeBit] = (-FHT_Buffer[TimeBit]);
1176 CodeBit+=1; CodeBit&=CodeWrap; }
1177 }
1178
EncodeBlock(uint8_t * InputBlock)1179 void EncodeBlock(uint8_t *InputBlock) {
1180 size_t FreqBit;
1181 size_t TimeBit;
1182 size_t nShift;
1183
1184 // nShift = (bContestia || bRTTYM) ? 5 : 13; // Contestia/RTTYM or Olivia
1185 nShift = (bContestia) ? 5 : 13; // Contestia/RTTYM or Olivia
1186
1187 for (TimeBit = 0; TimeBit < SymbolsPerBlock; TimeBit ++)
1188 OutputBlock[TimeBit] = 0;
1189
1190 for (FreqBit = 0; FreqBit < BitsPerSymbol; FreqBit++) {
1191 EncodeCharacter(InputBlock[FreqBit]);
1192 ScrambleFHT(FreqBit * nShift);
1193 size_t Rotate = 0;
1194 for (TimeBit = 0; TimeBit < SymbolsPerBlock; TimeBit++) {
1195 if (FHT_Buffer[TimeBit] < 0) {
1196 size_t Bit = FreqBit+Rotate;
1197 if (Bit >= BitsPerSymbol) Bit -= BitsPerSymbol;
1198 uint8_t Mask = 1;
1199 Mask <<= Bit;
1200 OutputBlock[TimeBit] |= Mask;
1201 }
1202 Rotate += 1;
1203 if (Rotate >= BitsPerSymbol)
1204 Rotate -= BitsPerSymbol;
1205 }
1206 }
1207 }
1208
PrintOutputBlock(void)1209 void PrintOutputBlock(void)
1210 { size_t TimeBit;
1211 for (TimeBit=0; TimeBit<SymbolsPerBlock; TimeBit++)
1212 { printf("%2d: ",(int)TimeBit);
1213 PrintBinary(OutputBlock[TimeBit],BitsPerSymbol);
1214 printf("\n"); }
1215 }
1216
1217 } ;
1218
1219 template <class InpType, class CalcType>
1220 class MFSK_SoftDecoder
1221 {
1222 public:
1223
1224 size_t BitsPerSymbol; // number of bits per symbol
1225 size_t BitsPerCharacter; // number of bits per character
1226 size_t Symbols; // number of symbols
1227 size_t SymbolsPerBlock; // number of symbols per FEC block
1228 float Signal, NoiseEnergy;
1229 uint8_t *OutputBlock;
1230
1231 bool bContestia;
1232 // bool bRTTYM;
1233
1234 private:
1235 static const uint64_t ScramblingCodeOlivia = 0xE257E6D0291574ECLL;
1236 static const uint64_t ScramblingCodeContestia = 0xEDB88320LL;
1237 // static const uint64_t ScramblingCodeRTTYM = 0xEDB88320LL;
1238 uint64_t ScramblingCode;
1239
1240 size_t InputBufferLen;
1241 InpType *InputBuffer;
1242 size_t InputPtr;
1243
1244 CalcType *FHT_Buffer;
1245
1246 public:
1247
MFSK_SoftDecoder()1248 MFSK_SoftDecoder() {
1249 bContestia = false;
1250 Init();
1251 Default();
1252 }
1253
~MFSK_SoftDecoder()1254 ~MFSK_SoftDecoder() {
1255 Free();
1256 }
1257
Default(void)1258 void Default(void) {
1259 bContestia = false;
1260 BitsPerSymbol = 5;
1261 BitsPerCharacter = 7;
1262 ScramblingCode = ScramblingCodeOlivia;
1263 }
1264
Init(void)1265 void Init(void) {
1266 InputBuffer = 0;
1267 FHT_Buffer = 0;
1268 OutputBlock = 0;
1269 }
1270
Free(void)1271 void Free(void) {
1272 delete [] InputBuffer;
1273 InputBuffer = 0;
1274 delete [] FHT_Buffer;
1275 FHT_Buffer = 0;
1276 delete [] OutputBlock;
1277 OutputBlock = 0;
1278 }
1279
Reset(void)1280 void Reset(void) {
1281 size_t Idx;
1282 for (Idx = 0; Idx < InputBufferLen; Idx++)
1283 InputBuffer[Idx] = 0;
1284 InputPtr = 0;
1285 }
1286
Preset(void)1287 int Preset(void) {
1288 // if (bRTTYM) {
1289 // BitsPerCharacter = 5;
1290 // ScramblingCode = ScramblingCodeRTTYM;
1291 // } else
1292 if (bContestia) {
1293 BitsPerCharacter = 6;
1294 ScramblingCode = ScramblingCodeContestia;
1295 } else {
1296 BitsPerCharacter = 7;
1297 ScramblingCode = ScramblingCodeOlivia;
1298 }
1299
1300 Symbols = 1 << BitsPerSymbol;
1301 SymbolsPerBlock = Exp2(BitsPerCharacter - 1);
1302 InputBufferLen = SymbolsPerBlock * BitsPerSymbol;
1303 if (ReallocArray(&InputBuffer, InputBufferLen) < 0) goto Error;
1304 if (ReallocArray(&FHT_Buffer, SymbolsPerBlock) < 0) goto Error;
1305 if (ReallocArray(&OutputBlock, BitsPerSymbol) < 0) goto Error;
1306 Reset();
1307 return 0;
1308 Error:
1309 Free();
1310 return -1;
1311 }
1312
Preset(MFSK_SoftDecoder<InpType,CalcType> & RefDecoder)1313 int Preset(MFSK_SoftDecoder<InpType,CalcType> &RefDecoder) {
1314 BitsPerSymbol = RefDecoder.BitsPerSymbol;
1315 // BitsPerCharacter = RefDecoder.BitsPerCharacter;
1316 return Preset();
1317 }
1318
Input(InpType * Symbol)1319 void Input(InpType *Symbol) {
1320 size_t FreqBit;
1321 for (FreqBit = 0; FreqBit < BitsPerSymbol; FreqBit++) {
1322 InputBuffer[InputPtr] = Symbol[FreqBit];
1323 InputPtr += 1;
1324 }
1325 if (InputPtr >= InputBufferLen) InputPtr -= InputBufferLen;
1326 }
1327
DecodeCharacter(size_t FreqBit)1328 void DecodeCharacter(size_t FreqBit) {
1329 size_t TimeBit;
1330 size_t Ptr = InputPtr;
1331 size_t Rotate = FreqBit;
1332 size_t CodeWrap = (SymbolsPerBlock - 1);
1333 // Olivia (13 bit shift) or Contestia/RTTYM (5 bit shift)
1334 // size_t nShift = (bContestia || bRTTYM) ? 5 : 13;
1335 size_t nShift = (bContestia) ? 5 : 13;
1336
1337 size_t CodeBit = FreqBit * nShift;
1338
1339 CodeBit &= CodeWrap;
1340 for (TimeBit = 0; TimeBit < SymbolsPerBlock; TimeBit++) {
1341 InpType Bit = InputBuffer[Ptr + Rotate];
1342 uint64_t CodeMask = 1;
1343 CodeMask <<= CodeBit;
1344 if (ScramblingCode & CodeMask) Bit = (-Bit);
1345 FHT_Buffer[TimeBit] = Bit;
1346 CodeBit += 1;
1347 CodeBit &= CodeWrap;
1348 Rotate += 1;
1349 if (Rotate >= BitsPerSymbol) Rotate -= BitsPerSymbol;
1350 Ptr += BitsPerSymbol;
1351 if (Ptr >= InputBufferLen) Ptr -= InputBufferLen;
1352 }
1353 FHT(FHT_Buffer, SymbolsPerBlock);
1354 CalcType Peak = 0;
1355 size_t PeakPos = 0;
1356 CalcType SqrSum = 0;
1357 for (TimeBit = 0; TimeBit < SymbolsPerBlock; TimeBit++) {
1358 CalcType Signal = FHT_Buffer[TimeBit];
1359 SqrSum += Signal * Signal;
1360 if (fabs(Signal) > fabs(Peak)) {
1361 Peak = Signal;
1362 PeakPos = TimeBit;
1363 }
1364 }
1365 uint8_t Char = PeakPos;
1366 if (Peak < 0) Char += SymbolsPerBlock;
1367 SqrSum -= Peak * Peak;
1368
1369 if (bContestia && Char > 0) {
1370 if (Char == 59)
1371 Char = ' ';
1372 else if (Char == 60)
1373 Char = '\r';
1374 else if (Char == 61)
1375 Char = 8; // backspace
1376 else
1377 Char += 32;
1378 }
1379 OutputBlock[FreqBit] = Char;
1380
1381 NoiseEnergy += (float)SqrSum / (SymbolsPerBlock - 1);
1382 Signal += fabs(Peak);
1383 }
1384
Process(void)1385 void Process(void) {
1386 size_t FreqBit;
1387 Signal = 0;
1388 NoiseEnergy = 0;
1389 for (FreqBit = 0; FreqBit < BitsPerSymbol; FreqBit++)
1390 DecodeCharacter(FreqBit);
1391 Signal /= BitsPerSymbol;
1392 NoiseEnergy /= BitsPerSymbol;
1393 }
1394
Output(uint8_t * Buffer)1395 size_t Output(uint8_t *Buffer) {
1396 size_t FreqBit;
1397 for (FreqBit = 0; FreqBit < BitsPerSymbol; FreqBit++)
1398 Buffer[FreqBit] = OutputBlock[FreqBit];
1399 return BitsPerSymbol;
1400 }
1401
Output(uint64_t & PackedBuffer)1402 size_t Output(uint64_t &PackedBuffer) {
1403 size_t FreqBit;
1404 PackedBuffer = 0;
1405 for (FreqBit = BitsPerSymbol; FreqBit > 0; ) {
1406 PackedBuffer <<= 8;
1407 FreqBit--;
1408 PackedBuffer |= OutputBlock[FreqBit];
1409 }
1410 return BitsPerSymbol;
1411 }
1412
1413 void PrintOutputBlock(FILE *File = stdout) {
1414 size_t FreqBit;
1415 fprintf(File, "'");
1416 for (FreqBit = 0; FreqBit < BitsPerSymbol; FreqBit++) {
1417 uint8_t Char = OutputBlock[FreqBit];
1418 fprintf(File, "%c", (Char >= ' ') && (Char < 127) ? Char:' ');
1419 }
1420 fprintf(File, "'");
1421 if (NoiseEnergy > 0)
1422 fprintf(File, ", S/N = %5.1f", Signal / sqrt(NoiseEnergy));
1423 fprintf(File, "\n");
1424 }
1425
Output(uint64_t * PackedBuffer)1426 size_t Output(uint64_t *PackedBuffer) {
1427 return Output(*PackedBuffer);
1428 }
1429
1430 };
1431
1432 // =====================================================================
1433
1434 // rate converter
1435 template <class Type=float>
1436 class RateConverter
1437 {
1438 public:
1439 // parameters to be set by the user
1440 size_t TapLen; // filter tap length (in term of input samples)
1441 size_t OverSampling; // internal oversampling factor
1442 Type UpperFreq; // upper frequency of the (lowpass) filter (in terms of input sampling rate)
1443 Type OutputRate; // the output rate (in terms of the input rate)
1444
1445 private:
1446 size_t FilterLen; // the total length of the filter (in term of oversampled rate)
1447 Type *FilterShape; // the shape of the filter
1448 Type *InputTap; // filter tap
1449 size_t InputTapPtr;
1450 size_t InputWrap;
1451
1452 Type OutputTime;
1453 Type OutputPeriod;
1454 Type OutputBefore;
1455 Type OutputAfter;
1456 size_t OutputPtr;
1457
1458 public:
1459
RateConverter()1460 RateConverter() {
1461 Init();
1462 Default();
1463 }
1464
~RateConverter()1465 ~RateConverter() {
1466 Free();
1467 }
1468
Init(void)1469 void Init(void) {
1470 FilterShape = 0;
1471 InputTap = 0;
1472 }
1473
Free(void)1474 void Free(void) {
1475 delete [] FilterShape;
1476 FilterShape = 0;
1477 delete [] InputTap;
1478 InputTap = 0;
1479 }
1480
Default(void)1481 void Default(void) {
1482 TapLen = 16;
1483 OverSampling = 16;
1484 UpperFreq = 3.0 / 8;
1485 OutputRate = 1.0;
1486 }
1487
Preset(void)1488 int Preset(void) {
1489 size_t Idx;
1490
1491 TapLen = Exp2(Log2(TapLen));
1492 FilterLen = TapLen * OverSampling;
1493
1494 if ((ReallocArray(&FilterShape, FilterLen)) < 0) goto Error;
1495 if ((ReallocArray(&InputTap, TapLen)) < 0) goto Error;
1496
1497 for (Idx = 0; Idx < FilterLen; Idx++) {
1498 Type Phase = (M_PI * (2 * (int)Idx - (int)FilterLen)) / FilterLen;
1499 // Hanning
1500 // Type Window = 0.50 + 0.50 * cos(Phase);
1501 // Blackman
1502 // Type Window = 0.42 + 0.50 * cos(Phase) + 0.08 * cos(2 * Phase);
1503 // Blackman-Harris
1504 Type Window = 0.35875 + 0.48829 * cos(Phase) +
1505 0.14128 * cos(2 * Phase) + 0.01168 * cos(3 * Phase);
1506 Type Filter = 1.0;
1507 if (Phase != 0) {
1508 Phase *= (UpperFreq * TapLen);
1509 Filter = sin(Phase) / Phase;
1510 }
1511 // printf("%3d: %+9.6f %+9.6f %+9.6f\n", Idx, Window, Filter, Window*Filter);
1512 FilterShape[Idx] = Window * Filter;
1513 }
1514 Reset();
1515 return 0;
1516 Error:
1517 Free();
1518 return -1;
1519 }
1520
Reset(void)1521 void Reset(void) {
1522 size_t Idx;
1523
1524 InputWrap = TapLen - 1;
1525 for (Idx = 0; Idx < TapLen; Idx++)
1526 InputTap[Idx] = 0;
1527 InputTapPtr = 0;
1528
1529 OutputTime = 0;
1530 OutputPeriod = OverSampling / OutputRate;
1531 OutputBefore = 0;
1532 OutputAfter = 0;
1533 OutputPtr = 0;
1534 }
1535
1536 private:
1537
1538 Type Convolute(size_t Shift=0) {
1539 Type Sum = 0;
1540 Shift = (OverSampling - 1) - Shift;
1541 size_t Idx = InputTapPtr;
1542 for ( ; Shift < FilterLen; Shift += OverSampling) {
1543 Sum += InputTap[Idx] * FilterShape[Shift];
1544 Idx += 1;
1545 Idx &= InputWrap;
1546 }
1547 return Sum;
1548 }
1549
NewInput(Type Input)1550 void NewInput(Type Input)
1551 { // printf("I:\n");
1552 InputTap[InputTapPtr]=Input;
1553 InputTapPtr+=1; InputTapPtr&=InputWrap; }
1554
1555 public:
1556
1557 template <class InpType, class OutType>
Process(InpType * Input,size_t InputLen,OutType * Output)1558 int Process(InpType *Input, size_t InputLen, OutType *Output) {
1559 size_t OutputLen = 0;
1560 // printf("E: %d %3.1f %d %d\n",OutputPtr, OutputTime, InputLen, OutputLen);
1561 for ( ; ; ) {
1562 // printf("L: %d %3.1f %d %d\n",OutputPtr, OutputTime, InputLen, OutputLen);
1563 if (OutputPtr) {
1564 size_t Idx = (size_t)floor(OutputTime) + 1;
1565 if (Idx >= OverSampling) {
1566 if (InputLen == 0) break;
1567 NewInput(*Input);
1568 Input++;
1569 InputLen -= 1;
1570 Idx -= OverSampling;
1571 OutputTime -= (Type)OverSampling;
1572 }
1573 OutputAfter = Convolute(Idx);
1574 Type Weight = Idx - OutputTime;
1575 (*Output) = Weight * OutputBefore + (1.0 - Weight) * OutputAfter;
1576 Output++;
1577 OutputLen += 1;
1578 // printf("O: %d %3.1f %d %d %d\n",OutputPtr, OutputTime, InputLen, OutputLen, Idx);
1579 OutputPtr = 0;
1580 } else {
1581 size_t Idx = (size_t)floor(OutputTime + OutputPeriod);
1582 if (Idx >= OverSampling) {
1583 if (InputLen == 0) break;
1584 NewInput(*Input);
1585 Input++;
1586 InputLen -= 1;
1587 Idx -= OverSampling;
1588 OutputTime -= (Type)OverSampling;
1589 }
1590 OutputBefore = Convolute(Idx);
1591 OutputTime += OutputPeriod;
1592 OutputPtr = 1;
1593 }
1594 }
1595 // printf("R: %d %3.1f %d %d\n",OutputPtr, OutputTime, InputLen, OutputLen);
1596 return OutputLen;
1597 }
1598
1599 template <class InpType, class OutType>
Process(InpType Input,size_t InputLen,Seq<OutType> & Output)1600 int Process(InpType Input, size_t InputLen, Seq<OutType> &Output) {
1601 size_t OutPtr = Output.Len;
1602 size_t MaxOutLen = (size_t)ceil(InputLen * OutputRate + 2);
1603 if (Output.EnsureSpace(OutPtr + MaxOutLen) < 0) return -1;
1604 int OutLen = Process(Input, InputLen, Output.Elem+OutPtr);
1605 Output.Len += OutLen;
1606 return OutLen;
1607 }
1608
1609 template <class InpType, class OutType>
Process(InpType Input,OutType * Output)1610 int Process(InpType Input, OutType *Output) {
1611 return Process(&Input, 1, Output);
1612 }
1613
1614 };
1615
1616 // =====================================================================
1617
1618 template <class Type=float>
1619 class MFSK_Transmitter
1620 {
1621 public:
1622 // primary parameters: set by the user
1623 bool bContestia;
1624 size_t Tones; // number of tones: 4, 8, 16, 32, 64, 128, 256
1625 size_t Bandwidth; // bandwidth: 125, 250, 500, 1000, 2000
1626 Type SampleRate; // audio sampling rate (internal processing)
1627 Type OutputSampleRate; // true sampling rate of the soundcard
1628 float FirstCarrierMultiplier;
1629 int Reverse;
1630 // secondary parameters: calculated by Preset()
1631 size_t BitsPerSymbol; // number of bits per symbol
1632 size_t SymbolsPerBlock; // number of symbols per one FEC code block
1633 size_t MaxOutputLen; // maximum length of the audio batch returned by Output()
1634
1635 private:
1636 static const int State_Running = 0x0001;
1637 static const int State_StopReq = 0x0010;
1638 int State;
1639
1640 FIFO<uint8_t> Input; // buffer(queue) for the characters to be encoded
1641 uint8_t InputBlock[8]; // FEC code block buffer
1642 FIFO<uint8_t> Monitor; // buffer for monitoring the characters being sent
1643
1644 MFSK_Encoder Encoder; // FEC encoder
1645 size_t SymbolPtr;
1646
1647 MFSK_Modulator<Type> Modulator; // MFSK modulator
1648
1649 Type *ModulatorOutput;
1650
1651 RateConverter<Type> Converter; // output rate converter
1652
1653 Type *ConverterOutput;
1654
1655 public:
1656
MFSK_Transmitter()1657 MFSK_Transmitter() {
1658 bContestia = false;
1659 Init();
1660 }
1661
~MFSK_Transmitter()1662 ~MFSK_Transmitter() {
1663 Free();
1664 }
1665
Init(void)1666 void Init(void) {
1667 ModulatorOutput = 0;
1668 ConverterOutput = 0;
1669 }
1670
Free(void)1671 void Free(void) {
1672 Input.Free();
1673 Monitor.Free();
1674 Encoder.Free();
1675 Modulator.Free();
1676 delete [] ModulatorOutput;
1677 ModulatorOutput = 0;
1678 Converter.Free();
1679 delete [] ConverterOutput;
1680 ConverterOutput = 0;
1681 }
1682
1683 // set default primary parameters
Default(void)1684 void Default(void) {
1685 Tones = 32;
1686 Bandwidth = 1000;
1687 Reverse = 0;
1688 SampleRate = 8000;
1689 OutputSampleRate = 8000;
1690 }
1691
1692 // preset internal arrays according to primary paramaters
Preset(void)1693 int Preset(void) {
1694 // impose limits on the primary parameters
1695 if (Tones < 2 ) Tones = 2;
1696 else if (Tones > 256) Tones = 256;
1697 if (Bandwidth < 125) Bandwidth = 125;
1698 else if (Bandwidth > 2000) Bandwidth = 2000;
1699
1700 // calculate the secondary parameters
1701 BitsPerSymbol = Log2(Tones);
1702 Tones = Exp2(BitsPerSymbol);
1703
1704 // preset the input character buffer
1705 Input.Len = 1024;
1706 if (Input.Preset() <0 ) goto Error;
1707 Monitor.Len = 256;
1708 if (Monitor.Preset() < 0) goto Error;
1709
1710 // preset the encoder
1711 Encoder.bContestia = bContestia;
1712 Encoder.BitsPerSymbol = BitsPerSymbol;
1713 if (Encoder.Preset() < 0) goto Error;
1714 SymbolsPerBlock = Encoder.SymbolsPerBlock;
1715
1716 // preset the modulator
1717 Modulator.Default();
1718 Modulator.BitsPerSymbol = BitsPerSymbol;
1719 Modulator.SymbolLen =
1720 (size_t)1 << (BitsPerSymbol + 7 - Log2(Bandwidth / 125));
1721 Bandwidth = Exp2(Log2(Bandwidth / 125)) * 125;
1722 Modulator.FirstCarrier =
1723 (size_t) ((Modulator.SymbolLen / 16) * FirstCarrierMultiplier) + 1;
1724 Modulator.Reverse = Reverse;
1725 if (Modulator.Preset() < 0) goto Error;
1726
1727 if (ReallocArray(&ModulatorOutput, Modulator.SymbolSepar) < 0)
1728 goto Error;
1729
1730 // preset the rate converter
1731 Converter.OutputRate = OutputSampleRate / SampleRate;
1732 if (Converter.Preset() < 0) goto Error;
1733
1734 MaxOutputLen =
1735 (size_t)ceil(Modulator.SymbolSepar * OutputSampleRate / SampleRate + 2);
1736 if (ReallocArray(&ConverterOutput, MaxOutputLen) < 0) goto Error;
1737
1738 // reset the state logic
1739 SymbolPtr = 0;
1740 State = 0;
1741
1742 return 0;
1743
1744 Error:
1745 Free();
1746 return -1;
1747 }
1748
Reset(void)1749 void Reset(void) {
1750 Input.Reset();
1751 Monitor.Reset();
1752 SymbolPtr = 0;
1753 State = 0;
1754 Converter.Reset();
1755 }
1756
BaudRate(void)1757 Type BaudRate(void) {
1758 return SampleRate / Modulator.SymbolSepar;
1759 }
1760
1761
BlockPeriod(void)1762 Type BlockPeriod(void) {
1763 return (SymbolsPerBlock * Modulator.SymbolSepar) / SampleRate;
1764 }
1765
CharactersPerSecond(void)1766 Type CharactersPerSecond(void) {
1767 return BitsPerSymbol * SampleRate /
1768 (SymbolsPerBlock * Modulator.SymbolSepar);
1769 }
1770
1771 // start the transmission
Start(void)1772 void Start(void) {
1773 State |= State_Running;
1774 }
1775
1776 // request to stop (and complete) the transmission
1777 // but the transmitter will only stop after transmitting all the data
Stop(void)1778 void Stop(void) {
1779 State |= State_StopReq;
1780 }
1781
1782 // check if the transmission is still running (not complete)
Running(void)1783 int Running(void) {
1784 return State & State_Running;
1785 }
1786
1787 // put the character into the transmitter input queue
PutChar(uint8_t Char)1788 int PutChar(uint8_t Char) {
1789 return Input.Write(Char);
1790 }
1791
1792 // get one character from the monitor buffer
GetChar(uint8_t & Char)1793 int GetChar(uint8_t &Char) {
1794 return Monitor.Read(Char);
1795 }
1796
GetReadReady(void)1797 size_t GetReadReady(void) {
1798 return Input.ReadReady();
1799 }
1800
DoPostambleYet(void)1801 int DoPostambleYet(void) {
1802 if (State == 0)
1803 return 1;
1804 else
1805 return 0;
1806 }
1807
GetSymbolPhase(void)1808 double GetSymbolPhase(void) {
1809 return Modulator.SymbolPhase;
1810 }
1811
1812 // get out the transmitter output (audio)
Output(double * Buffer)1813 int Output(double *Buffer) {
1814 if (SymbolPtr == 0) {
1815 if ((State&State_StopReq) && Input.Empty()) {
1816 State=0;
1817 } else if (State&State_Running) {
1818 size_t Idx;
1819 for (Idx = 0; Idx < BitsPerSymbol; Idx++) {
1820 uint8_t Char;
1821 if (Input.Read(Char) <= 0) break;
1822 InputBlock[Idx] = Char;
1823 Monitor.Write(Char);
1824 }
1825 for ( ; Idx < BitsPerSymbol; Idx++)
1826 InputBlock[Idx] = 0;
1827 Encoder.EncodeBlock(InputBlock);
1828 }
1829 }
1830 if (State&State_Running) {
1831 Modulator.Send(Encoder.OutputBlock[SymbolPtr]);
1832 SymbolPtr += 1;
1833 if (SymbolPtr >= SymbolsPerBlock) SymbolPtr = 0;
1834 }
1835 int ModLen = Modulator.Output(ModulatorOutput);
1836 int ConvLen = Converter.Process(ModulatorOutput, ModLen, ConverterOutput);
1837 if (ConvLen < 0) return ConvLen;
1838
1839 double maxval = 0, tempnum;
1840 for (int Idx = 0; Idx < ConvLen; Idx++)
1841 if ((tempnum = fabs(ConverterOutput[Idx])) > maxval)
1842 maxval = tempnum;
1843 for (int Idx = 0; Idx < ConvLen; Idx++)
1844 Buffer[Idx] = (double) ConverterOutput[Idx] / maxval;
1845
1846 return ConvLen;
1847 }
1848
1849 };
1850
1851 // =====================================================================
1852
1853 /*
1854
1855 How to use the MFSK_Receiver class:
1856
1857 1. create an object like:
1858
1859 #include "mfsk.h"
1860
1861 MFSK_Receiver<float> Receiver;
1862
1863 2. Set the parameters, for example:
1864
1865 Receiver.Tones = 32; // number of tones (symbols)
1866 Receiver.Bandwidth = 1000; // bandwidth [Hz]
1867 Receiver.SyncMargin = 8; // synchronizer tune margin [tone freq. spacing]
1868 Receiver.SyncIntegLen = 4; // synchronizer integration period [FEC blocks]
1869 Receiver.SyncThreshold = 3.2; // S/N threshold for printing
1870 Receiver.SampleRate = 8000.0; // internal processor sampling rate [Hz]
1871 Receiver.InputSampleRate = 8000.0; // soundcard sampling rate [Hz]
1872
1873 You don't need to set all the parameters, as upon creation
1874 of the Receiver object they are already given certain default
1875 values.
1876
1877 If you changed parameters at one time and want later to go back
1878 to the default values you can call: Receiver.Default();
1879
1880 3. Preset the Receiver internal arrays for the parameters you just set:
1881
1882 if (Receiver.Preset()<0)
1883 printf("Not enough RAM or another problem\n");
1884
1885 Each time you change the parameters you need to call Preset()
1886 in order to resize the internal arrays. Preset() will as well
1887 destroy all data being in the process of decoding, if you need
1888 this data then call first Receiver.Flush()
1889
1890 4. Read back the parameters you set in point 1., they could have been adjusted
1891 by Preset() to their closest allowed values.
1892
1893 5. Feed the audio into the Receiver:
1894
1895 Receiver.Process(AudioBuffer, BufferLength);
1896
1897 AudioBuffer can be an array of int16_t (16-bit signed integers)
1898 that you fill with the data from the soundcard. I suggest you feed
1899 the receiver with batches of 512 or 1024 samples, but in can be any number
1900 of samples at a time.
1901
1902 6. Call GetChar(Char) to get decoded characters. Note, that
1903 characters come in batches, and so, you need to call GetChar()
1904 possibly several times. GetChar() returns 0 when the decoder FIFO
1905 is empty or 1 when not empty. In the latter case the argument
1906 contains the character read form the FIFO. The loop could be like:
1907
1908 for ( ; ; )
1909 { uint8_t Char;
1910 if (Receiver.GetChar(Char)==0) break;
1911 printf("%c",Char);
1912 }
1913
1914 Keep in mind that you may see (random) control code characters here,
1915 and thus you must be able to deal with them. I suggest to process
1916 only carriage return (code=13) and Backspace (code=8). NUL (code=0)
1917 is the idle character: it is being sent when there is no text to be sent.
1918
1919 7. At any time you can read the signal-to-noise ratio of the
1920 incoming signal by calling Receiver.SignalToNoiseRatio() or frequency offset
1921 by calling Receiver.FrequencyOffset()
1922
1923 8. When the user decides to turn off the receiver and switch over
1924 to transmitt you may still call Receiver.Flush()in order to flush
1925 the data still being buffered in the decoder pipeplines.
1926
1927 */
1928
1929 template <class Type=float>
1930 class MFSK_Receiver
1931 {
1932 public:
1933 // primary parameters: set by the user
1934
1935 bool bContestia;
1936 size_t Tones; // number of tones: 4, 8, 16, 32, 64, 128, 256
1937 size_t Bandwidth; // bandwidth: 125, 250, 500, 1000, 2000
1938 size_t SyncMargin; // synchronizer search margin, frequency-wide
1939 size_t SyncIntegLen; // synchronizer integration period
1940 Type SyncThreshold; // synchronizer S/N threshold
1941 Type SampleRate; // audio sampling rate (internal processing)
1942 Type InputSampleRate; // true sampling rate of the soundcard
1943 float FirstCarrierMultiplier;
1944 int Reverse;
1945
1946 // secondary parameters: calculated by Preset()
1947 size_t BitsPerSymbol; // number of bits per symbol
1948 size_t SymbolsPerBlock; // number of symbols per one FEC code block
1949
1950 private:
1951 RateConverter<Type> Converter;
1952 Seq<Type> InputBuffer;
1953 MFSK_InputProcessor<Type> InputProcessor; // equalizes the input spectrum
1954 // and removes coherent interferences
1955 MFSK_Demodulator<Type> Demodulator; // FFT demodulator
1956
1957 const static size_t SlicesPerSymbol =
1958 MFSK_Demodulator<Type>::SpectraPerSymbol;
1959
1960 // number of possible frequency offsets
1961 size_t FreqOffsets;
1962 // number of possible time-phases within the FEC block
1963 size_t BlockPhases;
1964 // reference decoder
1965 MFSK_SoftDecoder<Type,Type> RefDecoder;
1966 // array of decoders
1967 MFSK_SoftDecoder<Type,Type> *Decoder;
1968 // current running time-phase
1969 size_t BlockPhase;
1970 // FEC noise integrators
1971 CircularBuffer< LowPass3_Filter<Type> > SyncNoiseEnergy;
1972 // FEC signal integrators
1973 CircularBuffer< LowPass3_Filter<Type> > SyncSignal;
1974 // weight for the integrators
1975 Type SyncFilterWeight;
1976 // best signal
1977 Type SyncBestSignal;
1978 // time-phase of the best signal
1979 size_t SyncBestBlockPhase;
1980 // frequency offset of the best signal
1981 size_t SyncBestFreqOffset;
1982 // S/N corresponding to the SyncBestSignal
1983 Type SyncSNR;
1984 // pipeline for decoded FEC blocks
1985 CircularBuffer<uint64_t> *DecodePipe;
1986 // buffer for decoded characters
1987 FIFO<uint8_t> Output;
1988
1989 public:
MFSK_Receiver()1990 MFSK_Receiver() {
1991 bContestia = false;
1992 Init();
1993 Default();
1994 }
~MFSK_Receiver()1995 ~MFSK_Receiver() {
1996 Free();
1997 }
Init(void)1998 void Init(void) {
1999 Decoder = 0;
2000 DecodePipe = 0;
2001 }
Free(void)2002 void Free(void) {
2003 if (Decoder) {
2004 size_t Idx;
2005 for (Idx = 0; Idx < (SlicesPerSymbol * FreqOffsets); Idx++)
2006 Decoder[Idx].Free();
2007 delete [] Decoder; Decoder=0;
2008 }
2009 if (DecodePipe) {
2010 size_t Idx;
2011 for (Idx = 0; Idx < BlockPhases; Idx++)
2012 DecodePipe[Idx].Free();
2013 delete [] DecodePipe;
2014 DecodePipe = 0;
2015 }
2016 Converter.Free();
2017 InputBuffer.Free();
2018 InputProcessor.Free();
2019 Demodulator.Free();
2020 RefDecoder.Free();
2021 SyncSignal.Free();
2022 SyncNoiseEnergy.Free();
2023 Output.Free();
2024 }
2025
2026 // set defaults values for all parameters
Default(void)2027 void Default(void) {
2028 Tones = 32;
2029 Reverse = 0;
2030 Bandwidth = 1000;
2031 SyncMargin = 8;
2032 SyncIntegLen = 4;
2033 SyncThreshold = 3.0;
2034 SampleRate = 8000;
2035 InputSampleRate = 8000.0;
2036 }
2037
2038 // resize internal arrays according the parameters
Preset(void)2039 int Preset(void) {
2040 size_t Idx;
2041 if (Tones < 2) Tones = 2;
2042 else if (Tones > 256) Tones = 256;
2043
2044 if (Bandwidth < 125) Bandwidth = 125;
2045 else if (Bandwidth > 2000) Bandwidth = 2000;
2046
2047 if (SyncMargin < 2) SyncMargin = 2;
2048
2049 if (SyncIntegLen < 2) SyncIntegLen = 2;
2050
2051 if (SyncThreshold < 3.0) SyncThreshold = 3.0;
2052
2053 BitsPerSymbol = Log2(Tones);
2054 Tones = Exp2(BitsPerSymbol);
2055
2056 Converter.OutputRate = SampleRate/InputSampleRate;
2057 if (Converter.Preset() < 0) goto Error;
2058
2059 Demodulator.BitsPerSymbol = BitsPerSymbol;
2060 Demodulator.SymbolLen = Exp2(BitsPerSymbol + 7 - Log2(Bandwidth/125));
2061 Bandwidth = Exp2(Log2(Bandwidth/125)) * 125;
2062 Demodulator.FirstCarrier =
2063 (size_t) ((Demodulator.SymbolLen/16) * FirstCarrierMultiplier) + 1;
2064 Demodulator.Reverse = Reverse;
2065 Demodulator.DecodeMargin = SyncMargin;
2066 if (Demodulator.Preset() < 0) goto Error;
2067 SyncMargin = Demodulator.DecodeMargin;
2068
2069 InputProcessor.WindowLen = 16 * Demodulator.SymbolLen;
2070 if (InputProcessor.Preset() < 0) goto Error;
2071
2072 RefDecoder.Default();
2073 RefDecoder.BitsPerSymbol=BitsPerSymbol;
2074 RefDecoder.bContestia = bContestia;
2075 if (RefDecoder.Preset()<0) goto Error;
2076 SymbolsPerBlock=RefDecoder.SymbolsPerBlock;
2077
2078 if (Decoder) {
2079 for (Idx = 0; Idx < (SlicesPerSymbol * FreqOffsets); Idx++)
2080 Decoder[Idx].Free();
2081 }
2082
2083 if (DecodePipe) {
2084 for (Idx = 0; Idx < BlockPhases; Idx++)
2085 DecodePipe[Idx].Free();
2086 }
2087
2088 FreqOffsets = 2 * SyncMargin + 1;
2089 BlockPhases = SlicesPerSymbol * SymbolsPerBlock;
2090
2091 if (ReallocArray(&Decoder, SlicesPerSymbol * FreqOffsets) < 0)
2092 goto Error;
2093
2094 for (Idx = 0; Idx < (SlicesPerSymbol * FreqOffsets); Idx++) {
2095 Decoder[Idx].bContestia = bContestia;
2096 Decoder[Idx].Init();
2097 }
2098
2099 for (Idx = 0; Idx < (SlicesPerSymbol * FreqOffsets); Idx++)
2100 if (Decoder[Idx].Preset(RefDecoder) < 0) goto Error;
2101
2102 if (ReallocArray(&DecodePipe,BlockPhases) < 0) goto Error;
2103 for (Idx = 0; Idx < BlockPhases; Idx++)
2104 DecodePipe[Idx].Init();
2105 for (Idx = 0; Idx < BlockPhases; Idx++) {
2106 DecodePipe[Idx].Width = FreqOffsets;
2107 DecodePipe[Idx].Len = SyncIntegLen;
2108 if (DecodePipe[Idx].Preset() < 0) goto Error;
2109 DecodePipe[Idx].Clear();
2110 }
2111
2112 SyncSignal.Width = FreqOffsets;
2113 SyncSignal.Len = BlockPhases;
2114 if (SyncSignal.Preset() < 0) goto Error;
2115 SyncSignal.Clear();
2116
2117 SyncNoiseEnergy.Width = FreqOffsets;
2118 SyncNoiseEnergy.Len = BlockPhases;
2119 if (SyncNoiseEnergy.Preset() < 0) goto Error;
2120 SyncNoiseEnergy.Clear();
2121
2122 SyncFilterWeight = 1.0 / SyncIntegLen;
2123
2124 BlockPhase=0;
2125
2126 SyncBestSignal = 0;
2127 SyncBestBlockPhase = 0;
2128 SyncBestFreqOffset = 0;
2129 SyncSNR = 0;
2130
2131 if (InputBuffer.EnsureSpace(InputProcessor.WindowLen + 2048) < 0)
2132 goto Error;
2133
2134 Output.Len = 1024;
2135 if (Output.Preset() < 0) goto Error;
2136
2137 return 0;
2138
2139 Error:
2140 Free();
2141 return -1;
2142 }
2143
Reset(void)2144 void Reset(void) {
2145 size_t Idx;
2146 Converter.Reset();
2147 InputBuffer.Clear();
2148 InputProcessor.Reset();
2149 for (Idx = 0; Idx < (SlicesPerSymbol * FreqOffsets); Idx++)
2150 Decoder[Idx].Reset();
2151
2152 for (Idx = 0; Idx < BlockPhases; Idx++)
2153 DecodePipe[Idx].Clear();
2154
2155 SyncSignal.Clear();
2156 SyncNoiseEnergy.Clear();
2157
2158 BlockPhase = 0;
2159
2160 SyncBestSignal = 0;
2161 SyncBestBlockPhase = 0;
2162 SyncBestFreqOffset = 0;
2163 SyncSNR = 0;
2164
2165 Output.Reset();
2166 }
2167
PrintParameters(void)2168 char *PrintParameters(void) {
2169 static char szParams[1000];
2170 Type FreqBin = SampleRate/Demodulator.SymbolLen;
2171 snprintf(szParams, sizeof(szParams), "\
2172 %d tones, %4.2f Hz/tone, %4.1f Hz bandwidth\n\
2173 %d bits/symbol(tone), %4.2f baud\n\
2174 FEC block: %d char. => %d x %d bits, %3.1f sec\n\
2175 Audio band: %3.1f - %3.1f Hz, %3.1f Hz total\n\
2176 Tuning tolerance = +/- %3.1f Hz\n\
2177 Sync. S/N threshold = %3.1f\n",
2178 (int)Demodulator.Carriers,
2179 FreqBin*Demodulator.CarrierSepar,
2180 FreqBin*Demodulator.CarrierSepar*Demodulator.Carriers,
2181 (int)Demodulator.BitsPerSymbol,
2182 SampleRate/Demodulator.SymbolSepar,
2183 (int)RefDecoder.BitsPerSymbol,
2184 (int)RefDecoder.BitsPerSymbol,
2185 (int)RefDecoder.SymbolsPerBlock,
2186 (SymbolsPerBlock*Demodulator.SymbolSepar)/SampleRate,
2187 FreqBin * Demodulator.FirstCarrier,
2188 FreqBin * (Demodulator.FirstCarrier + Demodulator.CarrierSepar * Demodulator.Carriers),
2189 FreqBin * Demodulator.CarrierSepar * Demodulator.Carriers,
2190 FreqBin * SyncMargin,
2191 SyncThreshold );
2192 return szParams;
2193 }
2194
BaudRate(void)2195 Type BaudRate(void) {
2196 return SampleRate / Demodulator.SymbolSepar;
2197 }
2198
TuneMargin(void)2199 Type TuneMargin(void) {
2200 return SyncMargin * (SampleRate / Demodulator.SymbolLen);
2201 }
2202
BlockPeriod(void)2203 Type BlockPeriod(void) {
2204 return (SymbolsPerBlock * Demodulator.SymbolSepar) / SampleRate;
2205 }
2206
CharactersPerSecond(void)2207 Type CharactersPerSecond(void) {
2208 return BitsPerSymbol * SampleRate /
2209 (SymbolsPerBlock*Demodulator.SymbolSepar);
2210 }
2211
SignalToNoiseRatio(void)2212 Type SignalToNoiseRatio(void) {
2213 return SyncSNR;
2214 }
2215
FrequencyOffset(void)2216 Type FrequencyOffset(void) {
2217 return ( (int)SyncBestFreqOffset -
2218 (int)FreqOffsets / 2) * (SampleRate / Demodulator.SymbolLen);
2219 }
2220
TimeOffset(void)2221 Type TimeOffset(void) {
2222 return ( (Type)SyncBestBlockPhase / SlicesPerSymbol) *
2223 (Demodulator.SymbolSepar / SampleRate);
2224 }
2225
2226 // process an audio batch: first the input processor, then the demodulator
2227 template <class InpType>
Process(InpType * Input,size_t InputLen)2228 int Process(InpType *Input, size_t InputLen) {
2229 if (Converter.Process(Input, InputLen, InputBuffer) < 0)
2230 return -1;
2231 ProcessInputBuffer();
2232 return 0;
2233 }
2234
Flush(void)2235 void Flush(void) {
2236 ProcessInputBuffer();
2237 size_t Idx;
2238
2239 for (Idx = InputBuffer.Len; Idx < InputProcessor.WindowLen; Idx++)
2240 InputBuffer[Idx] = 0;
2241 InputBuffer.Len = InputProcessor.WindowLen;
2242 ProcessInputBuffer();
2243
2244 for (Idx = 0; Idx < InputProcessor.WindowLen; Idx++)
2245 InputBuffer[Idx] = 0;
2246 size_t FlushLen = Demodulator.SymbolSepar * SymbolsPerBlock *
2247 SyncIntegLen * 2;
2248 for (Idx=0; Idx<FlushLen; Idx+=InputProcessor.WindowLen) {
2249 InputBuffer.Len = InputProcessor.WindowLen;
2250 ProcessInputBuffer();
2251 }
2252 }
2253
2254 // get one character from the output buffer
GetChar(uint8_t & Char)2255 int GetChar(uint8_t &Char) {
2256 return Output.Read(Char);
2257 }
2258
2259 private:
2260 // process the input buffer: first the input processor, then the demodulator
ProcessInputBuffer(void)2261 void ProcessInputBuffer(void) {
2262 while(InputBuffer.Len >= InputProcessor.WindowLen) {
2263 InputProcessor.Process(InputBuffer.Elem);
2264 InputBuffer.Delete(0, InputProcessor.WindowLen);
2265 size_t Idx;
2266 for (Idx = 0;
2267 Idx < InputProcessor.WindowLen;
2268 Idx += Demodulator.SymbolSepar )
2269 ProcessSymbol(InputProcessor.Output+Idx);
2270 }
2271 }
2272
2273 // process (through the demodulator) an audio batch corresponding to one symbol
2274 // demodulator always uses audio batches corresponding to one symbol period
2275 template <class InpType>
ProcessSymbol(InpType * Input)2276 void ProcessSymbol(InpType *Input) {
2277 Demodulator.Process(Input);
2278 MFSK_SoftDecoder<Type,Type> *DecoderPtr = Decoder;
2279
2280 size_t Offset,Slice;
2281 for (Slice = 0; Slice < SlicesPerSymbol; Slice++) {
2282 LowPass3_Filter<Type>
2283 *NoiseEnergyPtr = SyncNoiseEnergy.AbsPtr(BlockPhase);
2284 LowPass3_Filter<Type>
2285 *SignalPtr = SyncSignal.AbsPtr(BlockPhase);
2286
2287 uint64_t *DecodeBlockPtr = DecodePipe[BlockPhase].CurrPtr();
2288
2289 size_t BestSliceOffset = 0;
2290 Type BestSliceSignal = 0;
2291 Type NoiseEnergy = 0;
2292 Type Signal;
2293 Type Symbol[8];
2294
2295 for (Offset = 0; Offset < FreqOffsets; Offset++) {
2296 Demodulator.SoftDecode(Symbol, Slice, (int)Offset - (FreqOffsets / 2));
2297
2298 DecoderPtr->Input(Symbol);
2299 DecoderPtr->Process();
2300 DecoderPtr->Output(DecodeBlockPtr);
2301
2302 NoiseEnergy = DecoderPtr->NoiseEnergy;
2303 NoiseEnergyPtr->Process(NoiseEnergy, SyncFilterWeight);
2304
2305 Signal = DecoderPtr->Signal;
2306 SignalPtr->Process(Signal, SyncFilterWeight);
2307 Signal = SignalPtr->Output;
2308
2309 if (Signal > BestSliceSignal) {
2310 BestSliceSignal = Signal;
2311 BestSliceOffset = Offset;
2312 }
2313
2314 DecoderPtr++;
2315 DecodeBlockPtr++;
2316
2317 NoiseEnergyPtr++;
2318 SignalPtr++;
2319 }
2320 DecodePipe[BlockPhase] += 1;
2321
2322 if (BlockPhase == SyncBestBlockPhase) {
2323 SyncBestSignal = BestSliceSignal;
2324 SyncBestFreqOffset = BestSliceOffset;
2325 } else {
2326 if (BestSliceSignal > SyncBestSignal) {
2327 SyncBestSignal = BestSliceSignal;
2328 SyncBestBlockPhase = BlockPhase;
2329 SyncBestFreqOffset = BestSliceOffset;
2330 }
2331 }
2332
2333 int Dist = (int)BlockPhase - (int)SyncBestBlockPhase;
2334 if (Dist < 0) Dist += BlockPhases;
2335
2336 if (Dist == (int)(BlockPhases / 2)) {
2337 Type BestNoise = sqrt( NoiseEnergyPtr->Output);
2338 if (BestNoise == 0) SyncSNR = 0;
2339 else SyncSNR = SyncBestSignal / BestNoise;
2340 // printf(
2341 // "%d, %6.3f/%6.3f = %5.2f\n",
2342 // SyncBestFreqOffset,
2343 // SyncBestSignal,
2344 // BestNoise,
2345 // SyncSNR );
2346 if (SyncSNR >= SyncThreshold) {
2347 uint64_t *BestBlockPtr =
2348 DecodePipe[SyncBestBlockPhase].CurrPtr();
2349 uint64_t Block = BestBlockPtr[SyncBestFreqOffset];
2350
2351 for ( size_t Byte = 0; Byte < BitsPerSymbol; Byte++) {
2352 uint8_t Char = Block&0xFF;
2353 Output.Write(Char);
2354 Block >>= 8;
2355 }
2356 }
2357 if (SyncSNR > 100) SyncSNR = 0.0;
2358 }
2359
2360 BlockPhase++;
2361 }
2362 if (BlockPhase >= BlockPhases) BlockPhase -= BlockPhases;
2363 }
2364
2365 };
2366
2367 #endif
2368