1 /*---------------------------------------------------------------------------*\
2
3 FILE........: fsk.c
4 AUTHOR......: Brady O'Brien & David Rowe
5 DATE CREATED: 7 January 2016
6
7 C Implementation of 2/4FSK modulator/demodulator, based on octave/fsk_lib.m
8
9 \*---------------------------------------------------------------------------*/
10
11 /*
12 Copyright (C) 2016-2020 David Rowe
13
14 All rights reserved.
15
16 This program is free software; you can redistribute it and/or modify
17 it under the terms of the GNU Lesser General Public License version 2.1, as
18 published by the Free Software Foundation. This program is
19 distributed in the hope that it will be useful, but WITHOUT ANY
20 WARRANTY; without even the implied warranty of MERCHANTABILITY or
21 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
22 License for more details.
23
24 You should have received a copy of the GNU Lesser General Public License
25 along with this program; if not, see <http://www.gnu.org/licenses/>.
26 */
27
28 /*---------------------------------------------------------------------------*\
29
30 DEFINES
31
32 \*---------------------------------------------------------------------------*/
33
34 /* Define this to enable EbNodB estimate */
35 /* This needs square roots, may take more cpu time than it's worth */
36 #define EST_EBNO
37
38 /* This is a flag for the freq. estimator to use a precomputed/rt computed hann window table
39 On platforms with slow cosf, this will produce a substantial speedup at the cost of a small
40 amount of memory
41 */
42 #define USE_HANN_TABLE
43
44 /* This flag turns on run-time hann table generation. If USE_HANN_TABLE is unset,
45 this flag has no effect. If USE_HANN_TABLE is set and this flag is set, the
46 hann table will be allocated and generated when fsk_init or fsk_init_hbr is
47 called. If this flag is not set, a hann function table of size fsk->Ndft MUST
48 be provided. On small platforms, this can be used with a precomputed table to
49 save memory at the cost of flash space.
50 */
51 #define GENERATE_HANN_TABLE_RUNTIME
52
53 /* Turn off table generation if on cortex M4 to save memory */
54 #ifdef CORTEX_M4
55 #undef USE_HANN_TABLE
56 #endif
57
58 /*---------------------------------------------------------------------------*\
59
60 INCLUDES
61
62 \*---------------------------------------------------------------------------*/
63
64 #include <assert.h>
65 #include <stdlib.h>
66 #include <stdint.h>
67 #include <math.h>
68
69 #include "fsk.h"
70 #include "comp_prim.h"
71 #include "kiss_fftr.h"
72 #include "modem_probe.h"
73
74 /*---------------------------------------------------------------------------*\
75
76 FUNCTIONS
77
78 \*---------------------------------------------------------------------------*/
79
80 static void stats_init(struct FSK *fsk);
81
82 #ifdef USE_HANN_TABLE
83 /*
84 This is used by fsk_create and fsk_create_hbr to generate a hann function
85 table
86 */
fsk_generate_hann_table(struct FSK * fsk)87 static void fsk_generate_hann_table(struct FSK* fsk){
88 int Ndft = fsk->Ndft;
89 size_t i;
90
91 for(i=0; i<Ndft; i++){
92 fsk->hann_table[i] = 0.5 - 0.5 * cosf(2.0 * M_PI * (float)i / (float) (Ndft-1));
93 }
94 }
95 #endif
96
97 /*---------------------------------------------------------------------------*\
98
99 FUNCTION....: fsk_create_core
100 AUTHOR......: Brady O'Brien
101 DATE CREATED: 7 January 2016
102
103 In this version of the demod the standard/hbr modes have been
104 largely combined at they shared so much common code. The
105 fsk_create/fsk_create_hbr function interface has been retained to
106 maximise compatability with existing applications.
107
108 \*---------------------------------------------------------------------------*/
109
fsk_create_core(int Fs,int Rs,int M,int P,int Nsym,int f1_tx,int tone_spacing)110 struct FSK * fsk_create_core(int Fs, int Rs, int M, int P, int Nsym, int f1_tx, int tone_spacing)
111 {
112 struct FSK *fsk;
113 int i;
114
115 /* Check configuration validity */
116 assert(Fs > 0);
117 assert(Rs > 0);
118 assert(P > 0);
119 assert(Nsym > 0);
120 /* Ts (Fs/Rs) must be an integer */
121 assert( (Fs%Rs) == 0 );
122 /* Ts/P (Fs/Rs/P) must be an integer */
123 assert( ((Fs/Rs)%P) == 0 );
124 /* If P is too low we don't have a good choice of timing offsets to choose from */
125 assert( P >= 4 );
126 assert( M==2 || M==4);
127
128 fsk = (struct FSK*) calloc(1, sizeof(struct FSK)); assert(fsk != NULL);
129
130 // Need enough bins to within 10% of tone centre
131 float bin_width_Hz = 0.1*Rs;
132 float Ndft = (float)Fs/bin_width_Hz;
133 Ndft = pow(2.0, ceil(log2(Ndft)));
134
135 /* Set constant config parameters */
136 fsk->Fs = Fs;
137 fsk->Rs = Rs;
138 fsk->Ts = Fs/Rs;
139 fsk->burst_mode = 0;
140 fsk->P = P;
141 fsk->Nsym = Nsym;
142 fsk->N = fsk->Ts*fsk->Nsym;
143 fsk->Ndft = Ndft;
144 fsk->tc = 0.1;
145 fsk->Nmem = fsk->N+(2*fsk->Ts);
146 fsk->f1_tx = f1_tx;
147 fsk->tone_spacing = tone_spacing;
148 fsk->nin = fsk->N;
149 fsk->lock_nin = 0;
150 fsk->mode = M==2 ? MODE_2FSK : MODE_4FSK;
151 fsk->Nbits = M==2 ? fsk->Nsym : fsk->Nsym*2;
152 fsk->est_min = 0;
153 fsk->est_max = Fs;
154 fsk->est_space = 0.75*Rs;
155 fsk->freq_est_type = 0;
156
157 //printf("C.....: M: %d Fs: %d Rs: %d Ts: %d nsym: %d nbit: %d N: %d Ndft: %d fmin: %d fmax: %d\n",
158 // M, fsk->Fs, fsk->Rs, fsk->Ts, fsk->Nsym, fsk->Nbits, fsk->N, fsk->Ndft, fsk->est_min, fsk->est_max);
159 /* Set up rx state */
160 for(i=0; i<M; i++)
161 fsk->phi_c[i] = comp_exp_j(0);
162 fsk->f_dc = (COMP*)malloc(M*fsk->Nmem*sizeof(COMP)); assert(fsk->f_dc != NULL);
163 for(i=0; i<M*fsk->Nmem; i++)
164 fsk->f_dc[i] = comp0();
165
166 fsk->fft_cfg = kiss_fft_alloc(Ndft,0,NULL,NULL); assert(fsk->fft_cfg != NULL);
167 fsk->Sf = (float*)malloc(sizeof(float)*fsk->Ndft); assert(fsk->Sf != NULL);
168 for(i=0;i<Ndft;i++)fsk->Sf[i] = 0;
169
170 #ifdef USE_HANN_TABLE
171 #ifdef GENERATE_HANN_TABLE_RUNTIME
172 fsk->hann_table = (float*)malloc(sizeof(float)*fsk->Ndft); assert(fsk->hann_table != NULL);
173 fsk_generate_hann_table(fsk);
174 #else
175 fsk->hann_table = NULL;
176 #endif
177 #endif
178
179
180 fsk->norm_rx_timing = 0;
181
182 /* Set up tx state */
183 fsk->tx_phase_c = comp_exp_j(0);
184
185 /* Set up demod stats */
186 fsk->EbNodB = 0;
187
188 for( i=0; i<M; i++)
189 fsk->f_est[i] = 0;
190
191 fsk->ppm = 0;
192
193 fsk->stats = (struct MODEM_STATS*)malloc(sizeof(struct MODEM_STATS)); assert(fsk->stats != NULL);
194 stats_init(fsk);
195 fsk->normalise_eye = 1;
196
197 return fsk;
198 }
199
200 /*---------------------------------------------------------------------------* \
201
202 FUNCTION....: fsk_create
203 AUTHOR......: Brady O'Brien
204 DATE CREATED: 7 January 2016
205
206 Create and initialize an instance of the FSK modem. Returns a pointer
207 to the modem state/config struct. One modem config struct may be used
208 for both mod and demod.
209
210 If you are not intending to use the modulation functions, you can
211 set f1_tx to FSK_NONE.
212
213 \*---------------------------------------------------------------------------*/
214
fsk_create(int Fs,int Rs,int M,int tx_f1,int tx_fs)215 struct FSK * fsk_create(int Fs, int Rs, int M, int tx_f1, int tx_fs) {
216 return fsk_create_core(Fs, Rs, M, FSK_DEFAULT_P, FSK_DEFAULT_NSYM, tx_f1, tx_fs);
217 }
218
219 /*---------------------------------------------------------------------------*\
220
221 FUNCTION....: fsk_create_hbr
222 AUTHOR......: Brady O'Brien
223 DATE CREATED: 11 February 2016
224
225 Alternate version of create allows user defined oversampling P and
226 averaging window Nsym. In the current version of the demod it's
227 simply an alias for the default core function.
228
229 P is the oversampling rate of the internal demod processing, which
230 happens at Rs*P Hz. We filter the tones at P different timing
231 offsets, and choose the best one. P should be >=8, so we have a
232 choice of at least 8 timing offsets. This may require some
233 adjustment of Fs and Rs, as Fs/Rs/P must be an integer.
234
235 Nsym is the number of symbols we average demod parameters like
236 symbol timing over.
237
238 \*---------------------------------------------------------------------------*/
239
fsk_create_hbr(int Fs,int Rs,int M,int P,int Nsym,int f1_tx,int tone_spacing)240 struct FSK * fsk_create_hbr(int Fs, int Rs, int M, int P, int Nsym, int f1_tx, int tone_spacing) {
241 return fsk_create_core(Fs, Rs, M, P, Nsym, f1_tx, tone_spacing);
242 }
243
244 /*---------------------------------------------------------------------------*\
245
246 FUNCTION....: fsk_destroy
247 AUTHOR......: Brady O'Brien
248 DATE CREATED: 11 February 2016
249
250 Call this to free all memory and shut down the modem.
251
252 \*---------------------------------------------------------------------------*/
253
fsk_destroy(struct FSK * fsk)254 void fsk_destroy(struct FSK *fsk){
255 free(fsk->Sf);
256 free(fsk->f_dc);
257 free(fsk->fft_cfg);
258 free(fsk->stats);
259 free(fsk->hann_table);
260 free(fsk);
261 }
262
263 /*---------------------------------------------------------------------------*\
264
265 FUNCTION....: fsk_mod
266 AUTHOR......: Brady O'Brien
267 DATE CREATED: 11 February 2016
268
269 FSK modulator function, real valued output samples with amplitude 2.
270
271 \*---------------------------------------------------------------------------*/
272
fsk_mod(struct FSK * fsk,float fsk_out[],uint8_t tx_bits[],int nbits)273 void fsk_mod(struct FSK *fsk,float fsk_out[], uint8_t tx_bits[], int nbits) {
274 COMP tx_phase_c = fsk->tx_phase_c; /* Current complex TX phase */
275 int f1_tx = fsk->f1_tx; /* '0' frequency */
276 int tone_spacing = fsk->tone_spacing; /* space between frequencies */
277 int Ts = fsk->Ts; /* samples-per-symbol */
278 int Fs = fsk->Fs; /* sample freq */
279 int M = fsk->mode;
280 COMP dosc_f[M]; /* phase shift per sample */
281 COMP dph; /* phase shift of current bit */
282 size_t i,j,m,bit_i,sym;
283
284 /* trap these parametrs being set to FSK_UNUSED, then calling mod */
285 assert(f1_tx > 0);
286 assert(tone_spacing > 0);
287
288 /* Init the per sample phase shift complex numbers */
289 for( m=0; m<M; m++){
290 dosc_f[m] = comp_exp_j(2*M_PI*((float)(f1_tx+(tone_spacing*m))/(float)(Fs)));
291 }
292
293 bit_i = 0;
294 int nsym = nbits/(M>>1);
295 for(i=0; i<nsym; i++) {
296 sym = 0;
297 /* Pack the symbol number from the bit stream */
298 for( m=M; m>>=1; ){
299 uint8_t bit = tx_bits[bit_i];
300 bit = (bit==1)?1:0;
301 sym = (sym<<1)|bit;
302 bit_i++;
303 }
304 /* Look up symbol phase shift */
305 dph = dosc_f[sym];
306 /* Spin the oscillator for a symbol period */
307 for(j=0; j<Ts; j++){
308 tx_phase_c = cmult(tx_phase_c,dph);
309 fsk_out[i*Ts+j] = 2*tx_phase_c.real;
310 }
311
312 }
313
314 /* Normalize TX phase to prevent drift */
315 tx_phase_c = comp_normalize(tx_phase_c);
316
317 /* save TX phase */
318 fsk->tx_phase_c = tx_phase_c;
319
320 }
321
322 /*---------------------------------------------------------------------------*\
323
324 FUNCTION....: fsk_mod_c
325 AUTHOR......: Brady O'Brien
326 DATE CREATED: 11 February 2016
327
328 FSK modulator function, complex valued output samples with magnitude 1.
329
330 \*---------------------------------------------------------------------------*/
331
fsk_mod_c(struct FSK * fsk,COMP fsk_out[],uint8_t tx_bits[],int nbits)332 void fsk_mod_c(struct FSK *fsk,COMP fsk_out[], uint8_t tx_bits[], int nbits) {
333 COMP tx_phase_c = fsk->tx_phase_c; /* Current complex TX phase */
334 int f1_tx = fsk->f1_tx; /* '0' frequency */
335 int tone_spacing = fsk->tone_spacing; /* space between frequencies */
336 int Ts = fsk->Ts; /* samples-per-symbol */
337 int Fs = fsk->Fs; /* sample freq */
338 int M = fsk->mode;
339 COMP dosc_f[M]; /* phase shift per sample */
340 COMP dph; /* phase shift of current bit */
341 size_t i,j,bit_i,sym;
342 int m;
343
344 /* trap these parametrs being set to FSK_UNUSED, then calling mod */
345 assert(f1_tx > 0);
346 assert(tone_spacing > 0);
347
348 /* Init the per sample phase shift complex numbers */
349 for( m=0; m<M; m++){
350 dosc_f[m] = comp_exp_j(2*M_PI*((float)(f1_tx+(tone_spacing*m))/(float)(Fs)));
351 }
352
353 bit_i = 0;
354 int nsym = nbits/(M>>1);
355 for(i=0; i<nsym; i++) {
356 sym = 0;
357 /* Pack the symbol number from the bit stream */
358 for( m=M; m>>=1; ){
359 uint8_t bit = tx_bits[bit_i];
360 bit = (bit==1)?1:0;
361 sym = (sym<<1)|bit;
362 bit_i++;
363 }
364 /* Look up symbol phase shift */
365 dph = dosc_f[sym];
366 /* Spin the oscillator for a symbol period */
367 for(j=0; j<Ts; j++){
368 tx_phase_c = cmult(tx_phase_c,dph);
369 fsk_out[i*Ts+j] = tx_phase_c;
370 }
371 }
372
373 /* Normalize TX phase to prevent drift */
374 tx_phase_c = comp_normalize(tx_phase_c);
375
376 /* save TX phase */
377 fsk->tx_phase_c = tx_phase_c;
378
379 }
380
381
382 /*---------------------------------------------------------------------------*\
383
384 FUNCTION....: fsk_mod_ext_vco
385 AUTHOR......: David Rowe
386 DATE CREATED: February 2018
387
388 Modulator that assume an external VCO. The output is a voltage
389 that changes for each symbol.
390
391 \*---------------------------------------------------------------------------*/
392
fsk_mod_ext_vco(struct FSK * fsk,float vco_out[],uint8_t tx_bits[],int nbits)393 void fsk_mod_ext_vco(struct FSK *fsk, float vco_out[], uint8_t tx_bits[], int nbits) {
394 int f1_tx = fsk->f1_tx; /* '0' frequency */
395 int tone_spacing = fsk->tone_spacing; /* space between frequencies */
396 int Ts = fsk->Ts; /* samples-per-symbol */
397 int M = fsk->mode;
398 int i, j, m, sym, bit_i;
399
400 /* trap these parametrs being set to FSK_UNUSED, then calling mod */
401 assert(f1_tx > 0);
402 assert(tone_spacing > 0);
403
404 bit_i = 0;
405 int nsym = nbits/(M>>1);
406 for(i=0; i<nsym; i++) {
407 /* generate the symbol number from the bit stream,
408 e.g. 0,1 for 2FSK, 0,1,2,3 for 4FSK */
409
410 sym = 0;
411
412 /* unpack the symbol number from the bit stream */
413
414 for( m=M; m>>=1; ){
415 uint8_t bit = tx_bits[bit_i];
416 bit = (bit==1)?1:0;
417 sym = (sym<<1)|bit;
418 bit_i++;
419 }
420
421 /*
422 Map 'sym' to VCO frequency
423 Note: drive is inverted, a higher tone drives VCO voltage lower
424 */
425
426 //fprintf(stderr, "i: %d sym: %d freq: %f\n", i, sym, f1_tx + tone_spacing*(float)sym);
427 for(j=0; j<Ts; j++) {
428 vco_out[i*Ts+j] = f1_tx + tone_spacing*(float)sym;
429 }
430 }
431 }
432
433 /*---------------------------------------------------------------------------*\
434
435 FUNCTION....: fsk_nin
436 AUTHOR......: Brady O'Brien
437 DATE CREATED: 11 February 2016
438
439 Call me before each call to fsk_demod() to determine how many new
440 samples you should pass in. the number of samples will vary due to
441 timing variations.
442
443 \*---------------------------------------------------------------------------*/
444
fsk_nin(struct FSK * fsk)445 uint32_t fsk_nin(struct FSK *fsk){
446 return (uint32_t)fsk->nin;
447 }
448
449 /*
450 * Internal function to estimate the frequencies of the FSK tones.
451 * This is split off because it is fairly complicated, needs a bunch of memory, and probably
452 * takes more cycles than the rest of the demod.
453 * Parameters:
454 * fsk - FSK struct from demod containing FSK config
455 * fsk_in - block of samples in this demod cycles, must be nin long
456 * freqs - Array for the estimated frequencies
457 * M - number of frequency peaks to find
458 */
fsk_demod_freq_est(struct FSK * fsk,COMP fsk_in[],float * freqs,int M)459 void fsk_demod_freq_est(struct FSK *fsk, COMP fsk_in[], float *freqs, int M) {
460 int Ndft = fsk->Ndft;
461 int Fs = fsk->Fs;
462 int nin = fsk->nin;
463 size_t i,j;
464 float hann;
465 float max;
466 int imax;
467 kiss_fft_cfg fft_cfg = fsk->fft_cfg;
468 int freqi[M];
469 int st,en,f_zero;
470
471 kiss_fft_cpx *fftin = (kiss_fft_cpx*)malloc(sizeof(kiss_fft_cpx)*Ndft);
472 kiss_fft_cpx *fftout = (kiss_fft_cpx*)malloc(sizeof(kiss_fft_cpx)*Ndft);
473
474 st = (fsk->est_min*Ndft)/Fs + Ndft/2; if (st < 0) st = 0;
475 en = (fsk->est_max*Ndft)/Fs + Ndft/2; if (en > Ndft) en = Ndft;
476 //fprintf(stderr, "min: %d max: %d st: %d en: %d\n", fsk->est_min, fsk->est_max, st, en);
477
478 f_zero = (fsk->est_space*Ndft)/Fs;
479 //fprintf(stderr, "fsk->est_space: %d f_zero = %d\n", fsk->est_space, f_zero);
480
481 int numffts = floor((float)nin/(Ndft/2)) - 1;
482 for(j=0; j<numffts; j++){
483 int a = j*Ndft/2;
484 //fprintf(stderr, "numffts: %d j: %d a: %d\n", numffts, (int)j, a);
485 /* Copy FSK buffer into reals of FFT buffer and apply a hann window */
486 for(i=0; i<Ndft; i++){
487 #ifdef USE_HANN_TABLE
488 hann = fsk->hann_table[i];
489 #else
490 hann = 0.5 - 0.5 * cosf(2.0 * M_PI * (float)i / (float) (fft_samps-1));
491 #endif
492 fftin[i].r = hann*fsk_in[i+a].real;
493 fftin[i].i = hann*fsk_in[i+a].imag;
494 }
495
496 /* Do the FFT */
497 kiss_fft(fft_cfg,fftin,fftout);
498
499 /* FFT shift to put DC bin at Ndft/2 */
500 kiss_fft_cpx tmp;
501 for(i=0; i<Ndft/2; i++) {
502 tmp = fftout[i];
503 fftout[i] = fftout[i+Ndft/2];
504 fftout[i+Ndft/2] = tmp;
505 }
506
507 /* Find the magnitude^2 of each freq slot */
508 for(i=0; i<Ndft; i++) {
509 fftout[i].r = (fftout[i].r*fftout[i].r) + (fftout[i].i*fftout[i].i) ;
510 }
511
512 /* Mix back in with the previous fft block */
513 /* Copy new fft est into imag of fftout for frequency divination below */
514 float tc = fsk->tc;
515 for(i=0; i<Ndft; i++){
516 fsk->Sf[i] = (fsk->Sf[i]*(1-tc)) + (sqrtf(fftout[i].r)*tc);
517 fftout[i].i = fsk->Sf[i];
518 }
519 }
520
521 modem_probe_samp_f("t_Sf",fsk->Sf,Ndft);
522
523 max = 0;
524 /* Find the M frequency peaks here */
525 for(i=0; i<M; i++){
526 imax = 0;
527 max = 0;
528 for(j=st;j<en;j++){
529 if(fftout[j].i > max){
530 max = fftout[j].i;
531 imax = j;
532 }
533 }
534 /* Blank out FMax +/-Fspace/2 */
535 int f_min, f_max;
536 f_min = imax - f_zero;
537 f_min = f_min < 0 ? 0 : f_min;
538 f_max = imax + f_zero;
539 f_max = f_max > Ndft ? Ndft : f_max;
540 for(j=f_min; j<f_max; j++)
541 fftout[j].i = 0;
542
543 /* Stick the freq index on the list */
544 freqi[i] = imax - Ndft/2;
545 }
546
547 /* Gnome sort the freq list */
548 /* My favorite sort of sort*/
549 i = 1;
550 while(i<M){
551 if(freqi[i] >= freqi[i-1]) i++;
552 else{
553 j = freqi[i];
554 freqi[i] = freqi[i-1];
555 freqi[i-1] = j;
556 if(i>1) i--;
557 }
558 }
559
560 /* Convert freqs from indices to frequencies */
561 for(i=0; i<M; i++){
562 freqs[i] = (float)(freqi[i])*((float)Fs/(float)Ndft);
563 }
564
565 /* Search for each tone method 2 - correlate with mask with non-zero entries at tone spacings ----- */
566
567 /* construct mask */
568 float mask[Ndft];
569 for(i=0; i<Ndft; i++) mask[i] = 0.0;
570 for(i=0;i<3; i++) mask[i] = 1.0;
571 int bin=0;
572 for(int m=1; m<=M-1; m++) {
573 bin = round((float)m*fsk->tone_spacing*Ndft/Fs)-1;
574 for(i=bin; i<=bin+2; i++) mask[i] = 1.0;
575 }
576 int len_mask = bin+2+1;
577
578 #ifdef MODEMPROBE_ENABLE
579 modem_probe_samp_f("t_mask",mask,len_mask);
580 #endif
581
582 /* drag mask over Sf, looking for peak in correlation */
583 int b_max = st; float corr_max = 0.0;
584 float *Sf = fsk->Sf;
585 for (int b=st; b<en-len_mask; b++) {
586 float corr = 0.0;
587 for(i=0; i<len_mask; i++)
588 corr += mask[i] * Sf[b+i];
589 if (corr > corr_max) {
590 corr_max = corr;
591 b_max = b;
592 }
593 }
594 float foff = (b_max-Ndft/2)*Fs/Ndft;
595 //fprintf(stderr, "fsk->tone_spacing: %d\n",fsk->tone_spacing);
596 for (int m=0; m<M; m++)
597 fsk->f2_est[m] = foff + m*fsk->tone_spacing;
598
599 #ifdef MODEMPROBE_ENABLE
600 modem_probe_samp_f("t_f2_est",fsk->f2_est,M);
601 #endif
602
603 free(fftin);
604 free(fftout);
605 }
606
607 /* core demodulator function */
fsk_demod_core(struct FSK * fsk,uint8_t rx_bits[],float rx_filt[],COMP fsk_in[])608 void fsk_demod_core(struct FSK *fsk, uint8_t rx_bits[], float rx_filt[], COMP fsk_in[]){
609 int N = fsk->N;
610 int Ts = fsk->Ts;
611 int Rs = fsk->Rs;
612 int Fs = fsk->Fs;
613 int nsym = fsk->Nsym;
614 int nin = fsk->nin;
615 int P = fsk->P;
616 int Nmem = fsk->Nmem;
617 int M = fsk->mode;
618 size_t i,j,m;
619 float ft1;
620
621 COMP t[M]; /* complex number temps */
622 COMP t_c; /* another complex temp */
623 COMP *phi_c = fsk->phi_c;
624 COMP *f_dc = fsk->f_dc;
625 COMP phi_ft;
626 int nold = Nmem-nin;
627
628 COMP dphift;
629 float rx_timing,norm_rx_timing,old_norm_rx_timing,d_norm_rx_timing,appm;
630
631 float fc_avg,fc_tx;
632 float meanebno,stdebno,eye_max;
633 int neyesamp,neyeoffset;
634
635 #ifdef MODEMPROBE_ENABLE
636 #define NMP_NAME 26
637 char mp_name_tmp[NMP_NAME+1]; /* Temporary string for modem probe trace names */
638 #endif
639
640 /* Estimate tone frequencies */
641 fsk_demod_freq_est(fsk,fsk_in,fsk->f_est,M);
642 #ifdef MODEMPROBE_ENABLE
643 modem_probe_samp_f("t_f_est",fsk->f_est,M);
644 #endif
645 float *f_est;
646 if (fsk->freq_est_type)
647 f_est = fsk->f2_est;
648 else
649 f_est = fsk->f_est;
650
651 /* update filter (integrator) memory by shifting in nin samples */
652 for(m=0; m<M; m++) {
653 for(i=0,j=Nmem-nold; i<nold; i++,j++)
654 f_dc[m*Nmem+i] = f_dc[m*Nmem+j];
655 }
656
657 /* freq shift down to around DC, ensuring continuous phase from last frame */
658 COMP dphi_m;
659 for(m=0; m<M; m++) {
660 dphi_m = comp_exp_j(2*M_PI*((f_est[m])/(float)(Fs)));
661 for(i=nold,j=0; i<Nmem; i++,j++) {
662 phi_c[m] = cmult(phi_c[m],dphi_m);
663 f_dc[m*Nmem+i] = cmult(fsk_in[j],cconj(phi_c[m]));
664 }
665 phi_c[m] = comp_normalize(phi_c[m]);
666 #ifdef MODEMPROBE_ENABLE
667 snprintf(mp_name_tmp,NMP_NAME,"t_f%zd_dc",m+1);
668 modem_probe_samp_c(mp_name_tmp,&f_dc[m*Nmem],Nmem);
669 #endif
670 }
671
672 /* integrate over symbol period at a variety of offsets */
673 COMP f_int[M][(nsym+1)*P];
674 for(i=0; i<(nsym+1)*P; i++) {
675 int st = i*Ts/P;
676 int en = st+Ts-1;
677 for(m=0; m<M; m++) {
678 f_int[m][i] = comp0();
679 for(j=st; j<=en; j++)
680 f_int[m][i] = cadd(f_int[m][i], f_dc[m*Nmem+j]);
681 }
682 }
683
684 #ifdef MODEMPROBE_ENABLE
685 for(m=0; m<M; m++) {
686 snprintf(mp_name_tmp,NMP_NAME,"t_f%zd_int",m+1);
687 modem_probe_samp_c(mp_name_tmp,&f_int[m][0],(nsym+1)*P);
688 }
689 #endif
690
691 /* Fine Timing Estimation */
692 /* Apply magic nonlinearity to f1_int and f2_int, shift down to 0,
693 * extract angle */
694
695 /* Figure out how much to spin the oscillator to extract magic spectral line */
696 dphift = comp_exp_j(2*M_PI*((float)(Rs)/(float)(P*Rs)));
697 phi_ft.real = 1;
698 phi_ft.imag = 0;
699 t_c=comp0();
700 for(i=0; i<(nsym+1)*P; i++){
701 /* Get abs^2 of fx_int[i], and add 'em */
702 ft1 = 0;
703 for( m=0; m<M; m++){
704 ft1 += (f_int[m][i].real*f_int[m][i].real) + (f_int[m][i].imag*f_int[m][i].imag);
705 }
706
707 /* Down shift and accumulate magic line */
708 t_c = cadd(t_c,fcmult(ft1,phi_ft));
709
710 /* Spin the oscillator for the magic line shift */
711 phi_ft = cmult(phi_ft,dphift);
712 }
713
714 /* Check for NaNs in the fine timing estimate, return if found */
715 /* otherwise segfaults happen */
716 if( isnan(t_c.real) || isnan(t_c.imag)){
717 return;
718 }
719
720 /* Get the magic angle */
721 norm_rx_timing = atan2f(t_c.imag,t_c.real)/(2*M_PI);
722 rx_timing = norm_rx_timing*(float)P;
723
724 old_norm_rx_timing = fsk->norm_rx_timing;
725 fsk->norm_rx_timing = norm_rx_timing;
726
727 /* Estimate sample clock offset */
728 d_norm_rx_timing = norm_rx_timing - old_norm_rx_timing;
729
730 /* Filter out big jumps in due to nin change */
731 if(fabsf(d_norm_rx_timing) < .2){
732 appm = 1e6*d_norm_rx_timing/(float)nsym;
733 fsk->ppm = .9*fsk->ppm + .1*appm;
734 }
735
736 /* Figure out how many samples are needed the next modem cycle */
737 /* Unless we're in burst mode or nin locked */
738 if(!fsk->burst_mode && !fsk->lock_nin) {
739 if(norm_rx_timing > 0.25)
740 fsk->nin = N+Ts/4;
741 else if(norm_rx_timing < -0.25)
742 fsk->nin = N-Ts/4;
743 else
744 fsk->nin = N;
745 }
746
747 modem_probe_samp_f("t_norm_rx_timing",&(norm_rx_timing),1);
748 modem_probe_samp_i("t_nin",&(fsk->nin),1);
749
750 /* Re-sample the integrators with linear interpolation magic */
751 int low_sample = (int)floorf(rx_timing);
752 float fract = rx_timing - (float)low_sample;
753 int high_sample = (int)ceilf(rx_timing);
754
755 /* Vars for finding the max-of-4 for each bit */
756 float tmax[M];
757
758 #ifdef EST_EBNO
759 meanebno = 0;
760 stdebno = 0;
761 #endif
762
763 float rx_nse_pow = 1E-12; float rx_sig_pow = 0.0;
764 for(i=0; i<nsym; i++) {
765
766 /* resample at ideal sampling instant */
767 int st = (i+1)*P;
768 for( m=0; m<M; m++) {
769 t[m] = fcmult(1-fract,f_int[m][st+low_sample]);
770 t[m] = cadd(t[m],fcmult( fract,f_int[m][st+high_sample]));
771 /* Figure mag^2 of each resampled fx_int */
772 tmax[m] = (t[m].real*t[m].real) + (t[m].imag*t[m].imag);
773 }
774
775 /* hard decision decoding of bits */
776 float max = tmax[0]; /* Maximum for figuring correct symbol */
777 float min = tmax[0];
778 int sym = 0; /* Index of maximum */
779 for( m=0; m<M; m++){
780 if(tmax[m]>max){
781 max = tmax[m];
782 sym = m;
783 }
784 if(tmax[m]<min){
785 min = tmax[m];
786 }
787 }
788
789 if(rx_bits != NULL){
790 /* Get bits for 2FSK and 4FSK */
791 if(M==2){
792 rx_bits[i] = sym==1; /* 2FSK. 1 bit per symbol */
793 }else if(M==4){
794 rx_bits[(i*2)+1] = (sym&0x1); /* 4FSK. 2 bits per symbol */
795 rx_bits[(i*2) ] = (sym&0x2)>>1;
796 }
797 }
798
799 /* Optionally output filter magnitudes for soft decision/LLR
800 calculation. Update SNRest always as this is a useful
801 alternative to the earlier EbNo estimator below */
802 float sum = 0.0;
803 for(m=0; m<M; m++) {
804 if (rx_filt != NULL) rx_filt[m*nsym+i] = sqrtf(tmax[m]);
805 sum += tmax[m];
806 }
807 rx_sig_pow += max;
808 rx_nse_pow += (sum-max)/(M-1);
809
810 /* Accumulate resampled int magnitude for EbNodB estimation */
811 /* Standard deviation is calculated by algorithm devised by crafty soviets */
812 #ifdef EST_EBNO
813 /* Accumulate the square of the sampled value */
814 ft1 = max;
815 stdebno += ft1;
816
817 /* Figure the abs value of the max tone */
818 meanebno += sqrtf(ft1);
819 #endif
820 }
821
822 fsk->rx_sig_pow = rx_sig_pow = rx_sig_pow/nsym;
823 fsk->rx_nse_pow = rx_nse_pow = rx_nse_pow/nsym;
824 fsk->v_est = sqrt(rx_sig_pow-rx_nse_pow);
825 fsk->SNRest = rx_sig_pow/rx_nse_pow;
826
827 #ifdef EST_EBNO
828 /* Calculate mean for EbNodB estimation */
829 meanebno = meanebno/(float)nsym;
830
831 /* Calculate the std. dev for EbNodB estimate */
832 stdebno = (stdebno/(float)nsym) - (meanebno*meanebno);
833 /* trap any negative numbers to avoid NANs flowing through */
834 if (stdebno > 0.0) {
835 stdebno = sqrt(stdebno);
836 } else {
837 stdebno = 0.0;
838 }
839
840 fsk->EbNodB = -6+(20*log10f((1e-6+meanebno)/(1e-6+stdebno)));
841 #else
842 fsk->EbNodB = 1;
843 #endif
844
845 /* Write some statistics to the stats struct */
846
847 /* Save clock offset in ppm */
848 fsk->stats->clock_offset = fsk->ppm;
849
850 /* Calculate and save SNR from EbNodB estimate */
851
852 fsk->stats->snr_est = .5*fsk->stats->snr_est + .5*fsk->EbNodB;//+ 10*log10f(((float)Rs)/((float)Rs*M));
853
854 /* Save rx timing */
855 fsk->stats->rx_timing = (float)rx_timing;
856
857 /* Estimate and save frequency offset */
858 fc_avg = fc_tx = 0.0;
859 for(int m=0; m<M; m++) {
860 fc_avg += f_est[m]/M;
861 fc_tx += (fsk->f1_tx + m*fsk->tone_spacing)/M;
862 }
863 fsk->stats->foff = fc_tx-fc_avg;
864
865 /* Take a sample for the eye diagrams ---------------------------------- */
866
867 /* due to oversample rate P, we have too many samples for eye
868 trace. So lets output a decimated version. We use 2P
869 as we want two symbols worth of samples in trace */
870 #ifndef __EMBEDDED__
871 int neyesamp_dec = ceil(((float)P*2)/MODEM_STATS_EYE_IND_MAX);
872 neyesamp = (P*2)/neyesamp_dec;
873 assert(neyesamp <= MODEM_STATS_EYE_IND_MAX);
874 fsk->stats->neyesamp = neyesamp;
875
876 neyeoffset = high_sample+1;
877
878 int eye_traces = MODEM_STATS_ET_MAX/M;
879 int ind;
880
881 fsk->stats->neyetr = fsk->mode*eye_traces;
882 for( i=0; i<eye_traces; i++){
883 for ( m=0; m<M; m++){
884 for(j=0; j<neyesamp; j++) {
885 /*
886 2*P*i...........: advance two symbols for next trace
887 neyeoffset......: centre trace on ideal timing offset, peak eye opening
888 j*neweyesamp_dec: For 2*P>MODEM_STATS_EYE_IND_MAX advance through integrated
889 samples newamp_dec at a time so we dont overflow rx_eye[][]
890 */
891 ind = 2*P*(i+1) + neyeoffset + j*neyesamp_dec;
892 assert((i*M+m) < MODEM_STATS_ET_MAX);
893 assert(ind >= 0);
894 assert(ind < (nsym+1)*P);
895 fsk->stats->rx_eye[i*M+m][j] = cabsolute(f_int[m][ind]);
896 }
897 }
898 }
899
900 if (fsk->normalise_eye) {
901 eye_max = 0;
902 /* Normalize eye to +/- 1 */
903 for(i=0; i<M*eye_traces; i++)
904 for(j=0; j<neyesamp; j++)
905 if(fabsf(fsk->stats->rx_eye[i][j])>eye_max)
906 eye_max = fabsf(fsk->stats->rx_eye[i][j]);
907
908 for(i=0; i<M*eye_traces; i++)
909 for(j=0; j<neyesamp; j++)
910 fsk->stats->rx_eye[i][j] = fsk->stats->rx_eye[i][j]/eye_max;
911 }
912
913 fsk->stats->nr = 0;
914 fsk->stats->Nc = 0;
915
916 for(i=0; i<M; i++)
917 fsk->stats->f_est[i] = f_est[i];
918 #endif // !__EMBEDDED__
919
920 /* Dump some internal samples */
921 modem_probe_samp_f("t_EbNodB",&(fsk->EbNodB),1);
922 modem_probe_samp_f("t_ppm",&(fsk->ppm),1);
923 modem_probe_samp_f("t_rx_timing",&(rx_timing),1);
924 }
925
926 /*---------------------------------------------------------------------------*\
927
928 FUNCTION....: fsk_demod
929 AUTHOR......: Brady O'Brien
930 DATE CREATED: 11 February 2016
931
932 FSK demodulator functions:
933
934 fsk_demod...: complex samples in, bits out
935 fsk_demos_sd: complex samples in, soft decision symbols out
936
937 \*---------------------------------------------------------------------------*/
938
fsk_demod(struct FSK * fsk,uint8_t rx_bits[],COMP fsk_in[])939 void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], COMP fsk_in[]) {
940 fsk_demod_core(fsk,rx_bits,NULL,fsk_in);
941 }
942
fsk_demod_sd(struct FSK * fsk,float rx_filt[],COMP fsk_in[])943 void fsk_demod_sd(struct FSK *fsk, float rx_filt[], COMP fsk_in[]){
944 fsk_demod_core(fsk,NULL,rx_filt,fsk_in);
945 }
946
947 /* make sure stats have known values in case monitoring process reads stats before they are set */
stats_init(struct FSK * fsk)948 static void stats_init(struct FSK *fsk) {
949 /* Take a sample for the eye diagrams */
950 int i,j,m;
951 int P = fsk->P;
952 int M = fsk->mode;
953
954 /* due to oversample rate P, we have too many samples for eye
955 trace. So lets output a decimated version */
956
957 /* asserts below as we found some problems over-running eye matrix */
958
959 /* TODO: refactor eye tracing code here and in fsk_demod */
960 #ifndef __EMBEDDED__
961 int neyesamp_dec = ceil(((float)P*2)/MODEM_STATS_EYE_IND_MAX);
962 int neyesamp = (P*2)/neyesamp_dec;
963 assert(neyesamp <= MODEM_STATS_EYE_IND_MAX);
964 fsk->stats->neyesamp = neyesamp;
965
966 int eye_traces = MODEM_STATS_ET_MAX/M;
967
968 fsk->stats->neyetr = fsk->mode*eye_traces;
969 for(i=0; i<eye_traces; i++) {
970 for (m=0; m<M; m++){
971 for(j=0; j<neyesamp; j++) {
972 assert((i*M+m) < MODEM_STATS_ET_MAX);
973 fsk->stats->rx_eye[i*M+m][j] = 0;
974 }
975 }
976 }
977 #endif // !__EMBEDDED__
978
979 fsk->stats->rx_timing = fsk->stats->snr_est = 0;
980
981 }
982
983
984 /* Set the FSK modem into burst demod mode */
985
fsk_enable_burst_mode(struct FSK * fsk)986 void fsk_enable_burst_mode(struct FSK *fsk){
987 fsk->nin = fsk->N;
988 fsk->burst_mode = 1;
989 }
990
fsk_clear_estimators(struct FSK * fsk)991 void fsk_clear_estimators(struct FSK *fsk){
992 int i;
993 /* Clear freq estimator state */
994 for(i=0; i < (fsk->Ndft); i++){
995 fsk->Sf[i] = 0;
996 }
997 /* Reset timing diff correction */
998 fsk->nin = fsk->N;
999 }
1000
fsk_get_demod_stats(struct FSK * fsk,struct MODEM_STATS * stats)1001 void fsk_get_demod_stats(struct FSK *fsk, struct MODEM_STATS *stats){
1002 /* copy from internal stats, note we can't overwrite stats completely
1003 as it has other states rqd by caller, also we want a consistent
1004 interface across modem types for the freedv_api.
1005 */
1006
1007 stats->clock_offset = fsk->stats->clock_offset;
1008 stats->snr_est = fsk->stats->snr_est; // TODO: make this SNR not Eb/No
1009 stats->rx_timing = fsk->stats->rx_timing;
1010 stats->foff = fsk->stats->foff;
1011 #ifndef __EMBEDDED__
1012 stats->neyesamp = fsk->stats->neyesamp;
1013 stats->neyetr = fsk->stats->neyetr;
1014 memcpy(stats->rx_eye, fsk->stats->rx_eye, sizeof(stats->rx_eye));
1015 memcpy(stats->f_est, fsk->stats->f_est, fsk->mode*sizeof(float));
1016 #endif // !__EMBEDDED__
1017
1018 /* these fields not used for FSK so set to something sensible */
1019
1020 stats->sync = 0;
1021 stats->nr = fsk->stats->nr;
1022 stats->Nc = fsk->stats->Nc;
1023 }
1024
1025 /*
1026 * Set the minimum and maximum frequencies at which the freq. estimator can find tones
1027 */
fsk_set_freq_est_limits(struct FSK * fsk,int est_min,int est_max)1028 void fsk_set_freq_est_limits(struct FSK *fsk, int est_min, int est_max){
1029 assert(fsk != NULL);
1030 assert(est_min >= -fsk->Fs/2);
1031 assert(est_max <= fsk->Fs/2);
1032 assert(est_max > est_min);
1033 fsk->est_min = est_min;
1034 fsk->est_max = est_max;
1035 }
1036
fsk_stats_normalise_eye(struct FSK * fsk,int normalise_enable)1037 void fsk_stats_normalise_eye(struct FSK *fsk, int normalise_enable) {
1038 assert(fsk != NULL);
1039 fsk->normalise_eye = normalise_enable;
1040 }
1041
fsk_set_freq_est_alg(struct FSK * fsk,int est_type)1042 void fsk_set_freq_est_alg(struct FSK *fsk, int est_type) {
1043 assert(fsk != NULL);
1044 fsk->freq_est_type = est_type;
1045 }
1046
1047
1048
1049
1050
1051
1052
1053