1 /*---------------------------------------------------------------------------*\
2
3 FILE........: fdmdv.c
4 AUTHOR......: David Rowe
5 DATE CREATED: April 14 2012
6
7 Functions that implement the FDMDV modem.
8
9 \*---------------------------------------------------------------------------*/
10
11 /*
12 Copyright (C) 2012 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 INCLUDES
31
32 \*---------------------------------------------------------------------------*/
33
34 #include <assert.h>
35 #include <stdlib.h>
36 #include <stdio.h>
37 #include <string.h>
38 #include <math.h>
39
40 #include "fdmdv_internal.h"
41 #include "codec2_fdmdv.h"
42 #include "comp_prim.h"
43 #include "rn.h"
44 #include "rxdec_coeff.h"
45 #include "test_bits.h"
46 #include "pilot_coeff.h"
47 #include "codec2_fft.h"
48 #include "hanning.h"
49 #include "os.h"
50 #include "machdep.h"
51
52 #include "debug_alloc.h"
53
54 static int sync_uw[] = {1,-1,1,-1,1,-1};
55
56 static const COMP pi_on_4 = { .70710678118654752439, .70710678118654752439 }; // cosf(PI/4) , sinf(PI/4)
57
58
59 /*--------------------------------------------------------------------------* \
60
61 FUNCTION....: fdmdv_create
62 AUTHOR......: David Rowe
63 DATE CREATED: 16/4/2012
64
65 Create and initialise an instance of the modem. Returns a pointer
66 to the modem states or NULL on failure. One set of states is
67 sufficient for a full duplex modem.
68
69 \*---------------------------------------------------------------------------*/
70
fdmdv_create(int Nc)71 struct FDMDV * fdmdv_create(int Nc)
72 {
73 struct FDMDV *f;
74 int c, i, k;
75
76 assert(NC == FDMDV_NC_MAX); /* check public and private #defines match */
77 assert(Nc <= NC);
78 assert(FDMDV_NOM_SAMPLES_PER_FRAME == M_FAC);
79 assert(FDMDV_MAX_SAMPLES_PER_FRAME == (M_FAC+M_FAC/P));
80
81 f = (struct FDMDV*)MALLOC(sizeof(struct FDMDV));
82 if (f == NULL)
83 return NULL;
84
85 f->Nc = Nc;
86
87 f->ntest_bits = Nc*NB*4;
88 f->current_test_bit = 0;
89 f->rx_test_bits_mem = (int*)MALLOC(sizeof(int)*f->ntest_bits);
90 assert(f->rx_test_bits_mem != NULL);
91 for(i=0; i<f->ntest_bits; i++)
92 f->rx_test_bits_mem[i] = 0;
93 assert((sizeof(test_bits)/sizeof(int)) >= f->ntest_bits);
94
95 f->old_qpsk_mapping = 0;
96
97 f->tx_pilot_bit = 0;
98
99 for(c=0; c<Nc+1; c++) {
100 f->prev_tx_symbols[c].real = 1.0;
101 f->prev_tx_symbols[c].imag = 0.0;
102 f->prev_rx_symbols[c].real = 1.0;
103 f->prev_rx_symbols[c].imag = 0.0;
104
105 for(k=0; k<NSYM; k++) {
106 f->tx_filter_memory[c][k].real = 0.0;
107 f->tx_filter_memory[c][k].imag = 0.0;
108 }
109
110 /* Spread initial FDM carrier phase out as far as possible.
111 This helped PAPR for a few dB. We don't need to adjust rx
112 phase as DQPSK takes care of that. */
113
114 f->phase_tx[c].real = cosf(2.0*PI*c/(Nc+1));
115 f->phase_tx[c].imag = sinf(2.0*PI*c/(Nc+1));
116
117 f->phase_rx[c].real = 1.0;
118 f->phase_rx[c].imag = 0.0;
119
120 for(k=0; k<NT*P; k++) {
121 f->rx_filter_mem_timing[c][k].real = 0.0;
122 f->rx_filter_mem_timing[c][k].imag = 0.0;
123 }
124 }
125 f->prev_tx_symbols[Nc].real = 2.0;
126
127 fdmdv_set_fsep(f, FSEP);
128 f->freq[Nc].real = cosf(2.0*PI*0.0/FS);
129 f->freq[Nc].imag = sinf(2.0*PI*0.0/FS);
130 f->freq_pol[Nc] = 2.0*PI*0.0/FS;
131
132 f->fbb_rect.real = cosf(2.0*PI*FDMDV_FCENTRE/FS);
133 f->fbb_rect.imag = sinf(2.0*PI*FDMDV_FCENTRE/FS);
134 f->fbb_pol = 2.0*PI*FDMDV_FCENTRE/FS;
135 f->fbb_phase_tx.real = 1.0;
136 f->fbb_phase_tx.imag = 0.0;
137 f->fbb_phase_rx.real = 1.0;
138 f->fbb_phase_rx.imag = 0.0;
139
140 /* Generate DBPSK pilot Look Up Table (LUT) */
141
142 generate_pilot_lut(f->pilot_lut, &f->freq[Nc]);
143
144 /* freq Offset estimation states */
145
146 f->fft_pilot_cfg = codec2_fft_alloc (MPILOTFFT, 0, NULL, NULL);
147 assert(f->fft_pilot_cfg != NULL);
148
149 for(i=0; i<NPILOTBASEBAND; i++) {
150 f->pilot_baseband1[i].real = f->pilot_baseband2[i].real = 0.0;
151 f->pilot_baseband1[i].imag = f->pilot_baseband2[i].imag = 0.0;
152 }
153 f->pilot_lut_index = 0;
154 f->prev_pilot_lut_index = 3*M_FAC;
155
156 for(i=0; i<NRXDECMEM; i++) {
157 f->rxdec_lpf_mem[i].real = 0.0;
158 f->rxdec_lpf_mem[i].imag = 0.0;
159 }
160
161 for(i=0; i<NPILOTLPF; i++) {
162 f->pilot_lpf1[i].real = f->pilot_lpf2[i].real = 0.0;
163 f->pilot_lpf1[i].imag = f->pilot_lpf2[i].imag = 0.0;
164 }
165
166 f->foff = 0.0;
167 f->foff_phase_rect.real = 1.0;
168 f->foff_phase_rect.imag = 0.0;
169
170 for(i=0; i<NRX_FDM_MEM; i++) {
171 f->rx_fdm_mem[i].real = 0.0;
172 f->rx_fdm_mem[i].imag = 0.0;
173 }
174
175 f->fest_state = 0;
176 f->sync = 0;
177 f->timer = 0;
178 for(i=0; i<NSYNC_MEM; i++)
179 f->sync_mem[i] = 0;
180
181 for(c=0; c<Nc+1; c++) {
182 f->sig_est[c] = 0.0;
183 f->noise_est[c] = 0.0;
184 }
185
186 f->sig_pwr_av = 0.0;
187 f->foff_filt = 0.0;
188
189 return f;
190 }
191
192 /*---------------------------------------------------------------------------*\
193
194 FUNCTION....: fdmdv_destroy
195 AUTHOR......: David Rowe
196 DATE CREATED: 16/4/2012
197
198 Destroy an instance of the modem.
199
200 \*---------------------------------------------------------------------------*/
201
fdmdv_destroy(struct FDMDV * fdmdv)202 void fdmdv_destroy(struct FDMDV *fdmdv)
203 {
204 assert(fdmdv != NULL);
205 codec2_fft_free(fdmdv->fft_pilot_cfg);
206 FREE(fdmdv->rx_test_bits_mem);
207 FREE(fdmdv);
208 }
209
210
fdmdv_use_old_qpsk_mapping(struct FDMDV * fdmdv)211 void fdmdv_use_old_qpsk_mapping(struct FDMDV *fdmdv) {
212 fdmdv->old_qpsk_mapping = 1;
213 }
214
215
fdmdv_bits_per_frame(struct FDMDV * fdmdv)216 int fdmdv_bits_per_frame(struct FDMDV *fdmdv)
217 {
218 return (fdmdv->Nc * NB);
219 }
220
221 /*---------------------------------------------------------------------------*\
222
223 FUNCTION....: fdmdv_get_test_bits()
224 AUTHOR......: David Rowe
225 DATE CREATED: 16/4/2012
226
227 Generate a frame of bits from a repeating sequence of random data. OK so
228 it's not very random if it repeats but it makes syncing at the demod easier
229 for test purposes.
230
231 \*---------------------------------------------------------------------------*/
232
fdmdv_get_test_bits(struct FDMDV * f,int tx_bits[])233 void fdmdv_get_test_bits(struct FDMDV *f, int tx_bits[])
234 {
235 int i;
236 int bits_per_frame = fdmdv_bits_per_frame(f);
237
238 for(i=0; i<bits_per_frame; i++) {
239 tx_bits[i] = test_bits[f->current_test_bit];
240 f->current_test_bit++;
241 if (f->current_test_bit > (f->ntest_bits-1))
242 f->current_test_bit = 0;
243 }
244 }
245
fdmdv_get_fsep(struct FDMDV * f)246 float fdmdv_get_fsep(struct FDMDV *f)
247 {
248 return f->fsep;
249 }
250
fdmdv_set_fsep(struct FDMDV * f,float fsep)251 void fdmdv_set_fsep(struct FDMDV *f, float fsep) {
252 int c;
253 float carrier_freq;
254
255 f->fsep = fsep;
256
257 /* Set up frequency of each carrier */
258
259 for(c=0; c<f->Nc/2; c++) {
260 carrier_freq = (-f->Nc/2 + c)*f->fsep;
261 f->freq[c].real = cosf(2.0*PI*carrier_freq/FS);
262 f->freq[c].imag = sinf(2.0*PI*carrier_freq/FS);
263 f->freq_pol[c] = 2.0*PI*carrier_freq/FS;
264 }
265
266 for(c=f->Nc/2; c<f->Nc; c++) {
267 carrier_freq = (-f->Nc/2 + c + 1)*f->fsep;
268 f->freq[c].real = cosf(2.0*PI*carrier_freq/FS);
269 f->freq[c].imag = sinf(2.0*PI*carrier_freq/FS);
270 f->freq_pol[c] = 2.0*PI*carrier_freq/FS;
271 }
272 }
273
274
275 /*---------------------------------------------------------------------------*\
276
277 FUNCTION....: bits_to_dqpsk_symbols()
278 AUTHOR......: David Rowe
279 DATE CREATED: 16/4/2012
280
281 Maps bits to parallel DQPSK symbols. Generate Nc+1 QPSK symbols from
282 vector of (1,Nc*Nb) input tx_bits. The Nc+1 symbol is the +1 -1 +1
283 .... BPSK sync carrier.
284
285 \*---------------------------------------------------------------------------*/
286
bits_to_dqpsk_symbols(COMP tx_symbols[],int Nc,COMP prev_tx_symbols[],int tx_bits[],int * pilot_bit,int old_qpsk_mapping)287 void bits_to_dqpsk_symbols(COMP tx_symbols[], int Nc, COMP prev_tx_symbols[], int tx_bits[], int *pilot_bit, int old_qpsk_mapping)
288 {
289 int c, msb, lsb;
290 COMP j = {0.0,1.0};
291
292 /* Map tx_bits to to Nc DQPSK symbols. Note legacy support for
293 old (suboptimal) V0.91 FreeDV mapping */
294
295 for(c=0; c<Nc; c++) {
296 msb = tx_bits[2*c];
297 lsb = tx_bits[2*c+1];
298 if ((msb == 0) && (lsb == 0))
299 tx_symbols[c] = prev_tx_symbols[c];
300 if ((msb == 0) && (lsb == 1))
301 tx_symbols[c] = cmult(j, prev_tx_symbols[c]);
302 if ((msb == 1) && (lsb == 0)) {
303 if (old_qpsk_mapping)
304 tx_symbols[c] = cneg(prev_tx_symbols[c]);
305 else
306 tx_symbols[c] = cmult(cneg(j),prev_tx_symbols[c]);
307 }
308 if ((msb == 1) && (lsb == 1)) {
309 if (old_qpsk_mapping)
310 tx_symbols[c] = cmult(cneg(j),prev_tx_symbols[c]);
311 else
312 tx_symbols[c] = cneg(prev_tx_symbols[c]);
313 }
314 }
315
316 /* +1 -1 +1 -1 BPSK sync carrier, once filtered becomes (roughly)
317 two spectral lines at +/- Rs/2 */
318
319 if (*pilot_bit)
320 tx_symbols[Nc] = cneg(prev_tx_symbols[Nc]);
321 else
322 tx_symbols[Nc] = prev_tx_symbols[Nc];
323
324 if (*pilot_bit)
325 *pilot_bit = 0;
326 else
327 *pilot_bit = 1;
328 }
329
330
331 /*---------------------------------------------------------------------------*\
332
333 FUNCTION....: tx_filter()
334 AUTHOR......: David Rowe
335 DATE CREATED: 17/4/2012
336
337 Given Nc*NB bits construct M_FAC samples (1 symbol) of Nc+1 filtered
338 symbols streams.
339
340 \*---------------------------------------------------------------------------*/
341
tx_filter(COMP tx_baseband[NC+1][M_FAC],int Nc,COMP tx_symbols[],COMP tx_filter_memory[NC+1][NSYM])342 void tx_filter(COMP tx_baseband[NC+1][M_FAC], int Nc, COMP tx_symbols[], COMP tx_filter_memory[NC+1][NSYM])
343 {
344 int c;
345 int i,j,k;
346 float acc;
347 COMP gain;
348
349 gain.real = sqrtf(2.0)/2.0;
350 gain.imag = 0.0;
351
352 for(c=0; c<Nc+1; c++)
353 tx_filter_memory[c][NSYM-1] = cmult(tx_symbols[c], gain);
354
355 /*
356 tx filter each symbol, generate M_FAC filtered output samples for each symbol.
357 Efficient polyphase filter techniques used as tx_filter_memory is sparse
358 */
359
360 for(i=0; i<M_FAC; i++) {
361 for(c=0; c<Nc+1; c++) {
362
363 /* filter real sample of symbol for carrier c */
364
365 acc = 0.0;
366 for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC)
367 acc += M_FAC * tx_filter_memory[c][j].real * gt_alpha5_root[k];
368 tx_baseband[c][i].real = acc;
369
370 /* filter imag sample of symbol for carrier c */
371
372 acc = 0.0;
373 for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC)
374 acc += M_FAC * tx_filter_memory[c][j].imag * gt_alpha5_root[k];
375 tx_baseband[c][i].imag = acc;
376
377 }
378 }
379
380 /* shift memory, inserting zeros at end */
381
382 for(i=0; i<NSYM-1; i++)
383 for(c=0; c<Nc+1; c++)
384 tx_filter_memory[c][i] = tx_filter_memory[c][i+1];
385
386 for(c=0; c<Nc+1; c++) {
387 tx_filter_memory[c][NSYM-1].real = 0.0;
388 tx_filter_memory[c][NSYM-1].imag = 0.0;
389 }
390 }
391
392
393 /*---------------------------------------------------------------------------*\
394
395 FUNCTION....: tx_filter_and_upconvert()
396 AUTHOR......: David Rowe
397 DATE CREATED: 13 August 2014
398
399 Given Nc symbols construct M_FAC samples (1 symbol) of Nc+1 filtered
400 and upconverted symbols.
401
402 \*---------------------------------------------------------------------------*/
403
tx_filter_and_upconvert(COMP tx_fdm[],int Nc,COMP tx_symbols[],COMP tx_filter_memory[NC+1][NSYM],COMP phase_tx[],COMP freq[],COMP * fbb_phase,COMP fbb_rect)404 void tx_filter_and_upconvert(COMP tx_fdm[], int Nc, COMP tx_symbols[],
405 COMP tx_filter_memory[NC+1][NSYM],
406 COMP phase_tx[], COMP freq[],
407 COMP *fbb_phase, COMP fbb_rect)
408 {
409 int c;
410 int i,j,k;
411 float acc;
412 COMP gain;
413 COMP tx_baseband;
414 COMP two = {2.0, 0.0};
415 float mag;
416
417 gain.real = sqrtf(2.0)/2.0;
418 gain.imag = 0.0;
419
420 for(i=0; i<M_FAC; i++) {
421 tx_fdm[i].real = 0.0;
422 tx_fdm[i].imag = 0.0;
423 }
424
425 for(c=0; c<Nc+1; c++)
426 tx_filter_memory[c][NSYM-1] = cmult(tx_symbols[c], gain);
427
428 /*
429 tx filter each symbol, generate M_FAC filtered output samples for
430 each symbol, which we then freq shift and sum with other
431 carriers. Efficient polyphase filter techniques used as
432 tx_filter_memory is sparse
433 */
434
435 for(c=0; c<Nc+1; c++) {
436 for(i=0; i<M_FAC; i++) {
437
438 /* filter real sample of symbol for carrier c */
439
440 acc = 0.0;
441 for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC)
442 acc += M_FAC * tx_filter_memory[c][j].real * gt_alpha5_root[k];
443 tx_baseband.real = acc;
444
445 /* filter imag sample of symbol for carrier c */
446
447 acc = 0.0;
448 for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC)
449 acc += M_FAC * tx_filter_memory[c][j].imag * gt_alpha5_root[k];
450 tx_baseband.imag = acc;
451
452 /* freq shift and sum */
453
454 phase_tx[c] = cmult(phase_tx[c], freq[c]);
455 tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband, phase_tx[c]));
456 }
457 }
458
459 /* shift whole thing up to carrier freq */
460
461 for (i=0; i<M_FAC; i++) {
462 *fbb_phase = cmult(*fbb_phase, fbb_rect);
463 tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase);
464 }
465
466 /*
467 Scale such that total Carrier power C of real(tx_fdm) = Nc. This
468 excludes the power of the pilot tone.
469 We return the complex (single sided) signal to make frequency
470 shifting for the purpose of testing easier
471 */
472
473 for (i=0; i<M_FAC; i++)
474 tx_fdm[i] = cmult(two, tx_fdm[i]);
475
476 /* normalise digital oscillators as the magnitude can drift over time */
477
478 for (c=0; c<Nc+1; c++) {
479 mag = cabsolute(phase_tx[c]);
480 phase_tx[c].real /= mag;
481 phase_tx[c].imag /= mag;
482 }
483
484 mag = cabsolute(*fbb_phase);
485 fbb_phase->real /= mag;
486 fbb_phase->imag /= mag;
487
488 /* shift memory, inserting zeros at end */
489
490 for(i=0; i<NSYM-1; i++)
491 for(c=0; c<Nc+1; c++)
492 tx_filter_memory[c][i] = tx_filter_memory[c][i+1];
493
494 for(c=0; c<Nc+1; c++) {
495 tx_filter_memory[c][NSYM-1].real = 0.0;
496 tx_filter_memory[c][NSYM-1].imag = 0.0;
497 }
498 }
499
500
501 /*---------------------------------------------------------------------------*\
502
503 FUNCTION....: fdm_upconvert()
504 AUTHOR......: David Rowe
505 DATE CREATED: 17/4/2012
506
507 Construct FDM signal by frequency shifting each filtered symbol
508 stream. Returns complex signal so we can apply frequency offsets
509 easily.
510
511 \*---------------------------------------------------------------------------*/
512
fdm_upconvert(COMP tx_fdm[],int Nc,COMP tx_baseband[NC+1][M_FAC],COMP phase_tx[],COMP freq[],COMP * fbb_phase,COMP fbb_rect)513 void fdm_upconvert(COMP tx_fdm[], int Nc, COMP tx_baseband[NC+1][M_FAC], COMP phase_tx[], COMP freq[],
514 COMP *fbb_phase, COMP fbb_rect)
515 {
516 int i,c;
517 COMP two = {2.0, 0.0};
518 float mag;
519
520 for(i=0; i<M_FAC; i++) {
521 tx_fdm[i].real = 0.0;
522 tx_fdm[i].imag = 0.0;
523 }
524
525 for (c=0; c<=Nc; c++)
526 for (i=0; i<M_FAC; i++) {
527 phase_tx[c] = cmult(phase_tx[c], freq[c]);
528 tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband[c][i], phase_tx[c]));
529 }
530
531 /* shift whole thing up to carrier freq */
532
533 for (i=0; i<M_FAC; i++) {
534 *fbb_phase = cmult(*fbb_phase, fbb_rect);
535 tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase);
536 }
537
538 /*
539 Scale such that total Carrier power C of real(tx_fdm) = Nc. This
540 excludes the power of the pilot tone.
541 We return the complex (single sided) signal to make frequency
542 shifting for the purpose of testing easier
543 */
544
545 for (i=0; i<M_FAC; i++)
546 tx_fdm[i] = cmult(two, tx_fdm[i]);
547
548 /* normalise digital oscilators as the magnitude can drift over time */
549
550 for (c=0; c<Nc+1; c++) {
551 mag = cabsolute(phase_tx[c]);
552 phase_tx[c].real /= mag;
553 phase_tx[c].imag /= mag;
554 }
555
556 mag = cabsolute(*fbb_phase);
557 fbb_phase->real /= mag;
558 fbb_phase->imag /= mag;
559 }
560
561 /*---------------------------------------------------------------------------*\
562
563 FUNCTION....: fdmdv_mod()
564 AUTHOR......: David Rowe
565 DATE CREATED: 26/4/2012
566
567 FDMDV modulator, take a frame of FDMDV_BITS_PER_FRAME bits and
568 generates a frame of FDMDV_SAMPLES_PER_FRAME modulated symbols.
569 Sync bit is returned to aid alignment of your next frame.
570
571 The sync_bit value returned will be used for the _next_ frame.
572
573 The output signal is complex to support single sided frequency
574 shifting, for example when testing frequency offsets in channel
575 simulation.
576
577 \*---------------------------------------------------------------------------*/
578
fdmdv_mod(struct FDMDV * fdmdv,COMP tx_fdm[],int tx_bits[],int * sync_bit)579 void fdmdv_mod(struct FDMDV *fdmdv, COMP tx_fdm[], int tx_bits[], int *sync_bit)
580 {
581 COMP tx_symbols[NC+1];
582 PROFILE_VAR(mod_start, tx_filter_and_upconvert_start);
583
584 PROFILE_SAMPLE(mod_start);
585 bits_to_dqpsk_symbols(tx_symbols, fdmdv->Nc, fdmdv->prev_tx_symbols, tx_bits, &fdmdv->tx_pilot_bit, fdmdv->old_qpsk_mapping);
586 memcpy(fdmdv->prev_tx_symbols, tx_symbols, sizeof(COMP)*(fdmdv->Nc+1));
587 PROFILE_SAMPLE_AND_LOG(tx_filter_and_upconvert_start, mod_start, " bits_to_dqpsk_symbols");
588 tx_filter_and_upconvert(tx_fdm, fdmdv->Nc, tx_symbols, fdmdv->tx_filter_memory,
589 fdmdv->phase_tx, fdmdv->freq, &fdmdv->fbb_phase_tx, fdmdv->fbb_rect);
590 PROFILE_SAMPLE_AND_LOG2(tx_filter_and_upconvert_start, " tx_filter_and_upconvert");
591
592 *sync_bit = fdmdv->tx_pilot_bit;
593 }
594
595 /*---------------------------------------------------------------------------*\
596
597 FUNCTION....: generate_pilot_fdm()
598 AUTHOR......: David Rowe
599 DATE CREATED: 19/4/2012
600
601 Generate M_FAC samples of DBPSK pilot signal for Freq offset estimation.
602
603 \*---------------------------------------------------------------------------*/
604
generate_pilot_fdm(COMP * pilot_fdm,int * bit,float * symbol,float * filter_mem,COMP * phase,COMP * freq)605 void generate_pilot_fdm(COMP *pilot_fdm, int *bit, float *symbol,
606 float *filter_mem, COMP *phase, COMP *freq)
607 {
608 int i,j,k;
609 float tx_baseband[M_FAC];
610
611 /* +1 -1 +1 -1 DBPSK sync carrier, once filtered becomes (roughly)
612 two spectral lines at +/- RS/2 */
613
614 if (*bit)
615 *symbol = -*symbol;
616
617 if (*bit)
618 *bit = 0;
619 else
620 *bit = 1;
621
622 /* filter DPSK symbol to create M_FAC baseband samples */
623
624 filter_mem[NFILTER-1] = (sqrtf(2)/2) * *symbol;
625 for(i=0; i<M_FAC; i++) {
626 tx_baseband[i] = 0.0;
627 for(j=M_FAC-1,k=M_FAC-i-1; j<NFILTER; j+=M_FAC,k+=M_FAC)
628 tx_baseband[i] += M_FAC * filter_mem[j] * gt_alpha5_root[k];
629 }
630
631 /* shift memory, inserting zeros at end */
632
633 for(i=0; i<NFILTER-M_FAC; i++)
634 filter_mem[i] = filter_mem[i+M_FAC];
635
636 for(i=NFILTER-M_FAC; i<NFILTER; i++)
637 filter_mem[i] = 0.0;
638
639 /* upconvert */
640
641 for(i=0; i<M_FAC; i++) {
642 *phase = cmult(*phase, *freq);
643 pilot_fdm[i].real = sqrtf(2)*2*tx_baseband[i] * phase->real;
644 pilot_fdm[i].imag = sqrtf(2)*2*tx_baseband[i] * phase->imag;
645 }
646 }
647
648 /*---------------------------------------------------------------------------*\
649
650 FUNCTION....: generate_pilot_lut()
651 AUTHOR......: David Rowe
652 DATE CREATED: 19/4/2012
653
654 Generate a 4M sample vector of DBPSK pilot signal. As the pilot signal
655 is periodic in 4M samples we can then use this vector as a look up table
656 for pilot signal generation in the demod.
657
658 \*---------------------------------------------------------------------------*/
659
generate_pilot_lut(COMP pilot_lut[],COMP * pilot_freq)660 void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq)
661 {
662 int pilot_rx_bit = 0;
663 float pilot_symbol = sqrtf(2.0);
664 COMP pilot_phase = {1.0, 0.0};
665 float pilot_filter_mem[NFILTER];
666 COMP pilot[M_FAC];
667 int i,f;
668
669 for(i=0; i<NFILTER; i++)
670 pilot_filter_mem[i] = 0.0;
671
672 /* discard first 4 symbols as filter memory is filling, just keep
673 last four symbols */
674
675 for(f=0; f<8; f++) {
676 generate_pilot_fdm(pilot, &pilot_rx_bit, &pilot_symbol, pilot_filter_mem, &pilot_phase, pilot_freq);
677 if (f >= 4)
678 memcpy(&pilot_lut[M_FAC*(f-4)], pilot, M_FAC*sizeof(COMP));
679 }
680
681 // create complex conjugate since we need this and only this later on
682 for (f=0;f<4*M_FAC;f++)
683 {
684 pilot_lut[f] = cconj(pilot_lut[f]);
685 }
686
687 }
688
689 /*---------------------------------------------------------------------------*\
690
691 FUNCTION....: lpf_peak_pick()
692 AUTHOR......: David Rowe
693 DATE CREATED: 20/4/2012
694
695 LPF and peak pick part of freq est, put in a function as we call it twice.
696
697 \*---------------------------------------------------------------------------*/
698
lpf_peak_pick(float * foff,float * max,COMP pilot_baseband[],COMP pilot_lpf[],codec2_fft_cfg fft_pilot_cfg,COMP S[],int nin,int do_fft)699 void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[],
700 COMP pilot_lpf[], codec2_fft_cfg fft_pilot_cfg, COMP S[], int nin,
701 int do_fft)
702 {
703 int i,j,k;
704 int mpilot;
705 float mag, imax;
706 int ix;
707 float r;
708
709 /* LPF cutoff 200Hz, so we can handle max +/- 200 Hz freq offset */
710
711 for(i=0; i<NPILOTLPF-nin; i++)
712 pilot_lpf[i] = pilot_lpf[nin+i];
713 for(i=NPILOTLPF-nin, j=NPILOTBASEBAND-nin; i<NPILOTLPF; i++,j++) {
714 pilot_lpf[i].real = 0.0; pilot_lpf[i].imag = 0.0;
715
716 // STM32F4 hand optimized, this alone makes it go done from 1.6 to 1.17ms
717 // switching pilot_coeff to RAM (by removing const in pilot_coeff.h) would save
718 // another 0.11 ms at the expense of NPILOTCOEFF * 4 bytes == 120 bytes RAM
719
720 if (NPILOTCOEFF%5 == 0)
721 {
722 for(k=0; k<NPILOTCOEFF; k+=5)
723 {
724 COMP i0 = fcmult(pilot_coeff[k], pilot_baseband[j-NPILOTCOEFF+1+k]);
725 COMP i1 = fcmult(pilot_coeff[k+1], pilot_baseband[j-NPILOTCOEFF+1+k+1]);
726 COMP i2 = fcmult(pilot_coeff[k+2], pilot_baseband[j-NPILOTCOEFF+1+k+2]);
727 COMP i3 = fcmult(pilot_coeff[k+3], pilot_baseband[j-NPILOTCOEFF+1+k+3]);
728 COMP i4 = fcmult(pilot_coeff[k+4], pilot_baseband[j-NPILOTCOEFF+1+k+4]);
729
730 pilot_lpf[i] = cadd(cadd(cadd(cadd(cadd(pilot_lpf[i], i0),i1),i2),i3),i4);
731 }
732 }
733 else
734 {
735 for(k=0; k<NPILOTCOEFF; k++)
736 {
737 pilot_lpf[i] = cadd(pilot_lpf[i], fcmult(pilot_coeff[k], pilot_baseband[j-NPILOTCOEFF+1+k]));
738 }
739
740 }
741 }
742
743 /* We only need to do FFTs if we are out of sync. Making them optional saves CPU in sync, which is when
744 we need to run the codec */
745
746 imax = 0.0;
747 *foff = 0.0;
748 for(i=0; i<MPILOTFFT; i++) {
749 S[i].real = 0.0;
750 S[i].imag = 0.0;
751 }
752
753 if (do_fft) {
754
755 /* decimate to improve DFT resolution, window and DFT */
756 mpilot = FS/(2*200); /* calc decimation rate given new sample rate is twice LPF freq */
757 for(i=0,j=0; i<NPILOTLPF; i+=mpilot,j++) {
758 S[j] = fcmult(hanning[i], pilot_lpf[i]);
759 }
760
761 codec2_fft_inplace(fft_pilot_cfg, S);
762
763 /* peak pick and convert to Hz */
764
765 imax = 0.0;
766 ix = 0;
767 for(i=0; i<MPILOTFFT; i++) {
768 mag = S[i].real*S[i].real + S[i].imag*S[i].imag;
769 if (mag > imax) {
770 imax = mag;
771 ix = i;
772 }
773 }
774 r = 2.0*200.0/MPILOTFFT; /* maps FFT bin to frequency in Hz */
775
776 if (ix >= MPILOTFFT/2)
777 *foff = (ix - MPILOTFFT)*r;
778 else
779 *foff = (ix)*r;
780 }
781
782 *max = imax;
783
784 }
785
786 /*---------------------------------------------------------------------------*\
787
788 FUNCTION....: rx_est_freq_offset()
789 AUTHOR......: David Rowe
790 DATE CREATED: 19/4/2012
791
792 Estimate frequency offset of FDM signal using BPSK pilot. Note that
793 this algorithm is quite sensitive to pilot tone level wrt other
794 carriers, so test variations to the pilot amplitude carefully.
795
796 \*---------------------------------------------------------------------------*/
797
rx_est_freq_offset(struct FDMDV * f,COMP rx_fdm[],int nin,int do_fft)798 float rx_est_freq_offset(struct FDMDV *f, COMP rx_fdm[], int nin, int do_fft)
799 {
800 int i;
801 #ifndef FDV_ARM_MATH
802 int j;
803 #endif
804 COMP pilot[M_FAC+M_FAC/P];
805 COMP prev_pilot[M_FAC+M_FAC/P];
806 float foff, foff1, foff2;
807 float max1, max2;
808
809 assert(nin <= M_FAC+M_FAC/P);
810
811 /* get pilot samples used for correlation/down conversion of rx signal */
812
813 for (i=0; i<nin; i++) {
814 pilot[i] = f->pilot_lut[f->pilot_lut_index];
815 f->pilot_lut_index++;
816 if (f->pilot_lut_index >= 4*M_FAC)
817 f->pilot_lut_index = 0;
818
819 prev_pilot[i] = f->pilot_lut[f->prev_pilot_lut_index];
820 f->prev_pilot_lut_index++;
821 if (f->prev_pilot_lut_index >= 4*M_FAC)
822 f->prev_pilot_lut_index = 0;
823 }
824
825 /*
826 Down convert latest M_FAC samples of pilot by multiplying by ideal
827 BPSK pilot signal we have generated locally. The peak of the
828 resulting signal is sensitive to the time shift between the
829 received and local version of the pilot, so we do it twice at
830 different time shifts and choose the maximum.
831 */
832
833 for(i=0; i<NPILOTBASEBAND-nin; i++) {
834 f->pilot_baseband1[i] = f->pilot_baseband1[i+nin];
835 f->pilot_baseband2[i] = f->pilot_baseband2[i+nin];
836 }
837
838 #ifndef FDV_ARM_MATH
839 for(i=0,j=NPILOTBASEBAND-nin; i<nin; i++,j++) {
840 f->pilot_baseband1[j] = cmult(rx_fdm[i], pilot[i]);
841 f->pilot_baseband2[j] = cmult(rx_fdm[i], prev_pilot[i]);
842 }
843 #else
844 // TODO: Maybe a handwritten mult taking advantage of rx_fdm[0] being
845 // used twice would be faster but this is for sure faster than
846 // the implementation above in any case.
847 arm_cmplx_mult_cmplx_f32(&rx_fdm[0].real,&pilot[0].real,&f->pilot_baseband1[NPILOTBASEBAND-nin].real,nin);
848 arm_cmplx_mult_cmplx_f32(&rx_fdm[0].real,&prev_pilot[0].real,&f->pilot_baseband2[NPILOTBASEBAND-nin].real,nin);
849 #endif
850
851 lpf_peak_pick(&foff1, &max1, f->pilot_baseband1, f->pilot_lpf1, f->fft_pilot_cfg, f->S1, nin, do_fft);
852 lpf_peak_pick(&foff2, &max2, f->pilot_baseband2, f->pilot_lpf2, f->fft_pilot_cfg, f->S2, nin, do_fft);
853
854 if (max1 > max2)
855 foff = foff1;
856 else
857 foff = foff2;
858
859 return foff;
860 }
861
862 /*---------------------------------------------------------------------------*\
863
864 FUNCTION....: fdmdv_freq_shift()
865 AUTHOR......: David Rowe
866 DATE CREATED: 26/4/2012
867
868 Frequency shift modem signal. The use of complex input and output allows
869 single sided frequency shifting (no images).
870
871 \*---------------------------------------------------------------------------*/
872
fdmdv_freq_shift(COMP rx_fdm_fcorr[],COMP rx_fdm[],float foff,COMP * foff_phase_rect,int nin)873 void fdmdv_freq_shift(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff,
874 COMP *foff_phase_rect, int nin)
875 {
876 COMP foff_rect;
877 float mag;
878 int i;
879
880 foff_rect.real = cosf(2.0*PI*foff/FS);
881 foff_rect.imag = sinf(2.0*PI*foff/FS);
882 for(i=0; i<nin; i++) {
883 *foff_phase_rect = cmult(*foff_phase_rect, foff_rect);
884 rx_fdm_fcorr[i] = cmult(rx_fdm[i], *foff_phase_rect);
885 }
886
887 /* normalise digital oscilator as the magnitude can drfift over time */
888
889 mag = cabsolute(*foff_phase_rect);
890 foff_phase_rect->real /= mag;
891 foff_phase_rect->imag /= mag;
892 }
893
894 /*---------------------------------------------------------------------------*\
895
896 FUNCTION....: fdm_downconvert
897 AUTHOR......: David Rowe
898 DATE CREATED: 22/4/2012
899
900 Frequency shift each modem carrier down to Nc+1 baseband signals.
901
902 \*---------------------------------------------------------------------------*/
903
fdm_downconvert(COMP rx_baseband[NC+1][M_FAC+M_FAC/P],int Nc,COMP rx_fdm[],COMP phase_rx[],COMP freq[],int nin)904 void fdm_downconvert(COMP rx_baseband[NC+1][M_FAC+M_FAC/P], int Nc, COMP rx_fdm[], COMP phase_rx[], COMP freq[], int nin)
905 {
906 int i,c;
907 float mag;
908
909 /* maximum number of input samples to demod */
910
911 assert(nin <= (M_FAC+M_FAC/P));
912
913 /* downconvert */
914
915 for (c=0; c<Nc+1; c++)
916 for (i=0; i<nin; i++) {
917 phase_rx[c] = cmult(phase_rx[c], freq[c]);
918 rx_baseband[c][i] = cmult(rx_fdm[i], cconj(phase_rx[c]));
919 }
920
921 /* normalise digital oscilators as the magnitude can drift over time */
922
923 for (c=0; c<Nc+1; c++) {
924 mag = cabsolute(phase_rx[c]);
925 phase_rx[c].real /= mag;
926 phase_rx[c].imag /= mag;
927 }
928 }
929
930 /*---------------------------------------------------------------------------*\
931
932 FUNCTION....: rx_filter()
933 AUTHOR......: David Rowe
934 DATE CREATED: 22/4/2012
935
936 Receive filter each baseband signal at oversample rate P. Filtering at
937 rate P lowers CPU compared to rate M_FAC.
938
939 Depending on the number of input samples to the demod nin, we
940 produce P-1, P (usually), or P+1 filtered samples at rate P. nin is
941 occasionally adjusted to compensate for timing slips due to
942 different tx and rx sample clocks.
943
944 \*---------------------------------------------------------------------------*/
945
rx_filter(COMP rx_filt[][P+1],int Nc,COMP rx_baseband[][M_FAC+M_FAC/P],COMP rx_filter_memory[][NFILTER],int nin)946 void rx_filter(COMP rx_filt[][P+1], int Nc, COMP rx_baseband[][M_FAC+M_FAC/P], COMP rx_filter_memory[][NFILTER], int nin)
947 {
948 int c, i,j,k,l;
949 int n=M_FAC/P;
950
951 /* rx filter each symbol, generate P filtered output samples for
952 each symbol. Note we keep filter memory at rate M_FAC, it's just
953 the filter output at rate P */
954
955 for(i=0, j=0; i<nin; i+=n,j++) {
956
957 /* latest input sample */
958
959 for(c=0; c<Nc+1; c++)
960 for(k=NFILTER-n,l=i; k<NFILTER; k++,l++)
961 rx_filter_memory[c][k] = rx_baseband[c][l];
962
963 /* convolution (filtering) */
964
965 for(c=0; c<Nc+1; c++) {
966 rx_filt[c][j].real = 0.0; rx_filt[c][j].imag = 0.0;
967 for(k=0; k<NFILTER; k++)
968 rx_filt[c][j] = cadd(rx_filt[c][j], fcmult(gt_alpha5_root[k], rx_filter_memory[c][k]));
969 }
970
971 /* make room for next input sample */
972
973 for(c=0; c<Nc+1; c++)
974 for(k=0,l=n; k<NFILTER-n; k++,l++)
975 rx_filter_memory[c][k] = rx_filter_memory[c][l];
976 }
977
978 assert(j <= (P+1)); /* check for any over runs */
979 }
980
981
982 /*---------------------------------------------------------------------------*\
983
984 FUNCTION....: rxdec_filter()
985 AUTHOR......: David Rowe
986 DATE CREATED: 31 July 2014
987
988 +/- 1000Hz low pass filter, allows us to filter at rate Q to save CPU load.
989
990 \*---------------------------------------------------------------------------*/
991
rxdec_filter(COMP rx_fdm_filter[],COMP rx_fdm[],COMP rxdec_lpf_mem[],int nin)992 void rxdec_filter(COMP rx_fdm_filter[], COMP rx_fdm[], COMP rxdec_lpf_mem[], int nin) {
993 int i,j,k,st;
994
995 for(i=0; i<NRXDECMEM-nin; i++)
996 rxdec_lpf_mem[i] = rxdec_lpf_mem[i+nin];
997 for(i=0, j=NRXDECMEM-nin; i<nin; i++,j++)
998 rxdec_lpf_mem[j] = rx_fdm[i];
999
1000 st = NRXDECMEM - nin - NRXDEC + 1;
1001 for(i=0; i<nin; i++) {
1002 rx_fdm_filter[i].real = 0.0;
1003 for(k=0; k<NRXDEC; k++)
1004 rx_fdm_filter[i].real += rxdec_lpf_mem[st+i+k].real * rxdec_coeff[k];
1005 rx_fdm_filter[i].imag = 0.0;
1006 for(k=0; k<NRXDEC; k++)
1007 rx_fdm_filter[i].imag += rxdec_lpf_mem[st+i+k].imag * rxdec_coeff[k];
1008 }
1009 }
1010
1011 /*---------------------------------------------------------------------------*\
1012
1013 FUNCTION....: fir_filter2()
1014 AUTHOR......: Danilo Beuche
1015 DATE CREATED: August 2016
1016
1017 Ths version submitted by Danilo for the STM32F4 platform. The idea
1018 is to avoid reading the same value from the STM32F4 "slow" flash
1019 twice. 2-4ms of savings per frame were measured by Danilo and the mcHF
1020 team.
1021
1022 \*---------------------------------------------------------------------------*/
1023
fir_filter2(float acc[2],float mem[],const float coeff[],const unsigned int dec_rate)1024 static void fir_filter2(float acc[2], float mem[], const float coeff[], const unsigned int dec_rate) {
1025 acc[0] = 0.0;
1026 acc[1] = 0.0;
1027
1028 float c1,c2,c3,c4,c5,m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,a1,a2;
1029 float* inpCmplx = &mem[0];
1030 const float* coeffPtr = &coeff[0];
1031
1032 int m;
1033
1034 // this manual loop unrolling gives significant boost on STM32 machines
1035 // reduction from avg 3.2ms to 2.4ms in tfdmv.c test
1036 // 5 was the sweet spot, with 6 it took longer again
1037 // and should not harm other, more powerful machines
1038 // no significant difference in output, only rounding (which was to be expected)
1039 // TODO: try to move coeffs to RAM and check if it makes a significant difference
1040 if (NFILTER%(dec_rate*5) == 0) {
1041 for(m=0; m<NFILTER; m+=dec_rate*5) {
1042 c1 = *coeffPtr;
1043
1044 m1 = inpCmplx[0];
1045 m2 = inpCmplx[1];
1046
1047 inpCmplx+= dec_rate*2;
1048 coeffPtr+= dec_rate;
1049
1050 c2 = *coeffPtr;
1051 m3 = inpCmplx[0];
1052 m4 = inpCmplx[1];
1053
1054 inpCmplx+= dec_rate*2;
1055 coeffPtr+= dec_rate;
1056
1057 c3 = *coeffPtr;
1058 m5 = inpCmplx[0];
1059 m6 = inpCmplx[1];
1060
1061 inpCmplx+= dec_rate*2;
1062 coeffPtr+= dec_rate;
1063
1064 c4 = *coeffPtr;
1065 m7 = inpCmplx[0];
1066 m8 = inpCmplx[1];
1067
1068 inpCmplx+= dec_rate*2;
1069 coeffPtr+= dec_rate;
1070
1071 c5 = *coeffPtr;
1072 m9 = inpCmplx[0];
1073 m10 = inpCmplx[1];
1074
1075 inpCmplx+= dec_rate*2;
1076 coeffPtr+= dec_rate;
1077
1078 a1 = c1 * m1 + c2 * m3 + c3 * m5 + c4 * m7 + c5 * m9;
1079 a2 = c1 * m2 + c2 * m4 + c3 * m6 + c4 * m8 + c5 * m10;
1080 acc[0] += a1;
1081 acc[1] += a2;
1082 }
1083 }
1084 else
1085 {
1086 for(m=0; m<NFILTER; m+=dec_rate) {
1087 c1 = *coeffPtr;
1088
1089 m1 = inpCmplx[0];
1090 m2 = inpCmplx[1];
1091
1092 inpCmplx+= dec_rate*2;
1093 coeffPtr+= dec_rate;
1094
1095 a1 = c1 * m1;
1096 a2 = c1 * m2;
1097 acc[0] += a1;
1098 acc[1] += a2;
1099 }
1100 }
1101 acc[0] *= dec_rate;
1102 acc[1] *= dec_rate;
1103 }
1104
1105 /*---------------------------------------------------------------------------*\
1106
1107 FUNCTION....: down_convert_and_rx_filter()
1108 AUTHOR......: David Rowe
1109 DATE CREATED: 30/6/2014
1110
1111 Combined down convert and rx filter, more memory efficient but less
1112 intuitive design.
1113
1114 Depending on the number of input samples to the demod nin, we
1115 produce P-1, P (usually), or P+1 filtered samples at rate P. nin is
1116 occasionally adjusted to compensate for timing slips due to
1117 different tx and rx sample clocks.
1118
1119 \*---------------------------------------------------------------------------*/
1120
1121 /*
1122 TODO: [ ] windback phase calculated once at init time
1123 */
1124
down_convert_and_rx_filter(COMP rx_filt[NC+1][P+1],int Nc,COMP rx_fdm[],COMP rx_fdm_mem[],COMP phase_rx[],COMP freq[],float freq_pol[],int nin,int dec_rate)1125 void down_convert_and_rx_filter(COMP rx_filt[NC+1][P+1], int Nc, COMP rx_fdm[],
1126 COMP rx_fdm_mem[], COMP phase_rx[], COMP freq[],
1127 float freq_pol[], int nin, int dec_rate)
1128 {
1129 int i,k,c,st,Nval;
1130 float windback_phase, mag;
1131 COMP windback_phase_rect;
1132 COMP rx_baseband[NRX_FDM_MEM];
1133 COMP f_rect;
1134
1135 //PROFILE_VAR(windback_start, downconvert_start, filter_start);
1136
1137 /* update memory of rx_fdm */
1138
1139 #if 0
1140 for(i=0; i<NRX_FDM_MEM-nin; i++)
1141 rx_fdm_mem[i] = rx_fdm_mem[i+nin];
1142 for(i=NFILTER+M_FAC-nin, k=0; i<NFILTER+M_FAC; i++, k++)
1143 rx_fdm_mem[i] = rx_fdm[k];
1144 #else
1145 // this gives only 40uS gain on STM32 but now that we have, we keep it
1146 memmove(&rx_fdm_mem[0],&rx_fdm_mem[nin],(NRX_FDM_MEM-nin)*sizeof(COMP));
1147 memcpy(&rx_fdm_mem[NRX_FDM_MEM-nin],&rx_fdm[0],nin*sizeof(COMP));
1148 #endif
1149 for(c=0; c<Nc+1; c++) {
1150
1151 /*
1152
1153 So we have rx_fdm_mem, a baseband array of samples at
1154 rate Fs Hz, including the last nin samples at the end. To
1155 filter each symbol we require the baseband samples for all Nsym
1156 symbols that we filter over. So we need to downconvert the
1157 entire rx_fdm_mem array. To downconvert these we need the LO
1158 phase referenced to the start of the rx_fdm_mem array.
1159
1160
1161 <--------------- Nrx_filt_mem ------->
1162 nin
1163 |--------------------------|---------|
1164 1 |
1165 phase_rx(c)
1166
1167 This means winding phase(c) back from this point
1168 to ensure phase continuity.
1169
1170 */
1171
1172 //PROFILE_SAMPLE(windback_start);
1173 windback_phase = -freq_pol[c]*NFILTER;
1174 windback_phase_rect.real = cosf(windback_phase);
1175 windback_phase_rect.imag = sinf(windback_phase);
1176 phase_rx[c] = cmult(phase_rx[c],windback_phase_rect);
1177 //PROFILE_SAMPLE_AND_LOG(downconvert_start, windback_start, " windback");
1178
1179 /* down convert all samples in buffer */
1180
1181 st = NRX_FDM_MEM-1; /* end of buffer */
1182 st -= nin-1; /* first new sample */
1183 st -= NFILTER; /* first sample used in filtering */
1184
1185 /* freq shift per dec_rate step is dec_rate times original shift */
1186
1187 f_rect = freq[c];
1188 for(i=0; i<dec_rate-1; i++)
1189 f_rect = cmult(f_rect,freq[c]);
1190
1191 for(i=st; i<NRX_FDM_MEM; i+=dec_rate) {
1192 phase_rx[c] = cmult(phase_rx[c], f_rect);
1193 rx_baseband[i] = cmult(rx_fdm_mem[i],cconj(phase_rx[c]));
1194 }
1195 //PROFILE_SAMPLE_AND_LOG(filter_start, downconvert_start, " downconvert");
1196
1197 /* now we can filter this carrier's P symbols */
1198
1199 Nval=M_FAC/P;
1200 for(i=0, k=0; i<nin; i+=Nval, k++) {
1201 #ifdef ORIG
1202 rx_filt[c][k].real = 0.0; rx_filt[c][k].imag = 0.0;
1203
1204 for(m=0; m<NFILTER; m++)
1205 rx_filt[c][k] = cadd(rx_filt[c][k], fcmult(gt_alpha5_root[m], rx_baseband[st+i+m]));
1206 #else
1207 // rx_filt[c][k].real = fir_filter(&rx_baseband[st+i].real, (float*)gt_alpha5_root, dec_rate);
1208 // rx_filt[c][k].imag = fir_filter(&rx_baseband[st+i].imag, (float*)gt_alpha5_root, dec_rate);
1209 fir_filter2(&rx_filt[c][k].real,&rx_baseband[st+i].real, gt_alpha5_root, dec_rate);
1210 #endif
1211 }
1212 //PROFILE_SAMPLE_AND_LOG2(filter_start, " filter");
1213
1214 /* normalise digital oscilators as the magnitude can drift over time */
1215
1216 mag = cabsolute(phase_rx[c]);
1217 phase_rx[c].real /= mag;
1218 phase_rx[c].imag /= mag;
1219
1220 //printf("phase_rx[%d] = %f %f\n", c, phase_rx[c].real, phase_rx[c].imag);
1221 }
1222 }
1223
1224 /*---------------------------------------------------------------------------*\
1225
1226 FUNCTION....: rx_est_timing()
1227 AUTHOR......: David Rowe
1228 DATE CREATED: 23/4/2012
1229
1230 Estimate optimum timing offset, re-filter receive symbols at optimum
1231 timing estimate.
1232
1233 \*---------------------------------------------------------------------------*/
1234
rx_est_timing(COMP rx_symbols[],int Nc,COMP rx_filt[][P+1],COMP rx_filter_mem_timing[][NT * P],float env[],int nin,int m)1235 float rx_est_timing(COMP rx_symbols[],
1236 int Nc,
1237 COMP rx_filt[][P+1],
1238 COMP rx_filter_mem_timing[][NT*P],
1239 float env[],
1240 int nin,
1241 int m)
1242 {
1243 int c,i,j;
1244 int adjust;
1245 COMP x, phase, freq;
1246 float rx_timing, fract, norm_rx_timing;
1247 int low_sample, high_sample;
1248
1249 /*
1250 nin adjust
1251 --------------------------------
1252 120 -1 (one less rate P sample)
1253 160 0 (nominal)
1254 200 1 (one more rate P sample)
1255 */
1256
1257 adjust = P - nin*P/m;
1258
1259 /* update buffer of NT rate P filtered symbols */
1260
1261 for(c=0; c<Nc+1; c++)
1262 for(i=0,j=P-adjust; i<(NT-1)*P+adjust; i++,j++)
1263 rx_filter_mem_timing[c][i] = rx_filter_mem_timing[c][j];
1264 for(c=0; c<Nc+1; c++)
1265 for(i=(NT-1)*P+adjust,j=0; i<NT*P; i++,j++)
1266 rx_filter_mem_timing[c][i] = rx_filt[c][j];
1267
1268 /* sum envelopes of all carriers */
1269
1270 for(i=0; i<NT*P; i++) {
1271 env[i] = 0.0;
1272 for(c=0; c<Nc+1; c++)
1273 env[i] += cabsolute(rx_filter_mem_timing[c][i]);
1274 }
1275
1276 /* The envelope has a frequency component at the symbol rate. The
1277 phase of this frequency component indicates the timing. So work
1278 out single DFT at frequency 2*pi/P */
1279
1280 x.real = 0.0; x.imag = 0.0;
1281 freq.real = cosf(2*PI/P);
1282 freq.imag = sinf(2*PI/P);
1283 phase.real = 1.0;
1284 phase.imag = 0.0;
1285
1286 for(i=0; i<NT*P; i++) {
1287 x = cadd(x, fcmult(env[i], phase));
1288 phase = cmult(phase, freq);
1289 }
1290
1291 /* Map phase to estimated optimum timing instant at rate P. The
1292 P/4 part was adjusted by experiment, I know not why.... */
1293
1294 norm_rx_timing = atan2f(x.imag, x.real)/(2*PI);
1295 assert(fabsf(norm_rx_timing) < 1.0);
1296 rx_timing = norm_rx_timing*P + P/4;
1297
1298 if (rx_timing > P)
1299 rx_timing -= P;
1300 if (rx_timing < -P)
1301 rx_timing += P;
1302
1303 /* rx_filter_mem_timing contains Nt*P samples (Nt symbols at rate
1304 P), where Nt is odd. Lets use linear interpolation to resample
1305 in the centre of the timing estimation window .*/
1306
1307 rx_timing += floorf(NT/2.0)*P;
1308 low_sample = floorf(rx_timing);
1309 fract = rx_timing - low_sample;
1310 high_sample = ceilf(rx_timing);
1311
1312 //printf("rx_timing: %f low_sample: %d high_sample: %d fract: %f\n", rx_timing, low_sample, high_sample, fract);
1313
1314 for(c=0; c<Nc+1; c++) {
1315 rx_symbols[c] = cadd(fcmult(1.0-fract, rx_filter_mem_timing[c][low_sample-1]), fcmult(fract, rx_filter_mem_timing[c][high_sample-1]));
1316 //rx_symbols[c] = rx_filter_mem_timing[c][high_sample];
1317 }
1318
1319 /* This value will be +/- half a symbol so will wrap around at +/-
1320 M/2 or +/- 80 samples with M=160 */
1321
1322 return norm_rx_timing*m;
1323 }
1324
1325 /*---------------------------------------------------------------------------*\
1326
1327 FUNCTION....: qpsk_to_bits()
1328 AUTHOR......: David Rowe
1329 DATE CREATED: 24/4/2012
1330
1331 Convert DQPSK symbols back to an array of bits, extracts sync bit
1332 from DBPSK pilot, and also uses pilot to estimate fine frequency
1333 error.
1334
1335 \*---------------------------------------------------------------------------*/
1336
qpsk_to_bits(int rx_bits[],int * sync_bit,int Nc,COMP phase_difference[],COMP prev_rx_symbols[],COMP rx_symbols[],int old_qpsk_mapping)1337 float qpsk_to_bits(int rx_bits[], int *sync_bit, int Nc, COMP phase_difference[], COMP prev_rx_symbols[],
1338 COMP rx_symbols[], int old_qpsk_mapping)
1339 {
1340 int c;
1341 COMP d;
1342 int msb=0, lsb=0;
1343 float ferr, norm;
1344
1345
1346 /* Extra 45 degree clockwise lets us use real and imag axis as
1347 decision boundaries. "norm" makes sure the phase subtraction
1348 from the previous symbol doesn't affect the amplitude, which
1349 leads to sensible scatter plots */
1350
1351 for(c=0; c<Nc; c++) {
1352 norm = 1.0/(cabsolute(prev_rx_symbols[c])+1E-6);
1353 phase_difference[c] = cmult(cmult(rx_symbols[c], fcmult(norm,cconj(prev_rx_symbols[c]))), pi_on_4);
1354 }
1355
1356 /* map (Nc,1) DQPSK symbols back into an (1,Nc*Nb) array of bits */
1357
1358 for (c=0; c<Nc; c++) {
1359 d = phase_difference[c];
1360 if ((d.real >= 0) && (d.imag >= 0)) {
1361 msb = 0; lsb = 0;
1362 }
1363 if ((d.real < 0) && (d.imag >= 0)) {
1364 msb = 0; lsb = 1;
1365 }
1366 if ((d.real < 0) && (d.imag < 0)) {
1367 if (old_qpsk_mapping) {
1368 msb = 1; lsb = 0;
1369 } else {
1370 msb = 1; lsb = 1;
1371 }
1372 }
1373 if ((d.real >= 0) && (d.imag < 0)) {
1374 if (old_qpsk_mapping) {
1375 msb = 1; lsb = 1;
1376 } else {
1377 msb = 1; lsb = 0;
1378 }
1379 }
1380 rx_bits[2*c] = msb;
1381 rx_bits[2*c+1] = lsb;
1382 }
1383
1384 /* Extract DBPSK encoded Sync bit and fine freq offset estimate */
1385
1386 norm = 1.0/(cabsolute(prev_rx_symbols[Nc])+1E-6);
1387 phase_difference[Nc] = cmult(rx_symbols[Nc], fcmult(norm, cconj(prev_rx_symbols[Nc])));
1388 if (phase_difference[Nc].real < 0) {
1389 *sync_bit = 1;
1390 ferr = phase_difference[Nc].imag*norm; /* make f_err magnitude insensitive */
1391 }
1392 else {
1393 *sync_bit = 0;
1394 ferr = -phase_difference[Nc].imag*norm;
1395 }
1396
1397 /* pilot carrier gets an extra pi/4 rotation to make it consistent
1398 with other carriers, as we need it for snr_update and scatter
1399 diagram */
1400
1401 phase_difference[Nc] = cmult(phase_difference[Nc], pi_on_4);
1402
1403 return ferr;
1404 }
1405
1406 /*---------------------------------------------------------------------------*\
1407
1408 FUNCTION....: snr_update()
1409 AUTHOR......: David Rowe
1410 DATE CREATED: 17 May 2012
1411
1412 Given phase differences update estimates of signal and noise levels.
1413
1414 \*---------------------------------------------------------------------------*/
1415
snr_update(float sig_est[],float noise_est[],int Nc,COMP phase_difference[])1416 void snr_update(float sig_est[], float noise_est[], int Nc, COMP phase_difference[])
1417 {
1418 float s[NC+1];
1419 COMP refl_symbols[NC+1];
1420 float n[NC+1];
1421 int c;
1422
1423
1424 /* mag of each symbol is distance from origin, this gives us a
1425 vector of mags, one for each carrier. */
1426
1427 for(c=0; c<Nc+1; c++)
1428 s[c] = cabsolute(phase_difference[c]);
1429
1430 /* signal mag estimate for each carrier is a smoothed version of
1431 instantaneous magntitude, this gives us a vector of smoothed
1432 mag estimates, one for each carrier. */
1433
1434 for(c=0; c<Nc+1; c++)
1435 sig_est[c] = SNR_COEFF*sig_est[c] + (1.0 - SNR_COEFF)*s[c];
1436
1437 /* noise mag estimate is distance of current symbol from average
1438 location of that symbol. We reflect all symbols into the first
1439 quadrant for convenience. */
1440
1441 for(c=0; c<Nc+1; c++) {
1442 refl_symbols[c].real = fabsf(phase_difference[c].real);
1443 refl_symbols[c].imag = fabsf(phase_difference[c].imag);
1444 n[c] = cabsolute(cadd(fcmult(sig_est[c], pi_on_4), cneg(refl_symbols[c])));
1445 }
1446
1447 /* noise mag estimate for each carrier is a smoothed version of
1448 instantaneous noise mag, this gives us a vector of smoothed
1449 noise power estimates, one for each carrier. */
1450
1451 for(c=0; c<Nc+1; c++)
1452 noise_est[c] = SNR_COEFF*noise_est[c] + (1 - SNR_COEFF)*n[c];
1453 }
1454
1455 // returns number of shorts in error_pattern[], one short per error
1456
fdmdv_error_pattern_size(struct FDMDV * f)1457 int fdmdv_error_pattern_size(struct FDMDV *f) {
1458 return f->ntest_bits;
1459 }
1460
1461 /*---------------------------------------------------------------------------*\
1462
1463 FUNCTION....: fdmdv_put_test_bits()
1464 AUTHOR......: David Rowe
1465 DATE CREATED: 24/4/2012
1466
1467 Accepts nbits from rx and attempts to sync with test_bits sequence.
1468 If sync OK measures bit errors.
1469
1470 \*---------------------------------------------------------------------------*/
1471
fdmdv_put_test_bits(struct FDMDV * f,int * sync,short error_pattern[],int * bit_errors,int * ntest_bits,int rx_bits[])1472 void fdmdv_put_test_bits(struct FDMDV *f, int *sync, short error_pattern[],
1473 int *bit_errors, int *ntest_bits, int rx_bits[])
1474 {
1475 int i,j;
1476 float ber;
1477 int bits_per_frame = fdmdv_bits_per_frame(f);
1478
1479 /* Append to our memory */
1480
1481 for(i=0,j=bits_per_frame; i<f->ntest_bits-bits_per_frame; i++,j++)
1482 f->rx_test_bits_mem[i] = f->rx_test_bits_mem[j];
1483 for(i=f->ntest_bits-bits_per_frame,j=0; i<f->ntest_bits; i++,j++)
1484 f->rx_test_bits_mem[i] = rx_bits[j];
1485
1486 /* see how many bit errors we get when checked against test sequence */
1487
1488 *bit_errors = 0;
1489 for(i=0; i<f->ntest_bits; i++) {
1490 error_pattern[i] = test_bits[i] ^ f->rx_test_bits_mem[i];
1491 *bit_errors += error_pattern[i];
1492 //printf("%d %d %d %d\n", i, test_bits[i], f->rx_test_bits_mem[i], test_bits[i] ^ f->rx_test_bits_mem[i]);
1493 }
1494
1495 /* if less than a thresh we are aligned and in sync with test sequence */
1496
1497 ber = (float)*bit_errors/f->ntest_bits;
1498
1499 *sync = 0;
1500 if (ber < 0.2)
1501 *sync = 1;
1502
1503 *ntest_bits = f->ntest_bits;
1504
1505 }
1506
1507 /*---------------------------------------------------------------------------*\
1508
1509 FUNCTION....: freq_state(()
1510 AUTHOR......: David Rowe
1511 DATE CREATED: 24/4/2012
1512
1513 Freq offset state machine. Moves between coarse and fine states
1514 based on BPSK pilot sequence. Freq offset estimator occasionally
1515 makes mistakes when used continuously. So we use it until we have
1516 acquired the BPSK pilot, then switch to a more robust "fine"
1517 tracking algorithm. If we lose sync we switch back to coarse mode
1518 for fast re-acquisition of large frequency offsets.
1519
1520 The sync state is also useful for higher layers to determine when
1521 there is valid FDMDV data for decoding. We want to reliably and
1522 quickly get into sync, stay in sync even on fading channels, and
1523 fall out of sync quickly if tx stops or it's a false sync.
1524
1525 In multipath fading channels the BPSK sync carrier may be pushed
1526 down in the noise, despite other carriers being at full strength.
1527 We want to avoid loss of sync in these cases.
1528
1529 \*---------------------------------------------------------------------------*/
1530
freq_state(int * reliable_sync_bit,int sync_bit,int * state,int * timer,int * sync_mem)1531 int freq_state(int *reliable_sync_bit, int sync_bit, int *state, int *timer, int *sync_mem)
1532 {
1533 int next_state, sync, unique_word, i, corr;
1534
1535 /* look for 6 symbols (120ms) 101010 of sync sequence */
1536
1537 unique_word = 0;
1538 for(i=0; i<NSYNC_MEM-1; i++)
1539 sync_mem[i] = sync_mem[i+1];
1540 sync_mem[i] = 1 - 2*sync_bit;
1541 corr = 0;
1542 for(i=0; i<NSYNC_MEM; i++)
1543 corr += sync_mem[i]*sync_uw[i];
1544 if (abs(corr) == NSYNC_MEM)
1545 unique_word = 1;
1546 *reliable_sync_bit = (corr == NSYNC_MEM);
1547
1548 /* iterate state machine */
1549
1550 next_state = *state;
1551 switch(*state) {
1552 case 0:
1553 if (unique_word) {
1554 next_state = 1;
1555 *timer = 0;
1556 }
1557 break;
1558 case 1: /* tentative sync state */
1559 if (unique_word) {
1560 (*timer)++;
1561 if (*timer == 25) /* sync has been good for 500ms */
1562 next_state = 2;
1563 }
1564 else
1565 next_state = 0; /* quickly fall out of sync */
1566 break;
1567 case 2: /* good sync state */
1568 if (unique_word == 0) {
1569 *timer = 0;
1570 next_state = 3;
1571 }
1572 break;
1573 case 3: /* tentative bad state, but could be a fade */
1574 if (unique_word)
1575 next_state = 2;
1576 else {
1577 (*timer)++;
1578 if (*timer == 50) /* wait for 1000ms in case sync comes back */
1579 next_state = 0;
1580 }
1581 break;
1582 }
1583
1584 //printf("state: %d next_state: %d uw: %d timer: %d\n", *state, next_state, unique_word, *timer);
1585 *state = next_state;
1586 if (*state)
1587 sync = 1;
1588 else
1589 sync = 0;
1590
1591 return sync;
1592 }
1593
1594 /*---------------------------------------------------------------------------*\
1595
1596 FUNCTION....: fdmdv_demod()
1597 AUTHOR......: David Rowe
1598 DATE CREATED: 26/4/2012
1599
1600 FDMDV demodulator, take an array of FDMDV_SAMPLES_PER_FRAME
1601 modulated samples, returns an array of FDMDV_BITS_PER_FRAME bits,
1602 plus the sync bit.
1603
1604 The input signal is complex to support single sided frequency shifting
1605 before the demod input (e.g. fdmdv2 click to tune feature).
1606
1607 The number of input samples nin will normally be M_FAC ==
1608 FDMDV_SAMPLES_PER_FRAME. However to adjust for differences in
1609 transmit and receive sample clocks nin will occasionally be M_FAC-M_FAC/P,
1610 or M_FAC+M_FAC/P.
1611
1612 \*---------------------------------------------------------------------------*/
1613
1614
fdmdv_demod(struct FDMDV * fdmdv,int rx_bits[],int * reliable_sync_bit,COMP rx_fdm[],int * nin)1615 void fdmdv_demod(struct FDMDV *fdmdv, int rx_bits[],
1616 int *reliable_sync_bit, COMP rx_fdm[], int *nin)
1617 {
1618 float foff_coarse, foff_fine;
1619 COMP rx_fdm_fcorr[M_FAC+M_FAC/P];
1620 COMP rx_fdm_filter[M_FAC+M_FAC/P];
1621 COMP rx_fdm_bb[M_FAC+M_FAC/P];
1622 COMP rx_filt[NC+1][P+1];
1623 COMP rx_symbols[NC+1];
1624 float env[NT*P];
1625 int sync_bit;
1626 PROFILE_VAR(demod_start, fdmdv_freq_shift_start, down_convert_and_rx_filter_start);
1627 PROFILE_VAR(rx_est_timing_start, qpsk_to_bits_start, snr_update_start, freq_state_start);
1628
1629 /* shift down to complex baseband */
1630
1631 fdmdv_freq_shift(rx_fdm_bb, rx_fdm, -FDMDV_FCENTRE, &fdmdv->fbb_phase_rx, *nin);
1632
1633 /* freq offset estimation and correction */
1634
1635 PROFILE_SAMPLE(demod_start);
1636 foff_coarse = rx_est_freq_offset(fdmdv, rx_fdm_bb, *nin, !fdmdv->sync);
1637 PROFILE_SAMPLE_AND_LOG(fdmdv_freq_shift_start, demod_start, " rx_est_freq_offset");
1638
1639 if (fdmdv->sync == 0)
1640 fdmdv->foff = foff_coarse;
1641 fdmdv_freq_shift(rx_fdm_fcorr, rx_fdm_bb, -fdmdv->foff, &fdmdv->foff_phase_rect, *nin);
1642 PROFILE_SAMPLE_AND_LOG(down_convert_and_rx_filter_start, fdmdv_freq_shift_start, " fdmdv_freq_shift");
1643
1644 /* baseband processing */
1645
1646 rxdec_filter(rx_fdm_filter, rx_fdm_fcorr, fdmdv->rxdec_lpf_mem, *nin);
1647 down_convert_and_rx_filter(rx_filt, fdmdv->Nc, rx_fdm_filter, fdmdv->rx_fdm_mem, fdmdv->phase_rx, fdmdv->freq,
1648 fdmdv->freq_pol, *nin, M_FAC/Q);
1649 PROFILE_SAMPLE_AND_LOG(rx_est_timing_start, down_convert_and_rx_filter_start, " down_convert_and_rx_filter");
1650 fdmdv->rx_timing = rx_est_timing(rx_symbols, fdmdv->Nc, rx_filt, fdmdv->rx_filter_mem_timing, env, *nin, M_FAC);
1651 PROFILE_SAMPLE_AND_LOG(qpsk_to_bits_start, rx_est_timing_start, " rx_est_timing");
1652
1653 /* Adjust number of input samples to keep timing within bounds */
1654
1655 *nin = M_FAC;
1656
1657 if (fdmdv->rx_timing > M_FAC/P)
1658 *nin += M_FAC/P;
1659
1660 if (fdmdv->rx_timing < -M_FAC/P)
1661 *nin -= M_FAC/P;
1662
1663 foff_fine = qpsk_to_bits(rx_bits, &sync_bit, fdmdv->Nc, fdmdv->phase_difference, fdmdv->prev_rx_symbols, rx_symbols,
1664 fdmdv->old_qpsk_mapping);
1665 memcpy(fdmdv->prev_rx_symbols, rx_symbols, sizeof(COMP)*(fdmdv->Nc+1));
1666 PROFILE_SAMPLE_AND_LOG(snr_update_start, qpsk_to_bits_start, " qpsk_to_bits");
1667 snr_update(fdmdv->sig_est, fdmdv->noise_est, fdmdv->Nc, fdmdv->phase_difference);
1668 PROFILE_SAMPLE_AND_LOG(freq_state_start, snr_update_start, " snr_update");
1669
1670 /* freq offset estimation state machine */
1671
1672 fdmdv->sync = freq_state(reliable_sync_bit, sync_bit, &fdmdv->fest_state, &fdmdv->timer, fdmdv->sync_mem);
1673 PROFILE_SAMPLE_AND_LOG2(freq_state_start, " freq_state");
1674 fdmdv->foff -= TRACK_COEFF*foff_fine;
1675 }
1676
1677 /*---------------------------------------------------------------------------*\
1678
1679 FUNCTION....: calc_snr()
1680 AUTHOR......: David Rowe
1681 DATE CREATED: 17 May 2012
1682
1683 Calculate current SNR estimate (3000Hz noise BW)
1684
1685 \*---------------------------------------------------------------------------*/
1686
calc_snr(int Nc,float sig_est[],float noise_est[])1687 float calc_snr(int Nc, float sig_est[], float noise_est[])
1688 {
1689 float S, SdB;
1690 float mean, N50, N50dB, N3000dB;
1691 float snr_dB;
1692 int c;
1693
1694 S = 0.0;
1695 for(c=0; c<Nc+1; c++) {
1696 S += sig_est[c] * sig_est[c];
1697 }
1698 SdB = 10.0*log10f(S+1E-12);
1699
1700 /* Average noise mag across all carriers and square to get an
1701 average noise power. This is an estimate of the noise power in
1702 Rs = 50Hz of BW (note for raised root cosine filters Rs is the
1703 noise BW of the filter) */
1704
1705 mean = 0.0;
1706 for(c=0; c<Nc+1; c++)
1707 mean += noise_est[c];
1708 mean /= (Nc+1);
1709 N50 = mean * mean;
1710 N50dB = 10.0*log10f(N50+1E-12);
1711
1712 /* Now multiply by (3000 Hz)/(50 Hz) to find the total noise power
1713 in 3000 Hz */
1714
1715 N3000dB = N50dB + 10.0*log10f(3000.0/RS);
1716
1717 snr_dB = SdB - N3000dB;
1718
1719 return snr_dB;
1720 }
1721
1722 /*---------------------------------------------------------------------------*\
1723
1724 FUNCTION....: fdmdv_get_demod_stats()
1725 AUTHOR......: David Rowe
1726 DATE CREATED: 1 May 2012
1727
1728 Fills stats structure with a bunch of demod information.
1729
1730 \*---------------------------------------------------------------------------*/
1731
fdmdv_get_demod_stats(struct FDMDV * fdmdv,struct MODEM_STATS * stats)1732 void fdmdv_get_demod_stats(struct FDMDV *fdmdv, struct MODEM_STATS *stats)
1733 {
1734 assert(fdmdv->Nc <= MODEM_STATS_NC_MAX);
1735
1736 stats->Nc = fdmdv->Nc;
1737 stats->snr_est = calc_snr(fdmdv->Nc, fdmdv->sig_est, fdmdv->noise_est);
1738 stats->sync = fdmdv->sync;
1739 stats->foff = fdmdv->foff;
1740 stats->rx_timing = fdmdv->rx_timing;
1741 stats->clock_offset = 0.0; /* TODO - implement clock offset estimation */
1742
1743 #ifndef __EMBEDDED__
1744 stats->nr = 1;
1745 for(int c=0; c<fdmdv->Nc+1; c++) {
1746 stats->rx_symbols[0][c] = fdmdv->phase_difference[c];
1747 }
1748 #endif
1749 }
1750
1751 /*---------------------------------------------------------------------------*\
1752
1753 FUNCTION....: fdmdv_8_to_16()
1754 AUTHOR......: David Rowe
1755 DATE CREATED: 9 May 2012
1756
1757 Changes the sample rate of a signal from 8 to 16 kHz. Support function for
1758 SM1000.
1759
1760 \*---------------------------------------------------------------------------*/
1761
fdmdv_8_to_16(float out16k[],float in8k[],int n8k)1762 void fdmdv_8_to_16(float out16k[], float in8k[], int n8k)
1763 {
1764 int i,k,l;
1765 float acc;
1766
1767 /* this version unrolled for specific FDMDV_OS */
1768
1769 assert(FDMDV_OS == 2);
1770
1771 for(i=0; i<n8k; i++) {
1772 acc = 0.0;
1773 for(k=0,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++)
1774 acc += fdmdv_os_filter[k]*in8k[i-l];
1775 out16k[i*FDMDV_OS] = FDMDV_OS*acc;
1776
1777 acc = 0.0;
1778 for(k=1,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++)
1779 acc += fdmdv_os_filter[k]*in8k[i-l];
1780 out16k[i*FDMDV_OS+1] = FDMDV_OS*acc;
1781 }
1782
1783 /* update filter memory */
1784
1785 for(i=-(FDMDV_OS_TAPS_8K); i<0; i++)
1786 in8k[i] = in8k[i + n8k];
1787
1788 }
1789
fdmdv_8_to_16_short(short out16k[],short in8k[],int n8k)1790 void fdmdv_8_to_16_short(short out16k[], short in8k[], int n8k)
1791 {
1792 int i,k,l;
1793 float acc;
1794
1795 /* this version unrolled for specific FDMDV_OS */
1796
1797 assert(FDMDV_OS == 2);
1798
1799 for(i=0; i<n8k; i++) {
1800 acc = 0.0;
1801 for(k=0,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++)
1802 acc += fdmdv_os_filter[k]*(float)in8k[i-l];
1803 out16k[i*FDMDV_OS] = FDMDV_OS*acc;
1804
1805 acc = 0.0;
1806 for(k=1,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++)
1807 acc += fdmdv_os_filter[k]*(float)in8k[i-l];
1808 out16k[i*FDMDV_OS+1] = FDMDV_OS*acc;
1809 }
1810
1811 /* update filter memory */
1812
1813 for(i=-(FDMDV_OS_TAPS_8K); i<0; i++)
1814 in8k[i] = in8k[i + n8k];
1815
1816 }
1817
1818 /*---------------------------------------------------------------------------*\
1819
1820 FUNCTION....: fdmdv_16_to_8()
1821 AUTHOR......: David Rowe
1822 DATE CREATED: 9 May 2012
1823
1824 Changes the sample rate of a signal from 16 to 8 kHz.
1825
1826 n is the number of samples at the 8 kHz rate, there are FDMDV_OS*n
1827 samples at the 16 kHz rate. As above however a memory of
1828 FDMDV_OS_TAPS samples is reqd for in16k[] (see t16_8.c unit test as example).
1829
1830 Low pass filter the 16 kHz signal at 4 kHz using the same filter as
1831 the upsampler, then just output every FDMDV_OS-th filtered sample.
1832
1833 \*---------------------------------------------------------------------------*/
1834
fdmdv_16_to_8(float out8k[],float in16k[],int n)1835 void fdmdv_16_to_8(float out8k[], float in16k[], int n)
1836 {
1837 float acc;
1838 int i,j,k;
1839
1840 for(i=0, k=0; k<n; i+=FDMDV_OS, k++) {
1841 acc = 0.0;
1842 for(j=0; j<FDMDV_OS_TAPS_16K; j++)
1843 acc += fdmdv_os_filter[j]*in16k[i-j];
1844 out8k[k] = acc;
1845 }
1846
1847 /* update filter memory */
1848
1849 for(i=-FDMDV_OS_TAPS_16K; i<0; i++)
1850 in16k[i] = in16k[i + n*FDMDV_OS];
1851 }
1852
fdmdv_16_to_8_short(short out8k[],short in16k[],int n)1853 void fdmdv_16_to_8_short(short out8k[], short in16k[], int n)
1854 {
1855 float acc;
1856 int i,j,k;
1857
1858 for(i=0, k=0; k<n; i+=FDMDV_OS, k++) {
1859 acc = 0.0;
1860 for(j=0; j<FDMDV_OS_TAPS_16K; j++)
1861 acc += fdmdv_os_filter[j]*(float)in16k[i-j];
1862 out8k[k] = acc;
1863 }
1864
1865 /* update filter memory */
1866
1867 for(i=-FDMDV_OS_TAPS_16K; i<0; i++)
1868 in16k[i] = in16k[i + n*FDMDV_OS];
1869 }
1870
1871
1872 /*---------------------------------------------------------------------------*\
1873
1874 FUNCTION....: fdmdv_8_to_48()
1875 AUTHOR......: David Rowe
1876 DATE CREATED: 9 May 2012
1877
1878 Changes the sample rate of a signal from 8 to 48 kHz.
1879
1880 n is the number of samples at the 8 kHz rate, there are FDMDV_OS*n samples
1881 at the 48 kHz rate. A memory of FDMDV_OS_TAPS_48/FDMDV_OS samples is reqd for
1882 in8k[] (see t48_8.c unit test as example).
1883
1884 \*---------------------------------------------------------------------------*/
1885
fdmdv_8_to_48(float out48k[],float in8k[],int n)1886 void fdmdv_8_to_48(float out48k[], float in8k[], int n)
1887 {
1888 int i,j,k,l;
1889
1890 for(i=0; i<n; i++) {
1891 for(j=0; j<FDMDV_OS_48; j++) {
1892 out48k[i*FDMDV_OS_48+j] = 0.0;
1893 for(k=0,l=0; k<FDMDV_OS_TAPS_48K; k+=FDMDV_OS_48,l++)
1894 out48k[i*FDMDV_OS_48+j] += fdmdv_os_filter48[k+j]*in8k[i-l];
1895 out48k[i*FDMDV_OS_48+j] *= FDMDV_OS_48;
1896
1897 }
1898 }
1899
1900 /* update filter memory */
1901
1902 for(i=-FDMDV_OS_TAPS_48_8K; i<0; i++)
1903 in8k[i] = in8k[i + n];
1904 }
1905
fdmdv_8_to_48_short(short out48k[],short in8k[],int n)1906 void fdmdv_8_to_48_short(short out48k[], short in8k[], int n)
1907 {
1908 int i,j,k,l;
1909 float acc;
1910
1911 for(i=0; i<n; i++) {
1912 for(j=0; j<FDMDV_OS_48; j++) {
1913 acc = 0.0;
1914 for(k=0,l=0; k<FDMDV_OS_TAPS_48K; k+=FDMDV_OS_48,l++)
1915 acc += fdmdv_os_filter48[k+j]*in8k[i-l];
1916 out48k[i*FDMDV_OS_48+j] = acc*FDMDV_OS_48;
1917 }
1918 }
1919
1920 /* update filter memory */
1921
1922 for(i=-FDMDV_OS_TAPS_48_8K; i<0; i++)
1923 in8k[i] = in8k[i + n];
1924 }
1925
1926 /*---------------------------------------------------------------------------*\
1927
1928 FUNCTION....: fdmdv_48_to_8()
1929 AUTHOR......: David Rowe
1930 DATE CREATED: 9 May 2012
1931
1932 Changes the sample rate of a signal from 48 to 8 kHz.
1933
1934 n is the number of samples at the 8 kHz rate, there are FDMDV_OS_48*n
1935 samples at the 48 kHz rate. As above however a memory of
1936 FDMDV_OS_TAPS_48 samples is reqd for in48k[] (see t48_8.c unit test as example).
1937
1938 \*---------------------------------------------------------------------------*/
1939
fdmdv_48_to_8(float out8k[],float in48k[],int n)1940 void fdmdv_48_to_8(float out8k[], float in48k[], int n)
1941 {
1942 int i,j;
1943
1944 for(i=0; i<n; i++) {
1945 out8k[i] = 0.0;
1946 for(j=0; j<FDMDV_OS_TAPS_48K; j++)
1947 out8k[i] += fdmdv_os_filter48[j]*in48k[i*FDMDV_OS_48-j];
1948 }
1949
1950 /* update filter memory */
1951
1952 for(i=-FDMDV_OS_TAPS_48K; i<0; i++)
1953 in48k[i] = in48k[i + n*FDMDV_OS_48];
1954 }
1955
fdmdv_48_to_8_short(short out8k[],short in48k[],int n)1956 void fdmdv_48_to_8_short(short out8k[], short in48k[], int n)
1957 {
1958 int i,j;
1959 float acc;
1960
1961 for(i=0; i<n; i++) {
1962 acc = 0.0;
1963 for(j=0; j<FDMDV_OS_TAPS_48K; j++)
1964 acc += fdmdv_os_filter48[j]*in48k[i*FDMDV_OS_48-j];
1965 out8k[i] = acc;
1966 }
1967
1968 /* update filter memory */
1969
1970 for(i=-FDMDV_OS_TAPS_48K; i<0; i++)
1971 in48k[i] = in48k[i + n*FDMDV_OS_48];
1972 }
1973
1974 /*---------------------------------------------------------------------------*\
1975
1976 Function used during development to test if magnitude of digital
1977 oscillators was drifting. It was!
1978
1979 \*---------------------------------------------------------------------------*/
1980
fdmdv_dump_osc_mags(struct FDMDV * f)1981 void fdmdv_dump_osc_mags(struct FDMDV *f)
1982 {
1983 int i;
1984
1985 fprintf(stderr, "phase_tx[]:\n");
1986 for(i=0; i<=f->Nc; i++)
1987 fprintf(stderr," %1.3f", (double)cabsolute(f->phase_tx[i]));
1988 fprintf(stderr,"\nfreq[]:\n");
1989 for(i=0; i<=f->Nc; i++)
1990 fprintf(stderr," %1.3f", (double)cabsolute(f->freq[i]));
1991 fprintf(stderr,"\nfoff_phase_rect: %1.3f", (double)cabsolute(f->foff_phase_rect));
1992 fprintf(stderr,"\nphase_rx[]:\n");
1993 for(i=0; i<=f->Nc; i++)
1994 fprintf(stderr," %1.3f", (double)cabsolute(f->phase_rx[i]));
1995 fprintf(stderr, "\n\n");
1996 }
1997
1998
1999 /*---------------------------------------------------------------------------*\
2000
2001 FUNCTION....: randn()
2002 AUTHOR......: David Rowe
2003 DATE CREATED: 2 August 2014
2004
2005 Simple approximation to normal (gaussian) random number generator
2006 with 0 mean and unit variance.
2007
2008 \*---------------------------------------------------------------------------*/
2009
2010 #define RANDN_IT 12 /* This magic number of iterations gives us a
2011 unit variance. I think beacuse var =
2012 (b-a)^2/12 for one uniform random variable, so
2013 for a sum of n random variables it's
2014 n(b-a)^2/12, or for b=1, a = 0, n=12, we get
2015 var = 12(1-0)^2/12 = 1 */
2016
randn()2017 static float randn() {
2018 int i;
2019 float rn = 0.0;
2020
2021 for(i=0; i<RANDN_IT; i++)
2022 rn += (float)rand()/RAND_MAX;
2023
2024 rn -= (float)RANDN_IT/2.0;
2025 return rn;
2026 }
2027
2028
2029 /*---------------------------------------------------------------------------*\
2030
2031 FUNCTION....: fdmdv_simulate_channel()
2032 AUTHOR......: David Rowe
2033 DATE CREATED: 10 July 2014
2034
2035 Simple channel simulation function to aid in testing. Target SNR
2036 uses noise measured in a 3 kHz bandwidth.
2037
2038 Doesn't use fdmdv states so can be called from anywhere, e.g. non
2039 fdmdv applications.
2040
2041 TODO: Measured SNR is coming out a few dB higher than target_snr, this
2042 needs to be fixed.
2043
2044 \*---------------------------------------------------------------------------*/
2045
fdmdv_simulate_channel(float * sig_pwr_av,COMP samples[],int nin,float target_snr)2046 void fdmdv_simulate_channel(float *sig_pwr_av, COMP samples[], int nin, float target_snr)
2047 {
2048 float sig_pwr, target_snr_linear, noise_pwr, noise_pwr_1Hz, noise_pwr_4000Hz, noise_gain;
2049 int i;
2050
2051 /* prevent NAN when we divide by nin below */
2052 if (nin == 0) return;
2053
2054 /* estimate signal power */
2055
2056 sig_pwr = 0.0;
2057 for(i=0; i<nin; i++)
2058 sig_pwr += samples[i].real*samples[i].real + samples[i].imag*samples[i].imag;
2059
2060 sig_pwr /= nin;
2061
2062 *sig_pwr_av = 0.9**sig_pwr_av + 0.1*sig_pwr;
2063
2064 /* det noise to meet target SNR */
2065
2066 target_snr_linear = POW10F(target_snr/10.0);
2067 noise_pwr = *sig_pwr_av/target_snr_linear; /* noise pwr in a 3000 Hz BW */
2068 noise_pwr_1Hz = noise_pwr/3000.0; /* noise pwr in a 1 Hz bandwidth */
2069 noise_pwr_4000Hz = noise_pwr_1Hz*4000.0; /* noise pwr in a 4000 Hz BW, which
2070 due to fs=8000 Hz in our simulation noise BW */
2071
2072 noise_gain = sqrtf(0.5*noise_pwr_4000Hz); /* split noise pwr between real and imag sides */
2073
2074 for(i=0; i<nin; i++) {
2075 samples[i].real += noise_gain*randn();
2076 samples[i].imag += noise_gain*randn();
2077 }
2078 /*
2079 fprintf(stderr, "sig_pwr: %f f->sig_pwr_av: %e target_snr_linear: %f noise_pwr_4000Hz: %e noise_gain: %e\n",
2080 sig_pwr, *sig_pwr_av, target_snr_linear, noise_pwr_4000Hz, noise_gain);
2081 */
2082 }
2083