diff options
Diffstat (limited to 'src/fdmdv.c')
| -rw-r--r-- | src/fdmdv.c | 2615 |
1 files changed, 1290 insertions, 1325 deletions
diff --git a/src/fdmdv.c b/src/fdmdv.c index 7aeb4b8..2a1c4a4 100644 --- a/src/fdmdv.c +++ b/src/fdmdv.c @@ -32,29 +32,28 @@ \*---------------------------------------------------------------------------*/ #include <assert.h> -#include <stdlib.h> +#include <math.h> #include <stdio.h> +#include <stdlib.h> #include <string.h> -#include <math.h> -#include "fdmdv_internal.h" #include "codec2_fdmdv.h" +#include "codec2_fft.h" #include "comp_prim.h" +#include "debug_alloc.h" +#include "fdmdv_internal.h" +#include "hanning.h" +#include "machdep.h" +#include "os.h" +#include "pilot_coeff.h" #include "rn.h" #include "rxdec_coeff.h" #include "test_bits.h" -#include "pilot_coeff.h" -#include "codec2_fft.h" -#include "hanning.h" -#include "os.h" -#include "machdep.h" - -#include "debug_alloc.h" -static int sync_uw[] = {1,-1,1,-1,1,-1}; - -static const COMP pi_on_4 = { .70710678118654752439, .70710678118654752439 }; // cosf(PI/4) , sinf(PI/4) +static int sync_uw[] = {1, -1, 1, -1, 1, -1}; +static const COMP pi_on_4 = {.70710678118654752439, + .70710678118654752439}; // cosf(PI/4) , sinf(PI/4) /*--------------------------------------------------------------------------* \ @@ -68,125 +67,121 @@ static const COMP pi_on_4 = { .70710678118654752439, .70710678118654752439 }; / \*---------------------------------------------------------------------------*/ -struct FDMDV * fdmdv_create(int Nc) -{ - struct FDMDV *f; - int c, i, k; - - assert(NC == FDMDV_NC_MAX); /* check public and private #defines match */ - assert(Nc <= NC); - assert(FDMDV_NOM_SAMPLES_PER_FRAME == M_FAC); - assert(FDMDV_MAX_SAMPLES_PER_FRAME == (M_FAC+M_FAC/P)); - - f = (struct FDMDV*)MALLOC(sizeof(struct FDMDV)); - if (f == NULL) - return NULL; - - f->Nc = Nc; - - f->ntest_bits = Nc*NB*4; - f->current_test_bit = 0; - f->rx_test_bits_mem = (int*)MALLOC(sizeof(int)*f->ntest_bits); - assert(f->rx_test_bits_mem != NULL); - for(i=0; i<f->ntest_bits; i++) - f->rx_test_bits_mem[i] = 0; - assert((sizeof(test_bits)/sizeof(int)) >= f->ntest_bits); - - f->old_qpsk_mapping = 0; - - f->tx_pilot_bit = 0; - - for(c=0; c<Nc+1; c++) { - f->prev_tx_symbols[c].real = 1.0; - f->prev_tx_symbols[c].imag = 0.0; - f->prev_rx_symbols[c].real = 1.0; - f->prev_rx_symbols[c].imag = 0.0; - - for(k=0; k<NSYM; k++) { - f->tx_filter_memory[c][k].real = 0.0; - f->tx_filter_memory[c][k].imag = 0.0; - } - - /* Spread initial FDM carrier phase out as far as possible. - This helped PAPR for a few dB. We don't need to adjust rx - phase as DQPSK takes care of that. */ +struct FDMDV *fdmdv_create(int Nc) { + struct FDMDV *f; + int c, i, k; - f->phase_tx[c].real = cosf(2.0*PI*c/(Nc+1)); - f->phase_tx[c].imag = sinf(2.0*PI*c/(Nc+1)); + assert(NC == FDMDV_NC_MAX); /* check public and private #defines match */ + assert(Nc <= NC); + assert(FDMDV_NOM_SAMPLES_PER_FRAME == M_FAC); + assert(FDMDV_MAX_SAMPLES_PER_FRAME == (M_FAC + M_FAC / P)); - f->phase_rx[c].real = 1.0; - f->phase_rx[c].imag = 0.0; + f = (struct FDMDV *)MALLOC(sizeof(struct FDMDV)); + if (f == NULL) return NULL; - for(k=0; k<NT*P; k++) { - f->rx_filter_mem_timing[c][k].real = 0.0; - f->rx_filter_mem_timing[c][k].imag = 0.0; - } - } - f->prev_tx_symbols[Nc].real = 2.0; - - fdmdv_set_fsep(f, FSEP); - f->freq[Nc].real = cosf(2.0*PI*0.0/FS); - f->freq[Nc].imag = sinf(2.0*PI*0.0/FS); - f->freq_pol[Nc] = 2.0*PI*0.0/FS; - - f->fbb_rect.real = cosf(2.0*PI*FDMDV_FCENTRE/FS); - f->fbb_rect.imag = sinf(2.0*PI*FDMDV_FCENTRE/FS); - f->fbb_pol = 2.0*PI*FDMDV_FCENTRE/FS; - f->fbb_phase_tx.real = 1.0; - f->fbb_phase_tx.imag = 0.0; - f->fbb_phase_rx.real = 1.0; - f->fbb_phase_rx.imag = 0.0; - - /* Generate DBPSK pilot Look Up Table (LUT) */ + f->Nc = Nc; - generate_pilot_lut(f->pilot_lut, &f->freq[Nc]); + f->ntest_bits = Nc * NB * 4; + f->current_test_bit = 0; + f->rx_test_bits_mem = (int *)MALLOC(sizeof(int) * f->ntest_bits); + assert(f->rx_test_bits_mem != NULL); + for (i = 0; i < f->ntest_bits; i++) f->rx_test_bits_mem[i] = 0; + assert((sizeof(test_bits) / sizeof(int)) >= f->ntest_bits); - /* freq Offset estimation states */ + f->old_qpsk_mapping = 0; - f->fft_pilot_cfg = codec2_fft_alloc (MPILOTFFT, 0, NULL, NULL); - assert(f->fft_pilot_cfg != NULL); + f->tx_pilot_bit = 0; - for(i=0; i<NPILOTBASEBAND; i++) { - f->pilot_baseband1[i].real = f->pilot_baseband2[i].real = 0.0; - f->pilot_baseband1[i].imag = f->pilot_baseband2[i].imag = 0.0; - } - f->pilot_lut_index = 0; - f->prev_pilot_lut_index = 3*M_FAC; - - for(i=0; i<NRXDECMEM; i++) { - f->rxdec_lpf_mem[i].real = 0.0; - f->rxdec_lpf_mem[i].imag = 0.0; - } + for (c = 0; c < Nc + 1; c++) { + f->prev_tx_symbols[c].real = 1.0; + f->prev_tx_symbols[c].imag = 0.0; + f->prev_rx_symbols[c].real = 1.0; + f->prev_rx_symbols[c].imag = 0.0; - for(i=0; i<NPILOTLPF; i++) { - f->pilot_lpf1[i].real = f->pilot_lpf2[i].real = 0.0; - f->pilot_lpf1[i].imag = f->pilot_lpf2[i].imag = 0.0; + for (k = 0; k < NSYM; k++) { + f->tx_filter_memory[c][k].real = 0.0; + f->tx_filter_memory[c][k].imag = 0.0; } - f->foff = 0.0; - f->foff_phase_rect.real = 1.0; - f->foff_phase_rect.imag = 0.0; + /* Spread initial FDM carrier phase out as far as possible. + This helped PAPR for a few dB. We don't need to adjust rx + phase as DQPSK takes care of that. */ - for(i=0; i<NRX_FDM_MEM; i++) { - f->rx_fdm_mem[i].real = 0.0; - f->rx_fdm_mem[i].imag = 0.0; - } + f->phase_tx[c].real = cosf(2.0 * PI * c / (Nc + 1)); + f->phase_tx[c].imag = sinf(2.0 * PI * c / (Nc + 1)); - f->fest_state = 0; - f->sync = 0; - f->timer = 0; - for(i=0; i<NSYNC_MEM; i++) - f->sync_mem[i] = 0; + f->phase_rx[c].real = 1.0; + f->phase_rx[c].imag = 0.0; - for(c=0; c<Nc+1; c++) { - f->sig_est[c] = 0.0; - f->noise_est[c] = 0.0; + for (k = 0; k < NT * P; k++) { + f->rx_filter_mem_timing[c][k].real = 0.0; + f->rx_filter_mem_timing[c][k].imag = 0.0; } - - f->sig_pwr_av = 0.0; - f->foff_filt = 0.0; - - return f; + } + f->prev_tx_symbols[Nc].real = 2.0; + + fdmdv_set_fsep(f, FSEP); + f->freq[Nc].real = cosf(2.0 * PI * 0.0 / FS); + f->freq[Nc].imag = sinf(2.0 * PI * 0.0 / FS); + f->freq_pol[Nc] = 2.0 * PI * 0.0 / FS; + + f->fbb_rect.real = cosf(2.0 * PI * FDMDV_FCENTRE / FS); + f->fbb_rect.imag = sinf(2.0 * PI * FDMDV_FCENTRE / FS); + f->fbb_pol = 2.0 * PI * FDMDV_FCENTRE / FS; + f->fbb_phase_tx.real = 1.0; + f->fbb_phase_tx.imag = 0.0; + f->fbb_phase_rx.real = 1.0; + f->fbb_phase_rx.imag = 0.0; + + /* Generate DBPSK pilot Look Up Table (LUT) */ + + generate_pilot_lut(f->pilot_lut, &f->freq[Nc]); + + /* freq Offset estimation states */ + + f->fft_pilot_cfg = codec2_fft_alloc(MPILOTFFT, 0, NULL, NULL); + assert(f->fft_pilot_cfg != NULL); + + for (i = 0; i < NPILOTBASEBAND; i++) { + f->pilot_baseband1[i].real = f->pilot_baseband2[i].real = 0.0; + f->pilot_baseband1[i].imag = f->pilot_baseband2[i].imag = 0.0; + } + f->pilot_lut_index = 0; + f->prev_pilot_lut_index = 3 * M_FAC; + + for (i = 0; i < NRXDECMEM; i++) { + f->rxdec_lpf_mem[i].real = 0.0; + f->rxdec_lpf_mem[i].imag = 0.0; + } + + for (i = 0; i < NPILOTLPF; i++) { + f->pilot_lpf1[i].real = f->pilot_lpf2[i].real = 0.0; + f->pilot_lpf1[i].imag = f->pilot_lpf2[i].imag = 0.0; + } + + f->foff = 0.0; + f->foff_phase_rect.real = 1.0; + f->foff_phase_rect.imag = 0.0; + + for (i = 0; i < NRX_FDM_MEM; i++) { + f->rx_fdm_mem[i].real = 0.0; + f->rx_fdm_mem[i].imag = 0.0; + } + + f->fest_state = 0; + f->sync = 0; + f->timer = 0; + for (i = 0; i < NSYNC_MEM; i++) f->sync_mem[i] = 0; + + for (c = 0; c < Nc + 1; c++) { + f->sig_est[c] = 0.0; + f->noise_est[c] = 0.0; + } + + f->sig_pwr_av = 0.0; + f->foff_filt = 0.0; + + return f; } /*---------------------------------------------------------------------------*\ @@ -199,24 +194,18 @@ struct FDMDV * fdmdv_create(int Nc) \*---------------------------------------------------------------------------*/ -void fdmdv_destroy(struct FDMDV *fdmdv) -{ - assert(fdmdv != NULL); - codec2_fft_free(fdmdv->fft_pilot_cfg); - FREE(fdmdv->rx_test_bits_mem); - FREE(fdmdv); +void fdmdv_destroy(struct FDMDV *fdmdv) { + assert(fdmdv != NULL); + codec2_fft_free(fdmdv->fft_pilot_cfg); + FREE(fdmdv->rx_test_bits_mem); + FREE(fdmdv); } - void fdmdv_use_old_qpsk_mapping(struct FDMDV *fdmdv) { - fdmdv->old_qpsk_mapping = 1; + fdmdv->old_qpsk_mapping = 1; } - -int fdmdv_bits_per_frame(struct FDMDV *fdmdv) -{ - return (fdmdv->Nc * NB); -} +int fdmdv_bits_per_frame(struct FDMDV *fdmdv) { return (fdmdv->Nc * NB); } /*---------------------------------------------------------------------------*\ @@ -230,48 +219,42 @@ int fdmdv_bits_per_frame(struct FDMDV *fdmdv) \*---------------------------------------------------------------------------*/ -void fdmdv_get_test_bits(struct FDMDV *f, int tx_bits[]) -{ - int i; - int bits_per_frame = fdmdv_bits_per_frame(f); +void fdmdv_get_test_bits(struct FDMDV *f, int tx_bits[]) { + int i; + int bits_per_frame = fdmdv_bits_per_frame(f); - for(i=0; i<bits_per_frame; i++) { - tx_bits[i] = test_bits[f->current_test_bit]; - f->current_test_bit++; - if (f->current_test_bit > (f->ntest_bits-1)) - f->current_test_bit = 0; - } + for (i = 0; i < bits_per_frame; i++) { + tx_bits[i] = test_bits[f->current_test_bit]; + f->current_test_bit++; + if (f->current_test_bit > (f->ntest_bits - 1)) f->current_test_bit = 0; + } } -float fdmdv_get_fsep(struct FDMDV *f) -{ - return f->fsep; -} +float fdmdv_get_fsep(struct FDMDV *f) { return f->fsep; } void fdmdv_set_fsep(struct FDMDV *f, float fsep) { - int c; - float carrier_freq; - - f->fsep = fsep; - - /* Set up frequency of each carrier */ - - for(c=0; c<f->Nc/2; c++) { - carrier_freq = (-f->Nc/2 + c)*f->fsep; - f->freq[c].real = cosf(2.0*PI*carrier_freq/FS); - f->freq[c].imag = sinf(2.0*PI*carrier_freq/FS); - f->freq_pol[c] = 2.0*PI*carrier_freq/FS; - } - - for(c=f->Nc/2; c<f->Nc; c++) { - carrier_freq = (-f->Nc/2 + c + 1)*f->fsep; - f->freq[c].real = cosf(2.0*PI*carrier_freq/FS); - f->freq[c].imag = sinf(2.0*PI*carrier_freq/FS); - f->freq_pol[c] = 2.0*PI*carrier_freq/FS; - } + int c; + float carrier_freq; + + f->fsep = fsep; + + /* Set up frequency of each carrier */ + + for (c = 0; c < f->Nc / 2; c++) { + carrier_freq = (-f->Nc / 2 + c) * f->fsep; + f->freq[c].real = cosf(2.0 * PI * carrier_freq / FS); + f->freq[c].imag = sinf(2.0 * PI * carrier_freq / FS); + f->freq_pol[c] = 2.0 * PI * carrier_freq / FS; + } + + for (c = f->Nc / 2; c < f->Nc; c++) { + carrier_freq = (-f->Nc / 2 + c + 1) * f->fsep; + f->freq[c].real = cosf(2.0 * PI * carrier_freq / FS); + f->freq[c].imag = sinf(2.0 * PI * carrier_freq / FS); + f->freq_pol[c] = 2.0 * PI * carrier_freq / FS; + } } - /*---------------------------------------------------------------------------*\ FUNCTION....: bits_to_dqpsk_symbols() @@ -284,50 +267,48 @@ void fdmdv_set_fsep(struct FDMDV *f, float fsep) { \*---------------------------------------------------------------------------*/ -void bits_to_dqpsk_symbols(COMP tx_symbols[], int Nc, COMP prev_tx_symbols[], int tx_bits[], int *pilot_bit, int old_qpsk_mapping) -{ - int c, msb, lsb; - COMP j = {0.0,1.0}; - - /* Map tx_bits to to Nc DQPSK symbols. Note legacy support for - old (suboptimal) V0.91 FreeDV mapping */ - - for(c=0; c<Nc; c++) { - msb = tx_bits[2*c]; - lsb = tx_bits[2*c+1]; - if ((msb == 0) && (lsb == 0)) - tx_symbols[c] = prev_tx_symbols[c]; - if ((msb == 0) && (lsb == 1)) - tx_symbols[c] = cmult(j, prev_tx_symbols[c]); - if ((msb == 1) && (lsb == 0)) { - if (old_qpsk_mapping) - tx_symbols[c] = cneg(prev_tx_symbols[c]); - else - tx_symbols[c] = cmult(cneg(j),prev_tx_symbols[c]); - } - if ((msb == 1) && (lsb == 1)) { - if (old_qpsk_mapping) - tx_symbols[c] = cmult(cneg(j),prev_tx_symbols[c]); - else - tx_symbols[c] = cneg(prev_tx_symbols[c]); - } +void bits_to_dqpsk_symbols(COMP tx_symbols[], int Nc, COMP prev_tx_symbols[], + int tx_bits[], int *pilot_bit, + int old_qpsk_mapping) { + int c, msb, lsb; + COMP j = {0.0, 1.0}; + + /* Map tx_bits to to Nc DQPSK symbols. Note legacy support for + old (suboptimal) V0.91 FreeDV mapping */ + + for (c = 0; c < Nc; c++) { + msb = tx_bits[2 * c]; + lsb = tx_bits[2 * c + 1]; + if ((msb == 0) && (lsb == 0)) tx_symbols[c] = prev_tx_symbols[c]; + if ((msb == 0) && (lsb == 1)) tx_symbols[c] = cmult(j, prev_tx_symbols[c]); + if ((msb == 1) && (lsb == 0)) { + if (old_qpsk_mapping) + tx_symbols[c] = cneg(prev_tx_symbols[c]); + else + tx_symbols[c] = cmult(cneg(j), prev_tx_symbols[c]); } + if ((msb == 1) && (lsb == 1)) { + if (old_qpsk_mapping) + tx_symbols[c] = cmult(cneg(j), prev_tx_symbols[c]); + else + tx_symbols[c] = cneg(prev_tx_symbols[c]); + } + } - /* +1 -1 +1 -1 BPSK sync carrier, once filtered becomes (roughly) - two spectral lines at +/- Rs/2 */ + /* +1 -1 +1 -1 BPSK sync carrier, once filtered becomes (roughly) + two spectral lines at +/- Rs/2 */ - if (*pilot_bit) - tx_symbols[Nc] = cneg(prev_tx_symbols[Nc]); - else - tx_symbols[Nc] = prev_tx_symbols[Nc]; + if (*pilot_bit) + tx_symbols[Nc] = cneg(prev_tx_symbols[Nc]); + else + tx_symbols[Nc] = prev_tx_symbols[Nc]; - if (*pilot_bit) - *pilot_bit = 0; - else - *pilot_bit = 1; + if (*pilot_bit) + *pilot_bit = 0; + else + *pilot_bit = 1; } - /*---------------------------------------------------------------------------*\ FUNCTION....: tx_filter() @@ -339,57 +320,55 @@ void bits_to_dqpsk_symbols(COMP tx_symbols[], int Nc, COMP prev_tx_symbols[], in \*---------------------------------------------------------------------------*/ -void tx_filter(COMP tx_baseband[NC+1][M_FAC], int Nc, COMP tx_symbols[], COMP tx_filter_memory[NC+1][NSYM]) -{ - int c; - int i,j,k; - float acc; - COMP gain; +void tx_filter(COMP tx_baseband[NC + 1][M_FAC], int Nc, COMP tx_symbols[], + COMP tx_filter_memory[NC + 1][NSYM]) { + int c; + int i, j, k; + float acc; + COMP gain; - gain.real = sqrtf(2.0)/2.0; - gain.imag = 0.0; + gain.real = sqrtf(2.0) / 2.0; + gain.imag = 0.0; - for(c=0; c<Nc+1; c++) - tx_filter_memory[c][NSYM-1] = cmult(tx_symbols[c], gain); - - /* - tx filter each symbol, generate M_FAC filtered output samples for each symbol. - Efficient polyphase filter techniques used as tx_filter_memory is sparse - */ + for (c = 0; c < Nc + 1; c++) + tx_filter_memory[c][NSYM - 1] = cmult(tx_symbols[c], gain); - for(i=0; i<M_FAC; i++) { - for(c=0; c<Nc+1; c++) { + /* + tx filter each symbol, generate M_FAC filtered output samples for each + symbol. Efficient polyphase filter techniques used as tx_filter_memory is + sparse + */ - /* filter real sample of symbol for carrier c */ + for (i = 0; i < M_FAC; i++) { + for (c = 0; c < Nc + 1; c++) { + /* filter real sample of symbol for carrier c */ - acc = 0.0; - for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC) - acc += M_FAC * tx_filter_memory[c][j].real * gt_alpha5_root[k]; - tx_baseband[c][i].real = acc; + acc = 0.0; + for (j = 0, k = M_FAC - i - 1; j < NSYM; j++, k += M_FAC) + acc += M_FAC * tx_filter_memory[c][j].real * gt_alpha5_root[k]; + tx_baseband[c][i].real = acc; - /* filter imag sample of symbol for carrier c */ + /* filter imag sample of symbol for carrier c */ - acc = 0.0; - for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC) - acc += M_FAC * tx_filter_memory[c][j].imag * gt_alpha5_root[k]; - tx_baseband[c][i].imag = acc; - - } + acc = 0.0; + for (j = 0, k = M_FAC - i - 1; j < NSYM; j++, k += M_FAC) + acc += M_FAC * tx_filter_memory[c][j].imag * gt_alpha5_root[k]; + tx_baseband[c][i].imag = acc; } + } - /* shift memory, inserting zeros at end */ + /* shift memory, inserting zeros at end */ - for(i=0; i<NSYM-1; i++) - for(c=0; c<Nc+1; c++) - tx_filter_memory[c][i] = tx_filter_memory[c][i+1]; + for (i = 0; i < NSYM - 1; i++) + for (c = 0; c < Nc + 1; c++) + tx_filter_memory[c][i] = tx_filter_memory[c][i + 1]; - for(c=0; c<Nc+1; c++) { - tx_filter_memory[c][NSYM-1].real = 0.0; - tx_filter_memory[c][NSYM-1].imag = 0.0; - } + for (c = 0; c < Nc + 1; c++) { + tx_filter_memory[c][NSYM - 1].real = 0.0; + tx_filter_memory[c][NSYM - 1].imag = 0.0; + } } - /*---------------------------------------------------------------------------*\ FUNCTION....: tx_filter_and_upconvert() @@ -402,102 +381,98 @@ void tx_filter(COMP tx_baseband[NC+1][M_FAC], int Nc, COMP tx_symbols[], COMP tx \*---------------------------------------------------------------------------*/ void 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) -{ - int c; - int i,j,k; - float acc; - COMP gain; - COMP tx_baseband; - COMP two = {2.0, 0.0}; - float mag; - - gain.real = sqrtf(2.0)/2.0; - gain.imag = 0.0; - - for(i=0; i<M_FAC; i++) { - tx_fdm[i].real = 0.0; - tx_fdm[i].imag = 0.0; + COMP tx_filter_memory[NC + 1][NSYM], + COMP phase_tx[], COMP freq[], COMP *fbb_phase, + COMP fbb_rect) { + int c; + int i, j, k; + float acc; + COMP gain; + COMP tx_baseband; + COMP two = {2.0, 0.0}; + float mag; + + gain.real = sqrtf(2.0) / 2.0; + gain.imag = 0.0; + + for (i = 0; i < M_FAC; i++) { + tx_fdm[i].real = 0.0; + tx_fdm[i].imag = 0.0; + } + + for (c = 0; c < Nc + 1; c++) + tx_filter_memory[c][NSYM - 1] = cmult(tx_symbols[c], gain); + + /* + tx filter each symbol, generate M_FAC filtered output samples for + each symbol, which we then freq shift and sum with other + carriers. Efficient polyphase filter techniques used as + tx_filter_memory is sparse + */ + + for (c = 0; c < Nc + 1; c++) { + for (i = 0; i < M_FAC; i++) { + /* filter real sample of symbol for carrier c */ + + acc = 0.0; + for (j = 0, k = M_FAC - i - 1; j < NSYM; j++, k += M_FAC) + acc += M_FAC * tx_filter_memory[c][j].real * gt_alpha5_root[k]; + tx_baseband.real = acc; + + /* filter imag sample of symbol for carrier c */ + + acc = 0.0; + for (j = 0, k = M_FAC - i - 1; j < NSYM; j++, k += M_FAC) + acc += M_FAC * tx_filter_memory[c][j].imag * gt_alpha5_root[k]; + tx_baseband.imag = acc; + + /* freq shift and sum */ + + phase_tx[c] = cmult(phase_tx[c], freq[c]); + tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband, phase_tx[c])); } + } - for(c=0; c<Nc+1; c++) - tx_filter_memory[c][NSYM-1] = cmult(tx_symbols[c], gain); - - /* - tx filter each symbol, generate M_FAC filtered output samples for - each symbol, which we then freq shift and sum with other - carriers. Efficient polyphase filter techniques used as - tx_filter_memory is sparse - */ + /* shift whole thing up to carrier freq */ - for(c=0; c<Nc+1; c++) { - for(i=0; i<M_FAC; i++) { + for (i = 0; i < M_FAC; i++) { + *fbb_phase = cmult(*fbb_phase, fbb_rect); + tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase); + } - /* filter real sample of symbol for carrier c */ + /* + Scale such that total Carrier power C of real(tx_fdm) = Nc. This + excludes the power of the pilot tone. + We return the complex (single sided) signal to make frequency + shifting for the purpose of testing easier + */ - acc = 0.0; - for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC) - acc += M_FAC * tx_filter_memory[c][j].real * gt_alpha5_root[k]; - tx_baseband.real = acc; + for (i = 0; i < M_FAC; i++) tx_fdm[i] = cmult(two, tx_fdm[i]); - /* filter imag sample of symbol for carrier c */ + /* normalise digital oscillators as the magnitude can drift over time */ - acc = 0.0; - for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC) - acc += M_FAC * tx_filter_memory[c][j].imag * gt_alpha5_root[k]; - tx_baseband.imag = acc; + for (c = 0; c < Nc + 1; c++) { + mag = cabsolute(phase_tx[c]); + phase_tx[c].real /= mag; + phase_tx[c].imag /= mag; + } - /* freq shift and sum */ + mag = cabsolute(*fbb_phase); + fbb_phase->real /= mag; + fbb_phase->imag /= mag; - phase_tx[c] = cmult(phase_tx[c], freq[c]); - tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband, phase_tx[c])); - } - } + /* shift memory, inserting zeros at end */ - /* shift whole thing up to carrier freq */ + for (i = 0; i < NSYM - 1; i++) + for (c = 0; c < Nc + 1; c++) + tx_filter_memory[c][i] = tx_filter_memory[c][i + 1]; - for (i=0; i<M_FAC; i++) { - *fbb_phase = cmult(*fbb_phase, fbb_rect); - tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase); - } - - /* - Scale such that total Carrier power C of real(tx_fdm) = Nc. This - excludes the power of the pilot tone. - We return the complex (single sided) signal to make frequency - shifting for the purpose of testing easier - */ - - for (i=0; i<M_FAC; i++) - tx_fdm[i] = cmult(two, tx_fdm[i]); - - /* normalise digital oscillators as the magnitude can drift over time */ - - for (c=0; c<Nc+1; c++) { - mag = cabsolute(phase_tx[c]); - phase_tx[c].real /= mag; - phase_tx[c].imag /= mag; - } - - mag = cabsolute(*fbb_phase); - fbb_phase->real /= mag; - fbb_phase->imag /= mag; - - /* shift memory, inserting zeros at end */ - - for(i=0; i<NSYM-1; i++) - for(c=0; c<Nc+1; c++) - tx_filter_memory[c][i] = tx_filter_memory[c][i+1]; - - for(c=0; c<Nc+1; c++) { - tx_filter_memory[c][NSYM-1].real = 0.0; - tx_filter_memory[c][NSYM-1].imag = 0.0; - } + for (c = 0; c < Nc + 1; c++) { + tx_filter_memory[c][NSYM - 1].real = 0.0; + tx_filter_memory[c][NSYM - 1].imag = 0.0; + } } - /*---------------------------------------------------------------------------*\ FUNCTION....: fdm_upconvert() @@ -510,52 +485,51 @@ void tx_filter_and_upconvert(COMP tx_fdm[], int Nc, COMP tx_symbols[], \*---------------------------------------------------------------------------*/ -void 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) -{ - int i,c; - COMP two = {2.0, 0.0}; - float mag; - - for(i=0; i<M_FAC; i++) { - tx_fdm[i].real = 0.0; - tx_fdm[i].imag = 0.0; +void 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) { + int i, c; + COMP two = {2.0, 0.0}; + float mag; + + for (i = 0; i < M_FAC; i++) { + tx_fdm[i].real = 0.0; + tx_fdm[i].imag = 0.0; + } + + for (c = 0; c <= Nc; c++) + for (i = 0; i < M_FAC; i++) { + phase_tx[c] = cmult(phase_tx[c], freq[c]); + tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband[c][i], phase_tx[c])); } - for (c=0; c<=Nc; c++) - for (i=0; i<M_FAC; i++) { - phase_tx[c] = cmult(phase_tx[c], freq[c]); - tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband[c][i], phase_tx[c])); - } + /* shift whole thing up to carrier freq */ - /* shift whole thing up to carrier freq */ + for (i = 0; i < M_FAC; i++) { + *fbb_phase = cmult(*fbb_phase, fbb_rect); + tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase); + } - for (i=0; i<M_FAC; i++) { - *fbb_phase = cmult(*fbb_phase, fbb_rect); - tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase); - } + /* + Scale such that total Carrier power C of real(tx_fdm) = Nc. This + excludes the power of the pilot tone. + We return the complex (single sided) signal to make frequency + shifting for the purpose of testing easier + */ - /* - Scale such that total Carrier power C of real(tx_fdm) = Nc. This - excludes the power of the pilot tone. - We return the complex (single sided) signal to make frequency - shifting for the purpose of testing easier - */ + for (i = 0; i < M_FAC; i++) tx_fdm[i] = cmult(two, tx_fdm[i]); - for (i=0; i<M_FAC; i++) - tx_fdm[i] = cmult(two, tx_fdm[i]); + /* normalise digital oscilators as the magnitude can drift over time */ - /* normalise digital oscilators as the magnitude can drift over time */ + for (c = 0; c < Nc + 1; c++) { + mag = cabsolute(phase_tx[c]); + phase_tx[c].real /= mag; + phase_tx[c].imag /= mag; + } - for (c=0; c<Nc+1; c++) { - mag = cabsolute(phase_tx[c]); - phase_tx[c].real /= mag; - phase_tx[c].imag /= mag; - } - - mag = cabsolute(*fbb_phase); - fbb_phase->real /= mag; - fbb_phase->imag /= mag; + mag = cabsolute(*fbb_phase); + fbb_phase->real /= mag; + fbb_phase->imag /= mag; } /*---------------------------------------------------------------------------*\ @@ -576,20 +550,24 @@ void fdm_upconvert(COMP tx_fdm[], int Nc, COMP tx_baseband[NC+1][M_FAC], COMP ph \*---------------------------------------------------------------------------*/ -void fdmdv_mod(struct FDMDV *fdmdv, COMP tx_fdm[], int tx_bits[], int *sync_bit) -{ - COMP tx_symbols[NC+1]; - PROFILE_VAR(mod_start, tx_filter_and_upconvert_start); - - PROFILE_SAMPLE(mod_start); - bits_to_dqpsk_symbols(tx_symbols, fdmdv->Nc, fdmdv->prev_tx_symbols, tx_bits, &fdmdv->tx_pilot_bit, fdmdv->old_qpsk_mapping); - memcpy(fdmdv->prev_tx_symbols, tx_symbols, sizeof(COMP)*(fdmdv->Nc+1)); - PROFILE_SAMPLE_AND_LOG(tx_filter_and_upconvert_start, mod_start, " bits_to_dqpsk_symbols"); - tx_filter_and_upconvert(tx_fdm, fdmdv->Nc, tx_symbols, fdmdv->tx_filter_memory, - fdmdv->phase_tx, fdmdv->freq, &fdmdv->fbb_phase_tx, fdmdv->fbb_rect); - PROFILE_SAMPLE_AND_LOG2(tx_filter_and_upconvert_start, " tx_filter_and_upconvert"); - - *sync_bit = fdmdv->tx_pilot_bit; +void fdmdv_mod(struct FDMDV *fdmdv, COMP tx_fdm[], int tx_bits[], + int *sync_bit) { + COMP tx_symbols[NC + 1]; + PROFILE_VAR(mod_start, tx_filter_and_upconvert_start); + + PROFILE_SAMPLE(mod_start); + bits_to_dqpsk_symbols(tx_symbols, fdmdv->Nc, fdmdv->prev_tx_symbols, tx_bits, + &fdmdv->tx_pilot_bit, fdmdv->old_qpsk_mapping); + memcpy(fdmdv->prev_tx_symbols, tx_symbols, sizeof(COMP) * (fdmdv->Nc + 1)); + PROFILE_SAMPLE_AND_LOG(tx_filter_and_upconvert_start, mod_start, + " bits_to_dqpsk_symbols"); + tx_filter_and_upconvert(tx_fdm, fdmdv->Nc, tx_symbols, + fdmdv->tx_filter_memory, fdmdv->phase_tx, fdmdv->freq, + &fdmdv->fbb_phase_tx, fdmdv->fbb_rect); + PROFILE_SAMPLE_AND_LOG2(tx_filter_and_upconvert_start, + " tx_filter_and_upconvert"); + + *sync_bit = fdmdv->tx_pilot_bit; } /*---------------------------------------------------------------------------*\ @@ -603,46 +581,42 @@ void fdmdv_mod(struct FDMDV *fdmdv, COMP tx_fdm[], int tx_bits[], int *sync_bit) \*---------------------------------------------------------------------------*/ void generate_pilot_fdm(COMP *pilot_fdm, int *bit, float *symbol, - float *filter_mem, COMP *phase, COMP *freq) -{ - int i,j,k; - float tx_baseband[M_FAC]; + float *filter_mem, COMP *phase, COMP *freq) { + int i, j, k; + float tx_baseband[M_FAC]; - /* +1 -1 +1 -1 DBPSK sync carrier, once filtered becomes (roughly) - two spectral lines at +/- RS/2 */ + /* +1 -1 +1 -1 DBPSK sync carrier, once filtered becomes (roughly) + two spectral lines at +/- RS/2 */ - if (*bit) - *symbol = -*symbol; + if (*bit) *symbol = -*symbol; - if (*bit) - *bit = 0; - else - *bit = 1; + if (*bit) + *bit = 0; + else + *bit = 1; - /* filter DPSK symbol to create M_FAC baseband samples */ + /* filter DPSK symbol to create M_FAC baseband samples */ - filter_mem[NFILTER-1] = (sqrtf(2)/2) * *symbol; - for(i=0; i<M_FAC; i++) { - tx_baseband[i] = 0.0; - for(j=M_FAC-1,k=M_FAC-i-1; j<NFILTER; j+=M_FAC,k+=M_FAC) - tx_baseband[i] += M_FAC * filter_mem[j] * gt_alpha5_root[k]; - } + filter_mem[NFILTER - 1] = (sqrtf(2) / 2) * *symbol; + for (i = 0; i < M_FAC; i++) { + tx_baseband[i] = 0.0; + for (j = M_FAC - 1, k = M_FAC - i - 1; j < NFILTER; j += M_FAC, k += M_FAC) + tx_baseband[i] += M_FAC * filter_mem[j] * gt_alpha5_root[k]; + } - /* shift memory, inserting zeros at end */ + /* shift memory, inserting zeros at end */ - for(i=0; i<NFILTER-M_FAC; i++) - filter_mem[i] = filter_mem[i+M_FAC]; + for (i = 0; i < NFILTER - M_FAC; i++) filter_mem[i] = filter_mem[i + M_FAC]; - for(i=NFILTER-M_FAC; i<NFILTER; i++) - filter_mem[i] = 0.0; + for (i = NFILTER - M_FAC; i < NFILTER; i++) filter_mem[i] = 0.0; - /* upconvert */ + /* upconvert */ - for(i=0; i<M_FAC; i++) { - *phase = cmult(*phase, *freq); - pilot_fdm[i].real = sqrtf(2)*2*tx_baseband[i] * phase->real; - pilot_fdm[i].imag = sqrtf(2)*2*tx_baseband[i] * phase->imag; - } + for (i = 0; i < M_FAC; i++) { + *phase = cmult(*phase, *freq); + pilot_fdm[i].real = sqrtf(2) * 2 * tx_baseband[i] * phase->real; + pilot_fdm[i].imag = sqrtf(2) * 2 * tx_baseband[i] * phase->imag; + } } /*---------------------------------------------------------------------------*\ @@ -657,33 +631,30 @@ void generate_pilot_fdm(COMP *pilot_fdm, int *bit, float *symbol, \*---------------------------------------------------------------------------*/ -void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq) -{ - int pilot_rx_bit = 0; - float pilot_symbol = sqrtf(2.0); - COMP pilot_phase = {1.0, 0.0}; - float pilot_filter_mem[NFILTER]; - COMP pilot[M_FAC]; - int i,f; - - for(i=0; i<NFILTER; i++) - pilot_filter_mem[i] = 0.0; - - /* discard first 4 symbols as filter memory is filling, just keep - last four symbols */ - - for(f=0; f<8; f++) { - generate_pilot_fdm(pilot, &pilot_rx_bit, &pilot_symbol, pilot_filter_mem, &pilot_phase, pilot_freq); - if (f >= 4) - memcpy(&pilot_lut[M_FAC*(f-4)], pilot, M_FAC*sizeof(COMP)); - } - - // create complex conjugate since we need this and only this later on - for (f=0;f<4*M_FAC;f++) - { - pilot_lut[f] = cconj(pilot_lut[f]); - } - +void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq) { + int pilot_rx_bit = 0; + float pilot_symbol = sqrtf(2.0); + COMP pilot_phase = {1.0, 0.0}; + float pilot_filter_mem[NFILTER]; + COMP pilot[M_FAC]; + int i, f; + + for (i = 0; i < NFILTER; i++) pilot_filter_mem[i] = 0.0; + + /* discard first 4 symbols as filter memory is filling, just keep + last four symbols */ + + for (f = 0; f < 8; f++) { + generate_pilot_fdm(pilot, &pilot_rx_bit, &pilot_symbol, pilot_filter_mem, + &pilot_phase, pilot_freq); + if (f >= 4) + memcpy(&pilot_lut[M_FAC * (f - 4)], pilot, M_FAC * sizeof(COMP)); + } + + // create complex conjugate since we need this and only this later on + for (f = 0; f < 4 * M_FAC; f++) { + pilot_lut[f] = cconj(pilot_lut[f]); + } } /*---------------------------------------------------------------------------*\ @@ -697,90 +668,91 @@ void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq) \*---------------------------------------------------------------------------*/ void 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) -{ - int i,j,k; - int mpilot; - float mag, imax; - int ix; - float r; - - /* LPF cutoff 200Hz, so we can handle max +/- 200 Hz freq offset */ - - for(i=0; i<NPILOTLPF-nin; i++) - pilot_lpf[i] = pilot_lpf[nin+i]; - for(i=NPILOTLPF-nin, j=NPILOTBASEBAND-nin; i<NPILOTLPF; i++,j++) { - pilot_lpf[i].real = 0.0; pilot_lpf[i].imag = 0.0; - - // STM32F4 hand optimized, this alone makes it go done from 1.6 to 1.17ms - // switching pilot_coeff to RAM (by removing const in pilot_coeff.h) would save - // another 0.11 ms at the expense of NPILOTCOEFF * 4 bytes == 120 bytes RAM - - if (NPILOTCOEFF%5 == 0) - { - for(k=0; k<NPILOTCOEFF; k+=5) - { - COMP i0 = fcmult(pilot_coeff[k], pilot_baseband[j-NPILOTCOEFF+1+k]); - COMP i1 = fcmult(pilot_coeff[k+1], pilot_baseband[j-NPILOTCOEFF+1+k+1]); - COMP i2 = fcmult(pilot_coeff[k+2], pilot_baseband[j-NPILOTCOEFF+1+k+2]); - COMP i3 = fcmult(pilot_coeff[k+3], pilot_baseband[j-NPILOTCOEFF+1+k+3]); - COMP i4 = fcmult(pilot_coeff[k+4], pilot_baseband[j-NPILOTCOEFF+1+k+4]); - - pilot_lpf[i] = cadd(cadd(cadd(cadd(cadd(pilot_lpf[i], i0),i1),i2),i3),i4); - } - } - else - { - for(k=0; k<NPILOTCOEFF; k++) - { - pilot_lpf[i] = cadd(pilot_lpf[i], fcmult(pilot_coeff[k], pilot_baseband[j-NPILOTCOEFF+1+k])); - } - - } + COMP pilot_lpf[], codec2_fft_cfg fft_pilot_cfg, COMP S[], + int nin, int do_fft) { + int i, j, k; + int mpilot; + float mag, imax; + int ix; + float r; + + /* LPF cutoff 200Hz, so we can handle max +/- 200 Hz freq offset */ + + for (i = 0; i < NPILOTLPF - nin; i++) pilot_lpf[i] = pilot_lpf[nin + i]; + for (i = NPILOTLPF - nin, j = NPILOTBASEBAND - nin; i < NPILOTLPF; i++, j++) { + pilot_lpf[i].real = 0.0; + pilot_lpf[i].imag = 0.0; + + // STM32F4 hand optimized, this alone makes it go done from 1.6 to 1.17ms + // switching pilot_coeff to RAM (by removing const in pilot_coeff.h) would + // save another 0.11 ms at the expense of NPILOTCOEFF * 4 bytes == 120 bytes + // RAM + + if (NPILOTCOEFF % 5 == 0) { + for (k = 0; k < NPILOTCOEFF; k += 5) { + COMP i0 = + fcmult(pilot_coeff[k], pilot_baseband[j - NPILOTCOEFF + 1 + k]); + COMP i1 = fcmult(pilot_coeff[k + 1], + pilot_baseband[j - NPILOTCOEFF + 1 + k + 1]); + COMP i2 = fcmult(pilot_coeff[k + 2], + pilot_baseband[j - NPILOTCOEFF + 1 + k + 2]); + COMP i3 = fcmult(pilot_coeff[k + 3], + pilot_baseband[j - NPILOTCOEFF + 1 + k + 3]); + COMP i4 = fcmult(pilot_coeff[k + 4], + pilot_baseband[j - NPILOTCOEFF + 1 + k + 4]); + + pilot_lpf[i] = + cadd(cadd(cadd(cadd(cadd(pilot_lpf[i], i0), i1), i2), i3), i4); + } + } else { + for (k = 0; k < NPILOTCOEFF; k++) { + pilot_lpf[i] = cadd( + pilot_lpf[i], + fcmult(pilot_coeff[k], pilot_baseband[j - NPILOTCOEFF + 1 + k])); + } + } + } + + /* We only need to do FFTs if we are out of sync. Making them optional saves + CPU in sync, which is when we need to run the codec */ + + imax = 0.0; + *foff = 0.0; + for (i = 0; i < MPILOTFFT; i++) { + S[i].real = 0.0; + S[i].imag = 0.0; + } + + if (do_fft) { + /* decimate to improve DFT resolution, window and DFT */ + mpilot = FS / (2 * 200); /* calc decimation rate given new sample rate is + twice LPF freq */ + for (i = 0, j = 0; i < NPILOTLPF; i += mpilot, j++) { + S[j] = fcmult(hanning[i], pilot_lpf[i]); } - /* We only need to do FFTs if we are out of sync. Making them optional saves CPU in sync, which is when - we need to run the codec */ + codec2_fft_inplace(fft_pilot_cfg, S); - imax = 0.0; - *foff = 0.0; - for(i=0; i<MPILOTFFT; i++) { - S[i].real = 0.0; - S[i].imag = 0.0; - } + /* peak pick and convert to Hz */ - if (do_fft) { - - /* decimate to improve DFT resolution, window and DFT */ - mpilot = FS/(2*200); /* calc decimation rate given new sample rate is twice LPF freq */ - for(i=0,j=0; i<NPILOTLPF; i+=mpilot,j++) { - S[j] = fcmult(hanning[i], pilot_lpf[i]); - } - - codec2_fft_inplace(fft_pilot_cfg, S); - - /* peak pick and convert to Hz */ - - imax = 0.0; - ix = 0; - for(i=0; i<MPILOTFFT; i++) { - mag = S[i].real*S[i].real + S[i].imag*S[i].imag; - if (mag > imax) { - imax = mag; - ix = i; - } - } - r = 2.0*200.0/MPILOTFFT; /* maps FFT bin to frequency in Hz */ - - if (ix >= MPILOTFFT/2) - *foff = (ix - MPILOTFFT)*r; - else - *foff = (ix)*r; + imax = 0.0; + ix = 0; + for (i = 0; i < MPILOTFFT; i++) { + mag = S[i].real * S[i].real + S[i].imag * S[i].imag; + if (mag > imax) { + imax = mag; + ix = i; + } } + r = 2.0 * 200.0 / MPILOTFFT; /* maps FFT bin to frequency in Hz */ - *max = imax; + if (ix >= MPILOTFFT / 2) + *foff = (ix - MPILOTFFT) * r; + else + *foff = (ix)*r; + } + *max = imax; } /*---------------------------------------------------------------------------*\ @@ -795,68 +767,69 @@ void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[], \*---------------------------------------------------------------------------*/ -float rx_est_freq_offset(struct FDMDV *f, COMP rx_fdm[], int nin, int do_fft) -{ - int i; +float rx_est_freq_offset(struct FDMDV *f, COMP rx_fdm[], int nin, int do_fft) { + int i; #ifndef FDV_ARM_MATH - int j; + int j; #endif - COMP pilot[M_FAC+M_FAC/P]; - COMP prev_pilot[M_FAC+M_FAC/P]; - float foff, foff1, foff2; - float max1, max2; - - assert(nin <= M_FAC+M_FAC/P); - - /* get pilot samples used for correlation/down conversion of rx signal */ - - for (i=0; i<nin; i++) { - pilot[i] = f->pilot_lut[f->pilot_lut_index]; - f->pilot_lut_index++; - if (f->pilot_lut_index >= 4*M_FAC) - f->pilot_lut_index = 0; - - prev_pilot[i] = f->pilot_lut[f->prev_pilot_lut_index]; - f->prev_pilot_lut_index++; - if (f->prev_pilot_lut_index >= 4*M_FAC) - f->prev_pilot_lut_index = 0; - } - - /* - Down convert latest M_FAC samples of pilot by multiplying by ideal - BPSK pilot signal we have generated locally. The peak of the - resulting signal is sensitive to the time shift between the - received and local version of the pilot, so we do it twice at - different time shifts and choose the maximum. - */ - - for(i=0; i<NPILOTBASEBAND-nin; i++) { - f->pilot_baseband1[i] = f->pilot_baseband1[i+nin]; - f->pilot_baseband2[i] = f->pilot_baseband2[i+nin]; - } + COMP pilot[M_FAC + M_FAC / P]; + COMP prev_pilot[M_FAC + M_FAC / P]; + float foff, foff1, foff2; + float max1, max2; + + assert(nin <= M_FAC + M_FAC / P); + + /* get pilot samples used for correlation/down conversion of rx signal */ + + for (i = 0; i < nin; i++) { + pilot[i] = f->pilot_lut[f->pilot_lut_index]; + f->pilot_lut_index++; + if (f->pilot_lut_index >= 4 * M_FAC) f->pilot_lut_index = 0; + + prev_pilot[i] = f->pilot_lut[f->prev_pilot_lut_index]; + f->prev_pilot_lut_index++; + if (f->prev_pilot_lut_index >= 4 * M_FAC) f->prev_pilot_lut_index = 0; + } + + /* + Down convert latest M_FAC samples of pilot by multiplying by ideal + BPSK pilot signal we have generated locally. The peak of the + resulting signal is sensitive to the time shift between the + received and local version of the pilot, so we do it twice at + different time shifts and choose the maximum. + */ + + for (i = 0; i < NPILOTBASEBAND - nin; i++) { + f->pilot_baseband1[i] = f->pilot_baseband1[i + nin]; + f->pilot_baseband2[i] = f->pilot_baseband2[i + nin]; + } #ifndef FDV_ARM_MATH - for(i=0,j=NPILOTBASEBAND-nin; i<nin; i++,j++) { - f->pilot_baseband1[j] = cmult(rx_fdm[i], pilot[i]); - f->pilot_baseband2[j] = cmult(rx_fdm[i], prev_pilot[i]); - } + for (i = 0, j = NPILOTBASEBAND - nin; i < nin; i++, j++) { + f->pilot_baseband1[j] = cmult(rx_fdm[i], pilot[i]); + f->pilot_baseband2[j] = cmult(rx_fdm[i], prev_pilot[i]); + } #else - // TODO: Maybe a handwritten mult taking advantage of rx_fdm[0] being - // used twice would be faster but this is for sure faster than - // the implementation above in any case. - arm_cmplx_mult_cmplx_f32(&rx_fdm[0].real,&pilot[0].real,&f->pilot_baseband1[NPILOTBASEBAND-nin].real,nin); - arm_cmplx_mult_cmplx_f32(&rx_fdm[0].real,&prev_pilot[0].real,&f->pilot_baseband2[NPILOTBASEBAND-nin].real,nin); + // TODO: Maybe a handwritten mult taking advantage of rx_fdm[0] being + // used twice would be faster but this is for sure faster than + // the implementation above in any case. + arm_cmplx_mult_cmplx_f32(&rx_fdm[0].real, &pilot[0].real, + &f->pilot_baseband1[NPILOTBASEBAND - nin].real, nin); + arm_cmplx_mult_cmplx_f32(&rx_fdm[0].real, &prev_pilot[0].real, + &f->pilot_baseband2[NPILOTBASEBAND - nin].real, nin); #endif - lpf_peak_pick(&foff1, &max1, f->pilot_baseband1, f->pilot_lpf1, f->fft_pilot_cfg, f->S1, nin, do_fft); - lpf_peak_pick(&foff2, &max2, f->pilot_baseband2, f->pilot_lpf2, f->fft_pilot_cfg, f->S2, nin, do_fft); + lpf_peak_pick(&foff1, &max1, f->pilot_baseband1, f->pilot_lpf1, + f->fft_pilot_cfg, f->S1, nin, do_fft); + lpf_peak_pick(&foff2, &max2, f->pilot_baseband2, f->pilot_lpf2, + f->fft_pilot_cfg, f->S2, nin, do_fft); - if (max1 > max2) - foff = foff1; - else - foff = foff2; + if (max1 > max2) + foff = foff1; + else + foff = foff2; - return foff; + return foff; } /*---------------------------------------------------------------------------*\ @@ -871,24 +844,23 @@ float rx_est_freq_offset(struct FDMDV *f, COMP rx_fdm[], int nin, int do_fft) \*---------------------------------------------------------------------------*/ void fdmdv_freq_shift(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff, - COMP *foff_phase_rect, int nin) -{ - COMP foff_rect; - float mag; - int i; - - foff_rect.real = cosf(2.0*PI*foff/FS); - foff_rect.imag = sinf(2.0*PI*foff/FS); - for(i=0; i<nin; i++) { - *foff_phase_rect = cmult(*foff_phase_rect, foff_rect); - rx_fdm_fcorr[i] = cmult(rx_fdm[i], *foff_phase_rect); - } - - /* normalise digital oscillator as the magnitude can drift over time */ - - mag = cabsolute(*foff_phase_rect); - foff_phase_rect->real /= mag; - foff_phase_rect->imag /= mag; + COMP *foff_phase_rect, int nin) { + COMP foff_rect; + float mag; + int i; + + foff_rect.real = cosf(2.0 * PI * foff / FS); + foff_rect.imag = sinf(2.0 * PI * foff / FS); + for (i = 0; i < nin; i++) { + *foff_phase_rect = cmult(*foff_phase_rect, foff_rect); + rx_fdm_fcorr[i] = cmult(rx_fdm[i], *foff_phase_rect); + } + + /* normalise digital oscillator as the magnitude can drift over time */ + + mag = cabsolute(*foff_phase_rect); + foff_phase_rect->real /= mag; + foff_phase_rect->imag /= mag; } /*---------------------------------------------------------------------------*\ @@ -901,30 +873,30 @@ void fdmdv_freq_shift(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff, \*---------------------------------------------------------------------------*/ -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) -{ - int i,c; - float mag; +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) { + int i, c; + float mag; - /* maximum number of input samples to demod */ + /* maximum number of input samples to demod */ - assert(nin <= (M_FAC+M_FAC/P)); + assert(nin <= (M_FAC + M_FAC / P)); - /* downconvert */ + /* downconvert */ - for (c=0; c<Nc+1; c++) - for (i=0; i<nin; i++) { - phase_rx[c] = cmult(phase_rx[c], freq[c]); - rx_baseband[c][i] = cmult(rx_fdm[i], cconj(phase_rx[c])); - } + for (c = 0; c < Nc + 1; c++) + for (i = 0; i < nin; i++) { + phase_rx[c] = cmult(phase_rx[c], freq[c]); + rx_baseband[c][i] = cmult(rx_fdm[i], cconj(phase_rx[c])); + } - /* normalise digital oscilators as the magnitude can drift over time */ + /* normalise digital oscilators as the magnitude can drift over time */ - for (c=0; c<Nc+1; c++) { - mag = cabsolute(phase_rx[c]); - phase_rx[c].real /= mag; - phase_rx[c].imag /= mag; - } + for (c = 0; c < Nc + 1; c++) { + mag = cabsolute(phase_rx[c]); + phase_rx[c].real /= mag; + phase_rx[c].imag /= mag; + } } /*---------------------------------------------------------------------------*\ @@ -943,42 +915,43 @@ void fdm_downconvert(COMP rx_baseband[NC+1][M_FAC+M_FAC/P], int Nc, COMP rx_fdm[ \*---------------------------------------------------------------------------*/ -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) -{ - int c, i,j,k,l; - int n=M_FAC/P; +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) { + int c, i, j, k, l; + int n = M_FAC / P; - /* rx filter each symbol, generate P filtered output samples for - each symbol. Note we keep filter memory at rate M_FAC, it's just - the filter output at rate P */ + /* rx filter each symbol, generate P filtered output samples for + each symbol. Note we keep filter memory at rate M_FAC, it's just + the filter output at rate P */ - for(i=0, j=0; i<nin; i+=n,j++) { + for (i = 0, j = 0; i < nin; i += n, j++) { + /* latest input sample */ - /* latest input sample */ + for (c = 0; c < Nc + 1; c++) + for (k = NFILTER - n, l = i; k < NFILTER; k++, l++) + rx_filter_memory[c][k] = rx_baseband[c][l]; - for(c=0; c<Nc+1; c++) - for(k=NFILTER-n,l=i; k<NFILTER; k++,l++) - rx_filter_memory[c][k] = rx_baseband[c][l]; + /* convolution (filtering) */ - /* convolution (filtering) */ + for (c = 0; c < Nc + 1; c++) { + rx_filt[c][j].real = 0.0; + rx_filt[c][j].imag = 0.0; + for (k = 0; k < NFILTER; k++) + rx_filt[c][j] = cadd(rx_filt[c][j], + fcmult(gt_alpha5_root[k], rx_filter_memory[c][k])); + } - for(c=0; c<Nc+1; c++) { - rx_filt[c][j].real = 0.0; rx_filt[c][j].imag = 0.0; - for(k=0; k<NFILTER; k++) - rx_filt[c][j] = cadd(rx_filt[c][j], fcmult(gt_alpha5_root[k], rx_filter_memory[c][k])); - } + /* make room for next input sample */ - /* make room for next input sample */ + for (c = 0; c < Nc + 1; c++) + for (k = 0, l = n; k < NFILTER - n; k++, l++) + rx_filter_memory[c][k] = rx_filter_memory[c][l]; + } - for(c=0; c<Nc+1; c++) - for(k=0,l=n; k<NFILTER-n; k++,l++) - rx_filter_memory[c][k] = rx_filter_memory[c][l]; - } - - assert(j <= (P+1)); /* check for any over runs */ + assert(j <= (P + 1)); /* check for any over runs */ } - /*---------------------------------------------------------------------------*\ FUNCTION....: rxdec_filter() @@ -989,23 +962,24 @@ void rx_filter(COMP rx_filt[][P+1], int Nc, COMP rx_baseband[][M_FAC+M_FAC/P], C \*---------------------------------------------------------------------------*/ -void rxdec_filter(COMP rx_fdm_filter[], COMP rx_fdm[], COMP rxdec_lpf_mem[], int nin) { - int i,j,k,st; - - for(i=0; i<NRXDECMEM-nin; i++) - rxdec_lpf_mem[i] = rxdec_lpf_mem[i+nin]; - for(i=0, j=NRXDECMEM-nin; i<nin; i++,j++) - rxdec_lpf_mem[j] = rx_fdm[i]; - - st = NRXDECMEM - nin - NRXDEC + 1; - for(i=0; i<nin; i++) { - rx_fdm_filter[i].real = 0.0; - for(k=0; k<NRXDEC; k++) - rx_fdm_filter[i].real += rxdec_lpf_mem[st+i+k].real * rxdec_coeff[k]; - rx_fdm_filter[i].imag = 0.0; - for(k=0; k<NRXDEC; k++) - rx_fdm_filter[i].imag += rxdec_lpf_mem[st+i+k].imag * rxdec_coeff[k]; - } +void rxdec_filter(COMP rx_fdm_filter[], COMP rx_fdm[], COMP rxdec_lpf_mem[], + int nin) { + int i, j, k, st; + + for (i = 0; i < NRXDECMEM - nin; i++) + rxdec_lpf_mem[i] = rxdec_lpf_mem[i + nin]; + for (i = 0, j = NRXDECMEM - nin; i < nin; i++, j++) + rxdec_lpf_mem[j] = rx_fdm[i]; + + st = NRXDECMEM - nin - NRXDEC + 1; + for (i = 0; i < nin; i++) { + rx_fdm_filter[i].real = 0.0; + for (k = 0; k < NRXDEC; k++) + rx_fdm_filter[i].real += rxdec_lpf_mem[st + i + k].real * rxdec_coeff[k]; + rx_fdm_filter[i].imag = 0.0; + for (k = 0; k < NRXDEC; k++) + rx_fdm_filter[i].imag += rxdec_lpf_mem[st + i + k].imag * rxdec_coeff[k]; + } } /*---------------------------------------------------------------------------*\ @@ -1021,85 +995,86 @@ void rxdec_filter(COMP rx_fdm_filter[], COMP rx_fdm[], COMP rxdec_lpf_mem[], int \*---------------------------------------------------------------------------*/ -static void fir_filter2(float acc[], float mem[], const float coeff[], const unsigned int dec_rate) { - acc[0] = 0.0; - acc[1] = 0.0; +static void fir_filter2(float acc[], float mem[], const float coeff[], + const unsigned int dec_rate) { + acc[0] = 0.0; + acc[1] = 0.0; - float c1,c2,c3,c4,c5,m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,a1,a2; - float* inpCmplx = &mem[0]; - const float* coeffPtr = &coeff[0]; + float c1, c2, c3, c4, c5, m1, m2, m3, m4, m5, m6, m7, m8, m9, m10, a1, a2; + float *inpCmplx = &mem[0]; + const float *coeffPtr = &coeff[0]; - int m; + int m; - // this manual loop unrolling gives significant boost on STM32 machines - // reduction from avg 3.2ms to 2.4ms in tfdmv.c test - // 5 was the sweet spot, with 6 it took longer again - // and should not harm other, more powerful machines - // no significant difference in output, only rounding (which was to be expected) - // TODO: try to move coeffs to RAM and check if it makes a significant difference - if (NFILTER%(dec_rate*5) == 0) { - for(m=0; m<NFILTER; m+=dec_rate*5) { - c1 = *coeffPtr; + // this manual loop unrolling gives significant boost on STM32 machines + // reduction from avg 3.2ms to 2.4ms in tfdmv.c test + // 5 was the sweet spot, with 6 it took longer again + // and should not harm other, more powerful machines + // no significant difference in output, only rounding (which was to be + // expected) + // TODO: try to move coeffs to RAM and check if it makes a significant + // difference + if (NFILTER % (dec_rate * 5) == 0) { + for (m = 0; m < NFILTER; m += dec_rate * 5) { + c1 = *coeffPtr; - m1 = inpCmplx[0]; - m2 = inpCmplx[1]; + m1 = inpCmplx[0]; + m2 = inpCmplx[1]; - inpCmplx+= dec_rate*2; - coeffPtr+= dec_rate; + inpCmplx += dec_rate * 2; + coeffPtr += dec_rate; - c2 = *coeffPtr; - m3 = inpCmplx[0]; - m4 = inpCmplx[1]; + c2 = *coeffPtr; + m3 = inpCmplx[0]; + m4 = inpCmplx[1]; - inpCmplx+= dec_rate*2; - coeffPtr+= dec_rate; + inpCmplx += dec_rate * 2; + coeffPtr += dec_rate; - c3 = *coeffPtr; - m5 = inpCmplx[0]; - m6 = inpCmplx[1]; + c3 = *coeffPtr; + m5 = inpCmplx[0]; + m6 = inpCmplx[1]; - inpCmplx+= dec_rate*2; - coeffPtr+= dec_rate; + inpCmplx += dec_rate * 2; + coeffPtr += dec_rate; - c4 = *coeffPtr; - m7 = inpCmplx[0]; - m8 = inpCmplx[1]; + c4 = *coeffPtr; + m7 = inpCmplx[0]; + m8 = inpCmplx[1]; - inpCmplx+= dec_rate*2; - coeffPtr+= dec_rate; + inpCmplx += dec_rate * 2; + coeffPtr += dec_rate; - c5 = *coeffPtr; - m9 = inpCmplx[0]; - m10 = inpCmplx[1]; + c5 = *coeffPtr; + m9 = inpCmplx[0]; + m10 = inpCmplx[1]; - inpCmplx+= dec_rate*2; - coeffPtr+= dec_rate; + inpCmplx += dec_rate * 2; + coeffPtr += dec_rate; - a1 = c1 * m1 + c2 * m3 + c3 * m5 + c4 * m7 + c5 * m9; - a2 = c1 * m2 + c2 * m4 + c3 * m6 + c4 * m8 + c5 * m10; - acc[0] += a1; - acc[1] += a2; - } + a1 = c1 * m1 + c2 * m3 + c3 * m5 + c4 * m7 + c5 * m9; + a2 = c1 * m2 + c2 * m4 + c3 * m6 + c4 * m8 + c5 * m10; + acc[0] += a1; + acc[1] += a2; } - else - { - for(m=0; m<NFILTER; m+=dec_rate) { - c1 = *coeffPtr; + } else { + for (m = 0; m < NFILTER; m += dec_rate) { + c1 = *coeffPtr; - m1 = inpCmplx[0]; - m2 = inpCmplx[1]; + m1 = inpCmplx[0]; + m2 = inpCmplx[1]; - inpCmplx+= dec_rate*2; - coeffPtr+= dec_rate; + inpCmplx += dec_rate * 2; + coeffPtr += dec_rate; - a1 = c1 * m1; - a2 = c1 * m2; - acc[0] += a1; - acc[1] += a2; - } + a1 = c1 * m1; + a2 = c1 * m2; + acc[0] += a1; + acc[1] += a2; } - acc[0] *= dec_rate; - acc[1] *= dec_rate; + } + acc[0] *= dec_rate; + acc[1] *= dec_rate; } /*---------------------------------------------------------------------------*\ @@ -1122,19 +1097,19 @@ static void fir_filter2(float acc[], float mem[], const float coeff[], const uns TODO: [ ] windback phase calculated once at init time */ -void 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) -{ - int i,k,c,st,Nval; - float windback_phase, mag; - COMP windback_phase_rect; - COMP rx_baseband[NRX_FDM_MEM]; - COMP f_rect; +void 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) { + int i, k, c, st, Nval; + float windback_phase, mag; + COMP windback_phase_rect; + COMP rx_baseband[NRX_FDM_MEM]; + COMP f_rect; - //PROFILE_VAR(windback_start, downconvert_start, filter_start); + // PROFILE_VAR(windback_start, downconvert_start, filter_start); - /* update memory of rx_fdm */ + /* update memory of rx_fdm */ #if 0 for(i=0; i<NRX_FDM_MEM-nin; i++) @@ -1142,83 +1117,85 @@ void down_convert_and_rx_filter(COMP rx_filt[NC+1][P+1], int Nc, COMP rx_fdm[], for(i=NFILTER+M_FAC-nin, k=0; i<NFILTER+M_FAC; i++, k++) rx_fdm_mem[i] = rx_fdm[k]; #else - // this gives only 40uS gain on STM32 but now that we have, we keep it - memmove(&rx_fdm_mem[0],&rx_fdm_mem[nin],(NRX_FDM_MEM-nin)*sizeof(COMP)); - memcpy(&rx_fdm_mem[NRX_FDM_MEM-nin],&rx_fdm[0],nin*sizeof(COMP)); + // this gives only 40uS gain on STM32 but now that we have, we keep it + memmove(&rx_fdm_mem[0], &rx_fdm_mem[nin], (NRX_FDM_MEM - nin) * sizeof(COMP)); + memcpy(&rx_fdm_mem[NRX_FDM_MEM - nin], &rx_fdm[0], nin * sizeof(COMP)); #endif - for(c=0; c<Nc+1; c++) { - - /* + for (c = 0; c < Nc + 1; c++) { + /* - So we have rx_fdm_mem, a baseband array of samples at - rate Fs Hz, including the last nin samples at the end. To - filter each symbol we require the baseband samples for all Nsym - symbols that we filter over. So we need to downconvert the - entire rx_fdm_mem array. To downconvert these we need the LO - phase referenced to the start of the rx_fdm_mem array. + So we have rx_fdm_mem, a baseband array of samples at + rate Fs Hz, including the last nin samples at the end. To + filter each symbol we require the baseband samples for all Nsym + symbols that we filter over. So we need to downconvert the + entire rx_fdm_mem array. To downconvert these we need the LO + phase referenced to the start of the rx_fdm_mem array. - <--------------- Nrx_filt_mem -------> - nin - |--------------------------|---------| - 1 | - phase_rx(c) + <--------------- Nrx_filt_mem -------> + nin + |--------------------------|---------| + 1 | + phase_rx(c) - This means winding phase(c) back from this point - to ensure phase continuity. + This means winding phase(c) back from this point + to ensure phase continuity. - */ + */ - //PROFILE_SAMPLE(windback_start); - windback_phase = -freq_pol[c]*NFILTER; - windback_phase_rect.real = cosf(windback_phase); - windback_phase_rect.imag = sinf(windback_phase); - phase_rx[c] = cmult(phase_rx[c],windback_phase_rect); - //PROFILE_SAMPLE_AND_LOG(downconvert_start, windback_start, " windback"); + // PROFILE_SAMPLE(windback_start); + windback_phase = -freq_pol[c] * NFILTER; + windback_phase_rect.real = cosf(windback_phase); + windback_phase_rect.imag = sinf(windback_phase); + phase_rx[c] = cmult(phase_rx[c], windback_phase_rect); + // PROFILE_SAMPLE_AND_LOG(downconvert_start, windback_start, " windback"); - /* down convert all samples in buffer */ + /* down convert all samples in buffer */ - st = NRX_FDM_MEM-1; /* end of buffer */ - st -= nin-1; /* first new sample */ - st -= NFILTER; /* first sample used in filtering */ + st = NRX_FDM_MEM - 1; /* end of buffer */ + st -= nin - 1; /* first new sample */ + st -= NFILTER; /* first sample used in filtering */ - /* freq shift per dec_rate step is dec_rate times original shift */ + /* freq shift per dec_rate step is dec_rate times original shift */ - f_rect = freq[c]; - for(i=0; i<dec_rate-1; i++) - f_rect = cmult(f_rect,freq[c]); + f_rect = freq[c]; + for (i = 0; i < dec_rate - 1; i++) f_rect = cmult(f_rect, freq[c]); - for(i=st; i<NRX_FDM_MEM; i+=dec_rate) { - phase_rx[c] = cmult(phase_rx[c], f_rect); - rx_baseband[i] = cmult(rx_fdm_mem[i],cconj(phase_rx[c])); - } - //PROFILE_SAMPLE_AND_LOG(filter_start, downconvert_start, " downconvert"); + for (i = st; i < NRX_FDM_MEM; i += dec_rate) { + phase_rx[c] = cmult(phase_rx[c], f_rect); + rx_baseband[i] = cmult(rx_fdm_mem[i], cconj(phase_rx[c])); + } + // PROFILE_SAMPLE_AND_LOG(filter_start, downconvert_start, " downconvert"); - /* now we can filter this carrier's P symbols */ + /* now we can filter this carrier's P symbols */ - Nval=M_FAC/P; - for(i=0, k=0; i<nin; i+=Nval, k++) { + Nval = M_FAC / P; + for (i = 0, k = 0; i < nin; i += Nval, k++) { #ifdef ORIG - rx_filt[c][k].real = 0.0; rx_filt[c][k].imag = 0.0; + rx_filt[c][k].real = 0.0; + rx_filt[c][k].imag = 0.0; - for(m=0; m<NFILTER; m++) - rx_filt[c][k] = cadd(rx_filt[c][k], fcmult(gt_alpha5_root[m], rx_baseband[st+i+m])); + for (m = 0; m < NFILTER; m++) + rx_filt[c][k] = cadd( + rx_filt[c][k], fcmult(gt_alpha5_root[m], rx_baseband[st + i + m])); #else - // rx_filt[c][k].real = fir_filter(&rx_baseband[st+i].real, (float*)gt_alpha5_root, dec_rate); - // rx_filt[c][k].imag = fir_filter(&rx_baseband[st+i].imag, (float*)gt_alpha5_root, dec_rate); - fir_filter2(&rx_filt[c][k].real,&rx_baseband[st+i].real, gt_alpha5_root, dec_rate); + // rx_filt[c][k].real = fir_filter(&rx_baseband[st+i].real, + // (float*)gt_alpha5_root, dec_rate); rx_filt[c][k].imag = + // fir_filter(&rx_baseband[st+i].imag, (float*)gt_alpha5_root, dec_rate); + fir_filter2(&rx_filt[c][k].real, &rx_baseband[st + i].real, + gt_alpha5_root, dec_rate); #endif - } - //PROFILE_SAMPLE_AND_LOG2(filter_start, " filter"); + } + // PROFILE_SAMPLE_AND_LOG2(filter_start, " filter"); - /* normalise digital oscilators as the magnitude can drift over time */ + /* normalise digital oscilators as the magnitude can drift over time */ - mag = cabsolute(phase_rx[c]); - phase_rx[c].real /= mag; - phase_rx[c].imag /= mag; + mag = cabsolute(phase_rx[c]); + phase_rx[c].real /= mag; + phase_rx[c].imag /= mag; - //printf("phase_rx[%d] = %f %f\n", c, phase_rx[c].real, phase_rx[c].imag); - } + // printf("phase_rx[%d] = %f %f\n", c, phase_rx[c].real, phase_rx[c].imag); + } } /*---------------------------------------------------------------------------*\ @@ -1232,94 +1209,91 @@ void down_convert_and_rx_filter(COMP rx_filt[NC+1][P+1], int Nc, COMP rx_fdm[], \*---------------------------------------------------------------------------*/ -float 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) -{ - int c,i,j; - int adjust; - COMP x, phase, freq; - float rx_timing, fract, norm_rx_timing; - int low_sample, high_sample; - - /* - nin adjust - -------------------------------- - 120 -1 (one less rate P sample) - 160 0 (nominal) - 200 1 (one more rate P sample) - */ - - adjust = P - nin*P/m; - - /* update buffer of NT rate P filtered symbols */ - - for(c=0; c<Nc+1; c++) - for(i=0,j=P-adjust; i<(NT-1)*P+adjust; i++,j++) - rx_filter_mem_timing[c][i] = rx_filter_mem_timing[c][j]; - for(c=0; c<Nc+1; c++) - for(i=(NT-1)*P+adjust,j=0; i<NT*P; i++,j++) - rx_filter_mem_timing[c][i] = rx_filt[c][j]; - - /* sum envelopes of all carriers */ - - for(i=0; i<NT*P; i++) { - env[i] = 0.0; - for(c=0; c<Nc+1; c++) - env[i] += cabsolute(rx_filter_mem_timing[c][i]); - } - - /* The envelope has a frequency component at the symbol rate. The - phase of this frequency component indicates the timing. So work - out single DFT at frequency 2*pi/P */ - - x.real = 0.0; x.imag = 0.0; - freq.real = cosf(2*PI/P); - freq.imag = sinf(2*PI/P); - phase.real = 1.0; - phase.imag = 0.0; - - for(i=0; i<NT*P; i++) { - x = cadd(x, fcmult(env[i], phase)); - phase = cmult(phase, freq); - } - - /* Map phase to estimated optimum timing instant at rate P. The - P/4 part was adjusted by experiment, I know not why.... */ - - norm_rx_timing = atan2f(x.imag, x.real)/(2*PI); - assert(fabsf(norm_rx_timing) < 1.0); - rx_timing = norm_rx_timing*P + P/4; - - if (rx_timing > P) - rx_timing -= P; - if (rx_timing < -P) - rx_timing += P; - - /* rx_filter_mem_timing contains Nt*P samples (Nt symbols at rate - P), where Nt is odd. Lets use linear interpolation to resample - in the centre of the timing estimation window .*/ - - rx_timing += floorf(NT/2.0)*P; - low_sample = floorf(rx_timing); - fract = rx_timing - low_sample; - high_sample = ceilf(rx_timing); - - //printf("rx_timing: %f low_sample: %d high_sample: %d fract: %f\n", rx_timing, low_sample, high_sample, fract); - - for(c=0; c<Nc+1; c++) { - 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])); - //rx_symbols[c] = rx_filter_mem_timing[c][high_sample]; - } - - /* This value will be +/- half a symbol so will wrap around at +/- - M/2 or +/- 80 samples with M=160 */ - - return norm_rx_timing*m; +float 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) { + int c, i, j; + int adjust; + COMP x, phase, freq; + float rx_timing, fract, norm_rx_timing; + int low_sample, high_sample; + + /* + nin adjust + -------------------------------- + 120 -1 (one less rate P sample) + 160 0 (nominal) + 200 1 (one more rate P sample) + */ + + adjust = P - nin * P / m; + + /* update buffer of NT rate P filtered symbols */ + + for (c = 0; c < Nc + 1; c++) + for (i = 0, j = P - adjust; i < (NT - 1) * P + adjust; i++, j++) + rx_filter_mem_timing[c][i] = rx_filter_mem_timing[c][j]; + for (c = 0; c < Nc + 1; c++) + for (i = (NT - 1) * P + adjust, j = 0; i < NT * P; i++, j++) + rx_filter_mem_timing[c][i] = rx_filt[c][j]; + + /* sum envelopes of all carriers */ + + for (i = 0; i < NT * P; i++) { + env[i] = 0.0; + for (c = 0; c < Nc + 1; c++) + env[i] += cabsolute(rx_filter_mem_timing[c][i]); + } + + /* The envelope has a frequency component at the symbol rate. The + phase of this frequency component indicates the timing. So work + out single DFT at frequency 2*pi/P */ + + x.real = 0.0; + x.imag = 0.0; + freq.real = cosf(2 * PI / P); + freq.imag = sinf(2 * PI / P); + phase.real = 1.0; + phase.imag = 0.0; + + for (i = 0; i < NT * P; i++) { + x = cadd(x, fcmult(env[i], phase)); + phase = cmult(phase, freq); + } + + /* Map phase to estimated optimum timing instant at rate P. The + P/4 part was adjusted by experiment, I know not why.... */ + + norm_rx_timing = atan2f(x.imag, x.real) / (2 * PI); + assert(fabsf(norm_rx_timing) < 1.0); + rx_timing = norm_rx_timing * P + P / 4; + + if (rx_timing > P) rx_timing -= P; + if (rx_timing < -P) rx_timing += P; + + /* rx_filter_mem_timing contains Nt*P samples (Nt symbols at rate + P), where Nt is odd. Lets use linear interpolation to resample + in the centre of the timing estimation window .*/ + + rx_timing += floorf(NT / 2.0) * P; + low_sample = floorf(rx_timing); + fract = rx_timing - low_sample; + high_sample = ceilf(rx_timing); + + // printf("rx_timing: %f low_sample: %d high_sample: %d fract: %f\n", + // rx_timing, low_sample, high_sample, fract); + + for (c = 0; c < Nc + 1; c++) { + 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])); + // rx_symbols[c] = rx_filter_mem_timing[c][high_sample]; + } + + /* This value will be +/- half a symbol so will wrap around at +/- + M/2 or +/- 80 samples with M=160 */ + + return norm_rx_timing * m; } /*---------------------------------------------------------------------------*\ @@ -1334,73 +1308,80 @@ float rx_est_timing(COMP rx_symbols[], \*---------------------------------------------------------------------------*/ -float 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) -{ - int c; - COMP d; - int msb=0, lsb=0; - float ferr, norm; - - - /* Extra 45 degree clockwise lets us use real and imag axis as - decision boundaries. "norm" makes sure the phase subtraction - from the previous symbol doesn't affect the amplitude, which - leads to sensible scatter plots */ - - for(c=0; c<Nc; c++) { - norm = 1.0/(cabsolute(prev_rx_symbols[c])+1E-6); - phase_difference[c] = cmult(cmult(rx_symbols[c], fcmult(norm,cconj(prev_rx_symbols[c]))), pi_on_4); +float 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) { + int c; + COMP d; + int msb = 0, lsb = 0; + float ferr, norm; + + /* Extra 45 degree clockwise lets us use real and imag axis as + decision boundaries. "norm" makes sure the phase subtraction + from the previous symbol doesn't affect the amplitude, which + leads to sensible scatter plots */ + + for (c = 0; c < Nc; c++) { + norm = 1.0 / (cabsolute(prev_rx_symbols[c]) + 1E-6); + phase_difference[c] = cmult( + cmult(rx_symbols[c], fcmult(norm, cconj(prev_rx_symbols[c]))), pi_on_4); + } + + /* map (Nc,1) DQPSK symbols back into an (1,Nc*Nb) array of bits */ + + for (c = 0; c < Nc; c++) { + d = phase_difference[c]; + if ((d.real >= 0) && (d.imag >= 0)) { + msb = 0; + lsb = 0; } - - /* map (Nc,1) DQPSK symbols back into an (1,Nc*Nb) array of bits */ - - for (c=0; c<Nc; c++) { - d = phase_difference[c]; - if ((d.real >= 0) && (d.imag >= 0)) { - msb = 0; lsb = 0; - } - if ((d.real < 0) && (d.imag >= 0)) { - msb = 0; lsb = 1; - } - if ((d.real < 0) && (d.imag < 0)) { - if (old_qpsk_mapping) { - msb = 1; lsb = 0; - } else { - msb = 1; lsb = 1; - } - } - if ((d.real >= 0) && (d.imag < 0)) { - if (old_qpsk_mapping) { - msb = 1; lsb = 1; - } else { - msb = 1; lsb = 0; - } - } - rx_bits[2*c] = msb; - rx_bits[2*c+1] = lsb; + if ((d.real < 0) && (d.imag >= 0)) { + msb = 0; + lsb = 1; } - - /* Extract DBPSK encoded Sync bit and fine freq offset estimate */ - - norm = 1.0/(cabsolute(prev_rx_symbols[Nc])+1E-6); - phase_difference[Nc] = cmult(rx_symbols[Nc], fcmult(norm, cconj(prev_rx_symbols[Nc]))); - if (phase_difference[Nc].real < 0) { - *sync_bit = 1; - ferr = phase_difference[Nc].imag*norm; /* make f_err magnitude insensitive */ + if ((d.real < 0) && (d.imag < 0)) { + if (old_qpsk_mapping) { + msb = 1; + lsb = 0; + } else { + msb = 1; + lsb = 1; + } } - else { - *sync_bit = 0; - ferr = -phase_difference[Nc].imag*norm; + if ((d.real >= 0) && (d.imag < 0)) { + if (old_qpsk_mapping) { + msb = 1; + lsb = 1; + } else { + msb = 1; + lsb = 0; + } } - - /* pilot carrier gets an extra pi/4 rotation to make it consistent - with other carriers, as we need it for snr_update and scatter - diagram */ - - phase_difference[Nc] = cmult(phase_difference[Nc], pi_on_4); - - return ferr; + rx_bits[2 * c] = msb; + rx_bits[2 * c + 1] = lsb; + } + + /* Extract DBPSK encoded Sync bit and fine freq offset estimate */ + + norm = 1.0 / (cabsolute(prev_rx_symbols[Nc]) + 1E-6); + phase_difference[Nc] = + cmult(rx_symbols[Nc], fcmult(norm, cconj(prev_rx_symbols[Nc]))); + if (phase_difference[Nc].real < 0) { + *sync_bit = 1; + ferr = + phase_difference[Nc].imag * norm; /* make f_err magnitude insensitive */ + } else { + *sync_bit = 0; + ferr = -phase_difference[Nc].imag * norm; + } + + /* pilot carrier gets an extra pi/4 rotation to make it consistent + with other carriers, as we need it for snr_update and scatter + diagram */ + + phase_difference[Nc] = cmult(phase_difference[Nc], pi_on_4); + + return ferr; } /*---------------------------------------------------------------------------*\ @@ -1413,50 +1394,46 @@ float qpsk_to_bits(int rx_bits[], int *sync_bit, int Nc, COMP phase_difference[] \*---------------------------------------------------------------------------*/ -void snr_update(float sig_est[], float noise_est[], int Nc, COMP phase_difference[]) -{ - float s[NC+1]; - COMP refl_symbols[NC+1]; - float n[NC+1]; - int c; - +void snr_update(float sig_est[], float noise_est[], int Nc, + COMP phase_difference[]) { + float s[NC + 1]; + COMP refl_symbols[NC + 1]; + float n[NC + 1]; + int c; - /* mag of each symbol is distance from origin, this gives us a - vector of mags, one for each carrier. */ + /* mag of each symbol is distance from origin, this gives us a + vector of mags, one for each carrier. */ - for(c=0; c<Nc+1; c++) - s[c] = cabsolute(phase_difference[c]); + for (c = 0; c < Nc + 1; c++) s[c] = cabsolute(phase_difference[c]); - /* signal mag estimate for each carrier is a smoothed version of - instantaneous magntitude, this gives us a vector of smoothed - mag estimates, one for each carrier. */ + /* signal mag estimate for each carrier is a smoothed version of + instantaneous magntitude, this gives us a vector of smoothed + mag estimates, one for each carrier. */ - for(c=0; c<Nc+1; c++) - sig_est[c] = SNR_COEFF*sig_est[c] + (1.0 - SNR_COEFF)*s[c]; + for (c = 0; c < Nc + 1; c++) + sig_est[c] = SNR_COEFF * sig_est[c] + (1.0 - SNR_COEFF) * s[c]; - /* noise mag estimate is distance of current symbol from average - location of that symbol. We reflect all symbols into the first - quadrant for convenience. */ + /* noise mag estimate is distance of current symbol from average + location of that symbol. We reflect all symbols into the first + quadrant for convenience. */ - for(c=0; c<Nc+1; c++) { - refl_symbols[c].real = fabsf(phase_difference[c].real); - refl_symbols[c].imag = fabsf(phase_difference[c].imag); - n[c] = cabsolute(cadd(fcmult(sig_est[c], pi_on_4), cneg(refl_symbols[c]))); - } + for (c = 0; c < Nc + 1; c++) { + refl_symbols[c].real = fabsf(phase_difference[c].real); + refl_symbols[c].imag = fabsf(phase_difference[c].imag); + n[c] = cabsolute(cadd(fcmult(sig_est[c], pi_on_4), cneg(refl_symbols[c]))); + } - /* noise mag estimate for each carrier is a smoothed version of - instantaneous noise mag, this gives us a vector of smoothed - noise power estimates, one for each carrier. */ + /* noise mag estimate for each carrier is a smoothed version of + instantaneous noise mag, this gives us a vector of smoothed + noise power estimates, one for each carrier. */ - for(c=0; c<Nc+1; c++) - noise_est[c] = SNR_COEFF*noise_est[c] + (1 - SNR_COEFF)*n[c]; + for (c = 0; c < Nc + 1; c++) + noise_est[c] = SNR_COEFF * noise_est[c] + (1 - SNR_COEFF) * n[c]; } // returns number of shorts in error_pattern[], one short per error -int fdmdv_error_pattern_size(struct FDMDV *f) { - return f->ntest_bits; -} +int fdmdv_error_pattern_size(struct FDMDV *f) { return f->ntest_bits; } /*---------------------------------------------------------------------------*\ @@ -1470,38 +1447,36 @@ int fdmdv_error_pattern_size(struct FDMDV *f) { \*---------------------------------------------------------------------------*/ void fdmdv_put_test_bits(struct FDMDV *f, int *sync, short error_pattern[], - int *bit_errors, int *ntest_bits, int rx_bits[]) -{ - int i,j; - float ber; - int bits_per_frame = fdmdv_bits_per_frame(f); - - /* Append to our memory */ - - for(i=0,j=bits_per_frame; i<f->ntest_bits-bits_per_frame; i++,j++) - f->rx_test_bits_mem[i] = f->rx_test_bits_mem[j]; - for(i=f->ntest_bits-bits_per_frame,j=0; i<f->ntest_bits; i++,j++) - f->rx_test_bits_mem[i] = rx_bits[j]; - - /* see how many bit errors we get when checked against test sequence */ - - *bit_errors = 0; - for(i=0; i<f->ntest_bits; i++) { - error_pattern[i] = test_bits[i] ^ f->rx_test_bits_mem[i]; - *bit_errors += error_pattern[i]; - //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]); - } + int *bit_errors, int *ntest_bits, int rx_bits[]) { + int i, j; + float ber; + int bits_per_frame = fdmdv_bits_per_frame(f); + + /* Append to our memory */ + + for (i = 0, j = bits_per_frame; i < f->ntest_bits - bits_per_frame; i++, j++) + f->rx_test_bits_mem[i] = f->rx_test_bits_mem[j]; + for (i = f->ntest_bits - bits_per_frame, j = 0; i < f->ntest_bits; i++, j++) + f->rx_test_bits_mem[i] = rx_bits[j]; + + /* see how many bit errors we get when checked against test sequence */ - /* if less than a thresh we are aligned and in sync with test sequence */ + *bit_errors = 0; + for (i = 0; i < f->ntest_bits; i++) { + error_pattern[i] = test_bits[i] ^ f->rx_test_bits_mem[i]; + *bit_errors += error_pattern[i]; + // 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]); + } - ber = (float)*bit_errors/f->ntest_bits; + /* if less than a thresh we are aligned and in sync with test sequence */ - *sync = 0; - if (ber < 0.2) - *sync = 1; + ber = (float)*bit_errors / f->ntest_bits; - *ntest_bits = f->ntest_bits; + *sync = 0; + if (ber < 0.2) *sync = 1; + *ntest_bits = f->ntest_bits; } /*---------------------------------------------------------------------------*\ @@ -1528,67 +1503,64 @@ void fdmdv_put_test_bits(struct FDMDV *f, int *sync, short error_pattern[], \*---------------------------------------------------------------------------*/ -int freq_state(int *reliable_sync_bit, int sync_bit, int *state, int *timer, int *sync_mem) -{ - int next_state, sync, unique_word, i, corr; +int freq_state(int *reliable_sync_bit, int sync_bit, int *state, int *timer, + int *sync_mem) { + int next_state, sync, unique_word, i, corr; - /* look for 6 symbols (120ms) 101010 of sync sequence */ + /* look for 6 symbols (120ms) 101010 of sync sequence */ - unique_word = 0; - for(i=0; i<NSYNC_MEM-1; i++) - sync_mem[i] = sync_mem[i+1]; - sync_mem[i] = 1 - 2*sync_bit; - corr = 0; - for(i=0; i<NSYNC_MEM; i++) - corr += sync_mem[i]*sync_uw[i]; - if (abs(corr) == NSYNC_MEM) - unique_word = 1; - *reliable_sync_bit = (corr == NSYNC_MEM); + unique_word = 0; + for (i = 0; i < NSYNC_MEM - 1; i++) sync_mem[i] = sync_mem[i + 1]; + sync_mem[i] = 1 - 2 * sync_bit; + corr = 0; + for (i = 0; i < NSYNC_MEM; i++) corr += sync_mem[i] * sync_uw[i]; + if (abs(corr) == NSYNC_MEM) unique_word = 1; + *reliable_sync_bit = (corr == NSYNC_MEM); - /* iterate state machine */ + /* iterate state machine */ - next_state = *state; - switch(*state) { + next_state = *state; + switch (*state) { case 0: - if (unique_word) { - next_state = 1; - *timer = 0; - } - break; - case 1: /* tentative sync state */ - if (unique_word) { - (*timer)++; - if (*timer == 25) /* sync has been good for 500ms */ - next_state = 2; - } - else - next_state = 0; /* quickly fall out of sync */ - break; - case 2: /* good sync state */ - if (unique_word == 0) { - *timer = 0; - next_state = 3; - } - break; - case 3: /* tentative bad state, but could be a fade */ - if (unique_word) - next_state = 2; - else { - (*timer)++; - if (*timer == 50) /* wait for 1000ms in case sync comes back */ - next_state = 0; - } - break; - } - - //printf("state: %d next_state: %d uw: %d timer: %d\n", *state, next_state, unique_word, *timer); - *state = next_state; - if (*state) - sync = 1; - else - sync = 0; - - return sync; + if (unique_word) { + next_state = 1; + *timer = 0; + } + break; + case 1: /* tentative sync state */ + if (unique_word) { + (*timer)++; + if (*timer == 25) /* sync has been good for 500ms */ + next_state = 2; + } else + next_state = 0; /* quickly fall out of sync */ + break; + case 2: /* good sync state */ + if (unique_word == 0) { + *timer = 0; + next_state = 3; + } + break; + case 3: /* tentative bad state, but could be a fade */ + if (unique_word) + next_state = 2; + else { + (*timer)++; + if (*timer == 50) /* wait for 1000ms in case sync comes back */ + next_state = 0; + } + break; + } + + // printf("state: %d next_state: %d uw: %d timer: %d\n", *state, next_state, + // unique_word, *timer); + *state = next_state; + if (*state) + sync = 1; + else + sync = 0; + + return sync; } /*---------------------------------------------------------------------------*\ @@ -1611,67 +1583,77 @@ int freq_state(int *reliable_sync_bit, int sync_bit, int *state, int *timer, int \*---------------------------------------------------------------------------*/ - -void fdmdv_demod(struct FDMDV *fdmdv, int rx_bits[], - int *reliable_sync_bit, COMP rx_fdm[], int *nin) -{ - float foff_coarse, foff_fine; - COMP rx_fdm_fcorr[M_FAC+M_FAC/P]; - COMP rx_fdm_filter[M_FAC+M_FAC/P]; - COMP rx_fdm_bb[M_FAC+M_FAC/P]; - COMP rx_filt[NC+1][P+1]; - COMP rx_symbols[NC+1]; - float env[NT*P]; - int sync_bit; - PROFILE_VAR(demod_start, fdmdv_freq_shift_start, down_convert_and_rx_filter_start); - PROFILE_VAR(rx_est_timing_start, qpsk_to_bits_start, snr_update_start, freq_state_start); - - /* shift down to complex baseband */ - - fdmdv_freq_shift(rx_fdm_bb, rx_fdm, -FDMDV_FCENTRE, &fdmdv->fbb_phase_rx, *nin); - - /* freq offset estimation and correction */ - - PROFILE_SAMPLE(demod_start); - foff_coarse = rx_est_freq_offset(fdmdv, rx_fdm_bb, *nin, !fdmdv->sync); - PROFILE_SAMPLE_AND_LOG(fdmdv_freq_shift_start, demod_start, " rx_est_freq_offset"); - - if (fdmdv->sync == 0) - fdmdv->foff = foff_coarse; - fdmdv_freq_shift(rx_fdm_fcorr, rx_fdm_bb, -fdmdv->foff, &fdmdv->foff_phase_rect, *nin); - PROFILE_SAMPLE_AND_LOG(down_convert_and_rx_filter_start, fdmdv_freq_shift_start, " fdmdv_freq_shift"); - - /* baseband processing */ - - rxdec_filter(rx_fdm_filter, rx_fdm_fcorr, fdmdv->rxdec_lpf_mem, *nin); - down_convert_and_rx_filter(rx_filt, fdmdv->Nc, rx_fdm_filter, fdmdv->rx_fdm_mem, fdmdv->phase_rx, fdmdv->freq, - fdmdv->freq_pol, *nin, M_FAC/Q); - PROFILE_SAMPLE_AND_LOG(rx_est_timing_start, down_convert_and_rx_filter_start, " down_convert_and_rx_filter"); - fdmdv->rx_timing = rx_est_timing(rx_symbols, fdmdv->Nc, rx_filt, fdmdv->rx_filter_mem_timing, env, *nin, M_FAC); - PROFILE_SAMPLE_AND_LOG(qpsk_to_bits_start, rx_est_timing_start, " rx_est_timing"); - - /* Adjust number of input samples to keep timing within bounds */ - - *nin = M_FAC; - - if (fdmdv->rx_timing > M_FAC/P) - *nin += M_FAC/P; - - if (fdmdv->rx_timing < -M_FAC/P) - *nin -= M_FAC/P; - - foff_fine = qpsk_to_bits(rx_bits, &sync_bit, fdmdv->Nc, fdmdv->phase_difference, fdmdv->prev_rx_symbols, rx_symbols, - fdmdv->old_qpsk_mapping); - memcpy(fdmdv->prev_rx_symbols, rx_symbols, sizeof(COMP)*(fdmdv->Nc+1)); - PROFILE_SAMPLE_AND_LOG(snr_update_start, qpsk_to_bits_start, " qpsk_to_bits"); - snr_update(fdmdv->sig_est, fdmdv->noise_est, fdmdv->Nc, fdmdv->phase_difference); - PROFILE_SAMPLE_AND_LOG(freq_state_start, snr_update_start, " snr_update"); - - /* freq offset estimation state machine */ - - fdmdv->sync = freq_state(reliable_sync_bit, sync_bit, &fdmdv->fest_state, &fdmdv->timer, fdmdv->sync_mem); - PROFILE_SAMPLE_AND_LOG2(freq_state_start, " freq_state"); - fdmdv->foff -= TRACK_COEFF*foff_fine; +void fdmdv_demod(struct FDMDV *fdmdv, int rx_bits[], int *reliable_sync_bit, + COMP rx_fdm[], int *nin) { + float foff_coarse, foff_fine; + COMP rx_fdm_fcorr[M_FAC + M_FAC / P]; + COMP rx_fdm_filter[M_FAC + M_FAC / P]; + COMP rx_fdm_bb[M_FAC + M_FAC / P]; + COMP rx_filt[NC + 1][P + 1]; + COMP rx_symbols[NC + 1]; + float env[NT * P]; + int sync_bit; + PROFILE_VAR(demod_start, fdmdv_freq_shift_start, + down_convert_and_rx_filter_start); + PROFILE_VAR(rx_est_timing_start, qpsk_to_bits_start, snr_update_start, + freq_state_start); + + /* shift down to complex baseband */ + + fdmdv_freq_shift(rx_fdm_bb, rx_fdm, -FDMDV_FCENTRE, &fdmdv->fbb_phase_rx, + *nin); + + /* freq offset estimation and correction */ + + PROFILE_SAMPLE(demod_start); + foff_coarse = rx_est_freq_offset(fdmdv, rx_fdm_bb, *nin, !fdmdv->sync); + PROFILE_SAMPLE_AND_LOG(fdmdv_freq_shift_start, demod_start, + " rx_est_freq_offset"); + + if (fdmdv->sync == 0) fdmdv->foff = foff_coarse; + fdmdv_freq_shift(rx_fdm_fcorr, rx_fdm_bb, -fdmdv->foff, + &fdmdv->foff_phase_rect, *nin); + PROFILE_SAMPLE_AND_LOG(down_convert_and_rx_filter_start, + fdmdv_freq_shift_start, " fdmdv_freq_shift"); + + /* baseband processing */ + + rxdec_filter(rx_fdm_filter, rx_fdm_fcorr, fdmdv->rxdec_lpf_mem, *nin); + down_convert_and_rx_filter(rx_filt, fdmdv->Nc, rx_fdm_filter, + fdmdv->rx_fdm_mem, fdmdv->phase_rx, fdmdv->freq, + fdmdv->freq_pol, *nin, M_FAC / Q); + PROFILE_SAMPLE_AND_LOG(rx_est_timing_start, down_convert_and_rx_filter_start, + " down_convert_and_rx_filter"); + fdmdv->rx_timing = + rx_est_timing(rx_symbols, fdmdv->Nc, rx_filt, fdmdv->rx_filter_mem_timing, + env, *nin, M_FAC); + PROFILE_SAMPLE_AND_LOG(qpsk_to_bits_start, rx_est_timing_start, + " rx_est_timing"); + + /* Adjust number of input samples to keep timing within bounds */ + + *nin = M_FAC; + + if (fdmdv->rx_timing > M_FAC / P) *nin += M_FAC / P; + + if (fdmdv->rx_timing < -M_FAC / P) *nin -= M_FAC / P; + + foff_fine = + qpsk_to_bits(rx_bits, &sync_bit, fdmdv->Nc, fdmdv->phase_difference, + fdmdv->prev_rx_symbols, rx_symbols, fdmdv->old_qpsk_mapping); + memcpy(fdmdv->prev_rx_symbols, rx_symbols, sizeof(COMP) * (fdmdv->Nc + 1)); + PROFILE_SAMPLE_AND_LOG(snr_update_start, qpsk_to_bits_start, + " qpsk_to_bits"); + snr_update(fdmdv->sig_est, fdmdv->noise_est, fdmdv->Nc, + fdmdv->phase_difference); + PROFILE_SAMPLE_AND_LOG(freq_state_start, snr_update_start, " snr_update"); + + /* freq offset estimation state machine */ + + fdmdv->sync = freq_state(reliable_sync_bit, sync_bit, &fdmdv->fest_state, + &fdmdv->timer, fdmdv->sync_mem); + PROFILE_SAMPLE_AND_LOG2(freq_state_start, " freq_state"); + fdmdv->foff -= TRACK_COEFF * foff_fine; } /*---------------------------------------------------------------------------*\ @@ -1684,39 +1666,37 @@ void fdmdv_demod(struct FDMDV *fdmdv, int rx_bits[], \*---------------------------------------------------------------------------*/ -float calc_snr(int Nc, float sig_est[], float noise_est[]) -{ - float S, SdB; - float mean, N50, N50dB, N3000dB; - float snr_dB; - int c; +float calc_snr(int Nc, float sig_est[], float noise_est[]) { + float S, SdB; + float mean, N50, N50dB, N3000dB; + float snr_dB; + int c; - S = 0.0; - for(c=0; c<Nc+1; c++) { - S += sig_est[c] * sig_est[c]; - } - SdB = 10.0*log10f(S+1E-12); + S = 0.0; + for (c = 0; c < Nc + 1; c++) { + S += sig_est[c] * sig_est[c]; + } + SdB = 10.0 * log10f(S + 1E-12); - /* Average noise mag across all carriers and square to get an - average noise power. This is an estimate of the noise power in - Rs = 50Hz of BW (note for raised root cosine filters Rs is the - noise BW of the filter) */ + /* Average noise mag across all carriers and square to get an + average noise power. This is an estimate of the noise power in + Rs = 50Hz of BW (note for raised root cosine filters Rs is the + noise BW of the filter) */ - mean = 0.0; - for(c=0; c<Nc+1; c++) - mean += noise_est[c]; - mean /= (Nc+1); - N50 = mean * mean; - N50dB = 10.0*log10f(N50+1E-12); + mean = 0.0; + for (c = 0; c < Nc + 1; c++) mean += noise_est[c]; + mean /= (Nc + 1); + N50 = mean * mean; + N50dB = 10.0 * log10f(N50 + 1E-12); - /* Now multiply by (3000 Hz)/(50 Hz) to find the total noise power - in 3000 Hz */ + /* Now multiply by (3000 Hz)/(50 Hz) to find the total noise power + in 3000 Hz */ - N3000dB = N50dB + 10.0*log10f(3000.0/RS); + N3000dB = N50dB + 10.0 * log10f(3000.0 / RS); - snr_dB = SdB - N3000dB; + snr_dB = SdB - N3000dB; - return snr_dB; + return snr_dB; } /*---------------------------------------------------------------------------*\ @@ -1729,22 +1709,21 @@ float calc_snr(int Nc, float sig_est[], float noise_est[]) \*---------------------------------------------------------------------------*/ -void fdmdv_get_demod_stats(struct FDMDV *fdmdv, struct MODEM_STATS *stats) -{ - assert(fdmdv->Nc <= MODEM_STATS_NC_MAX); +void fdmdv_get_demod_stats(struct FDMDV *fdmdv, struct MODEM_STATS *stats) { + assert(fdmdv->Nc <= MODEM_STATS_NC_MAX); - stats->Nc = fdmdv->Nc; - stats->snr_est = calc_snr(fdmdv->Nc, fdmdv->sig_est, fdmdv->noise_est); - stats->sync = fdmdv->sync; - stats->foff = fdmdv->foff; - stats->rx_timing = fdmdv->rx_timing; - stats->clock_offset = 0.0; /* TODO - implement clock offset estimation */ + stats->Nc = fdmdv->Nc; + stats->snr_est = calc_snr(fdmdv->Nc, fdmdv->sig_est, fdmdv->noise_est); + stats->sync = fdmdv->sync; + stats->foff = fdmdv->foff; + stats->rx_timing = fdmdv->rx_timing; + stats->clock_offset = 0.0; /* TODO - implement clock offset estimation */ #ifndef __EMBEDDED__ - stats->nr = 1; - for(int c=0; c<fdmdv->Nc+1; c++) { - stats->rx_symbols[0][c] = fdmdv->phase_difference[c]; - } + stats->nr = 1; + for (int c = 0; c < fdmdv->Nc + 1; c++) { + stats->rx_symbols[0][c] = fdmdv->phase_difference[c]; + } #endif } @@ -1759,60 +1738,54 @@ void fdmdv_get_demod_stats(struct FDMDV *fdmdv, struct MODEM_STATS *stats) \*---------------------------------------------------------------------------*/ -void fdmdv_8_to_16(float out16k[], float in8k[], int n8k) -{ - int i,k,l; - float acc; +void fdmdv_8_to_16(float out16k[], float in8k[], int n8k) { + int i, k, l; + float acc; - /* this version unrolled for specific FDMDV_OS */ + /* this version unrolled for specific FDMDV_OS */ - assert(FDMDV_OS == 2); + assert(FDMDV_OS == 2); - for(i=0; i<n8k; i++) { - acc = 0.0; - for(k=0,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++) - acc += fdmdv_os_filter[k]*in8k[i-l]; - out16k[i*FDMDV_OS] = FDMDV_OS*acc; + for (i = 0; i < n8k; i++) { + acc = 0.0; + for (k = 0, l = 0; k < FDMDV_OS_TAPS_16K; k += FDMDV_OS, l++) + acc += fdmdv_os_filter[k] * in8k[i - l]; + out16k[i * FDMDV_OS] = FDMDV_OS * acc; - acc = 0.0; - for(k=1,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++) - acc += fdmdv_os_filter[k]*in8k[i-l]; - out16k[i*FDMDV_OS+1] = FDMDV_OS*acc; - } + acc = 0.0; + for (k = 1, l = 0; k < FDMDV_OS_TAPS_16K; k += FDMDV_OS, l++) + acc += fdmdv_os_filter[k] * in8k[i - l]; + out16k[i * FDMDV_OS + 1] = FDMDV_OS * acc; + } - /* update filter memory */ - - for(i=-(FDMDV_OS_TAPS_8K); i<0; i++) - in8k[i] = in8k[i + n8k]; + /* update filter memory */ + for (i = -(FDMDV_OS_TAPS_8K); i < 0; i++) in8k[i] = in8k[i + n8k]; } -void fdmdv_8_to_16_short(short out16k[], short in8k[], int n8k) -{ - int i,k,l; - float acc; - - /* this version unrolled for specific FDMDV_OS */ +void fdmdv_8_to_16_short(short out16k[], short in8k[], int n8k) { + int i, k, l; + float acc; - assert(FDMDV_OS == 2); + /* this version unrolled for specific FDMDV_OS */ - for(i=0; i<n8k; i++) { - acc = 0.0; - for(k=0,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++) - acc += fdmdv_os_filter[k]*(float)in8k[i-l]; - out16k[i*FDMDV_OS] = FDMDV_OS*acc; + assert(FDMDV_OS == 2); - acc = 0.0; - for(k=1,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++) - acc += fdmdv_os_filter[k]*(float)in8k[i-l]; - out16k[i*FDMDV_OS+1] = FDMDV_OS*acc; - } + for (i = 0; i < n8k; i++) { + acc = 0.0; + for (k = 0, l = 0; k < FDMDV_OS_TAPS_16K; k += FDMDV_OS, l++) + acc += fdmdv_os_filter[k] * (float)in8k[i - l]; + out16k[i * FDMDV_OS] = FDMDV_OS * acc; - /* update filter memory */ + acc = 0.0; + for (k = 1, l = 0; k < FDMDV_OS_TAPS_16K; k += FDMDV_OS, l++) + acc += fdmdv_os_filter[k] * (float)in8k[i - l]; + out16k[i * FDMDV_OS + 1] = FDMDV_OS * acc; + } - for(i=-(FDMDV_OS_TAPS_8K); i<0; i++) - in8k[i] = in8k[i + n8k]; + /* update filter memory */ + for (i = -(FDMDV_OS_TAPS_8K); i < 0; i++) in8k[i] = in8k[i + n8k]; } /*---------------------------------------------------------------------------*\ @@ -1832,47 +1805,42 @@ void fdmdv_8_to_16_short(short out16k[], short in8k[], int n8k) \*---------------------------------------------------------------------------*/ -void fdmdv_16_to_8(float out8k[], float in16k[], int n) -{ - float acc; - int i,j,k; +void fdmdv_16_to_8(float out8k[], float in16k[], int n) { + float acc; + int i, j, k; - for(i=0, k=0; k<n; i+=FDMDV_OS, k++) { - acc = 0.0; - for(j=0; j<FDMDV_OS_TAPS_16K; j++) - acc += fdmdv_os_filter[j]*in16k[i-j]; - out8k[k] = acc; - } + for (i = 0, k = 0; k < n; i += FDMDV_OS, k++) { + acc = 0.0; + for (j = 0; j < FDMDV_OS_TAPS_16K; j++) + acc += fdmdv_os_filter[j] * in16k[i - j]; + out8k[k] = acc; + } - /* update filter memory */ + /* update filter memory */ - for(i=-FDMDV_OS_TAPS_16K; i<0; i++) - in16k[i] = in16k[i + n*FDMDV_OS]; + for (i = -FDMDV_OS_TAPS_16K; i < 0; i++) in16k[i] = in16k[i + n * FDMDV_OS]; } -void fdmdv_16_to_8_short(short out8k[], short in16k[], int n) -{ - float acc; - int i,j,k; +void fdmdv_16_to_8_short(short out8k[], short in16k[], int n) { + float acc; + int i, j, k; - for(i=0, k=0; k<n; i+=FDMDV_OS, k++) { - acc = 0.0; - for(j=0; j<FDMDV_OS_TAPS_16K; j++) - acc += fdmdv_os_filter[j]*(float)in16k[i-j]; - out8k[k] = acc; - } + for (i = 0, k = 0; k < n; i += FDMDV_OS, k++) { + acc = 0.0; + for (j = 0; j < FDMDV_OS_TAPS_16K; j++) + acc += fdmdv_os_filter[j] * (float)in16k[i - j]; + out8k[k] = acc; + } - /* update filter memory */ + /* update filter memory */ - for(i=-FDMDV_OS_TAPS_16K; i<0; i++) - in16k[i] = in16k[i + n*FDMDV_OS]; + for (i = -FDMDV_OS_TAPS_16K; i < 0; i++) in16k[i] = in16k[i + n * FDMDV_OS]; } - /*---------------------------------------------------------------------------*\ - - FUNCTION....: fdmdv_8_to_48() - AUTHOR......: David Rowe + + FUNCTION....: fdmdv_8_to_48() + AUTHOR......: David Rowe DATE CREATED: 9 May 2012 Changes the sample rate of a signal from 8 to 48 kHz. @@ -1883,92 +1851,86 @@ void fdmdv_16_to_8_short(short out8k[], short in16k[], int n) \*---------------------------------------------------------------------------*/ -void fdmdv_8_to_48(float out48k[], float in8k[], int n) -{ - int i,j,k,l; +void fdmdv_8_to_48(float out48k[], float in8k[], int n) { + int i, j, k, l; - for(i=0; i<n; i++) { - for(j=0; j<FDMDV_OS_48; j++) { - out48k[i*FDMDV_OS_48+j] = 0.0; - for(k=0,l=0; k<FDMDV_OS_TAPS_48K; k+=FDMDV_OS_48,l++) - out48k[i*FDMDV_OS_48+j] += fdmdv_os_filter48[k+j]*in8k[i-l]; - out48k[i*FDMDV_OS_48+j] *= FDMDV_OS_48; - - } - } + for (i = 0; i < n; i++) { + for (j = 0; j < FDMDV_OS_48; j++) { + out48k[i * FDMDV_OS_48 + j] = 0.0; + for (k = 0, l = 0; k < FDMDV_OS_TAPS_48K; k += FDMDV_OS_48, l++) + out48k[i * FDMDV_OS_48 + j] += fdmdv_os_filter48[k + j] * in8k[i - l]; + out48k[i * FDMDV_OS_48 + j] *= FDMDV_OS_48; + } + } - /* update filter memory */ + /* update filter memory */ - for(i=-FDMDV_OS_TAPS_48_8K; i<0; i++) - in8k[i] = in8k[i + n]; + for (i = -FDMDV_OS_TAPS_48_8K; i < 0; i++) in8k[i] = in8k[i + n]; } -void fdmdv_8_to_48_short(short out48k[], short in8k[], int n) -{ - int i,j,k,l; - float acc; - - for(i=0; i<n; i++) { - for(j=0; j<FDMDV_OS_48; j++) { - acc = 0.0; - for(k=0,l=0; k<FDMDV_OS_TAPS_48K; k+=FDMDV_OS_48,l++) - acc += fdmdv_os_filter48[k+j]*in8k[i-l]; - out48k[i*FDMDV_OS_48+j] = acc*FDMDV_OS_48; - } - } - - /* update filter memory */ - - for(i=-FDMDV_OS_TAPS_48_8K; i<0; i++) - in8k[i] = in8k[i + n]; +void fdmdv_8_to_48_short(short out48k[], short in8k[], int n) { + int i, j, k, l; + float acc; + + for (i = 0; i < n; i++) { + for (j = 0; j < FDMDV_OS_48; j++) { + acc = 0.0; + for (k = 0, l = 0; k < FDMDV_OS_TAPS_48K; k += FDMDV_OS_48, l++) + acc += fdmdv_os_filter48[k + j] * in8k[i - l]; + out48k[i * FDMDV_OS_48 + j] = acc * FDMDV_OS_48; + } + } + + /* update filter memory */ + + for (i = -FDMDV_OS_TAPS_48_8K; i < 0; i++) in8k[i] = in8k[i + n]; } /*---------------------------------------------------------------------------*\ - - FUNCTION....: fdmdv_48_to_8() - AUTHOR......: David Rowe + + FUNCTION....: fdmdv_48_to_8() + AUTHOR......: David Rowe DATE CREATED: 9 May 2012 Changes the sample rate of a signal from 48 to 8 kHz. - + n is the number of samples at the 8 kHz rate, there are FDMDV_OS_48*n samples at the 48 kHz rate. As above however a memory of - FDMDV_OS_TAPS_48 samples is reqd for in48k[] (see t48_8.c unit test as example). + FDMDV_OS_TAPS_48 samples is reqd for in48k[] (see t48_8.c unit test as +example). \*---------------------------------------------------------------------------*/ -void fdmdv_48_to_8(float out8k[], float in48k[], int n) -{ - int i,j; +void fdmdv_48_to_8(float out8k[], float in48k[], int n) { + int i, j; - for(i=0; i<n; i++) { - out8k[i] = 0.0; - for(j=0; j<FDMDV_OS_TAPS_48K; j++) - out8k[i] += fdmdv_os_filter48[j]*in48k[i*FDMDV_OS_48-j]; - } + for (i = 0; i < n; i++) { + out8k[i] = 0.0; + for (j = 0; j < FDMDV_OS_TAPS_48K; j++) + out8k[i] += fdmdv_os_filter48[j] * in48k[i * FDMDV_OS_48 - j]; + } - /* update filter memory */ + /* update filter memory */ - for(i=-FDMDV_OS_TAPS_48K; i<0; i++) - in48k[i] = in48k[i + n*FDMDV_OS_48]; + for (i = -FDMDV_OS_TAPS_48K; i < 0; i++) + in48k[i] = in48k[i + n * FDMDV_OS_48]; } -void fdmdv_48_to_8_short(short out8k[], short in48k[], int n) -{ - int i,j; - float acc; - - for(i=0; i<n; i++) { - acc = 0.0; - for(j=0; j<FDMDV_OS_TAPS_48K; j++) - acc += fdmdv_os_filter48[j]*in48k[i*FDMDV_OS_48-j]; - out8k[i] = acc; - } +void fdmdv_48_to_8_short(short out8k[], short in48k[], int n) { + int i, j; + float acc; + + for (i = 0; i < n; i++) { + acc = 0.0; + for (j = 0; j < FDMDV_OS_TAPS_48K; j++) + acc += fdmdv_os_filter48[j] * in48k[i * FDMDV_OS_48 - j]; + out8k[i] = acc; + } - /* update filter memory */ + /* update filter memory */ - for(i=-FDMDV_OS_TAPS_48K; i<0; i++) - in48k[i] = in48k[i + n*FDMDV_OS_48]; + for (i = -FDMDV_OS_TAPS_48K; i < 0; i++) + in48k[i] = in48k[i + n * FDMDV_OS_48]; } /*---------------------------------------------------------------------------*\ @@ -1978,24 +1940,23 @@ void fdmdv_48_to_8_short(short out8k[], short in48k[], int n) \*---------------------------------------------------------------------------*/ -void fdmdv_dump_osc_mags(struct FDMDV *f) -{ - int i; - - fprintf(stderr, "phase_tx[]:\n"); - for(i=0; i<=f->Nc; i++) - fprintf(stderr," %1.3f", (double)cabsolute(f->phase_tx[i])); - fprintf(stderr,"\nfreq[]:\n"); - for(i=0; i<=f->Nc; i++) - fprintf(stderr," %1.3f", (double)cabsolute(f->freq[i])); - fprintf(stderr,"\nfoff_phase_rect: %1.3f", (double)cabsolute(f->foff_phase_rect)); - fprintf(stderr,"\nphase_rx[]:\n"); - for(i=0; i<=f->Nc; i++) - fprintf(stderr," %1.3f", (double)cabsolute(f->phase_rx[i])); - fprintf(stderr, "\n\n"); +void fdmdv_dump_osc_mags(struct FDMDV *f) { + int i; + + fprintf(stderr, "phase_tx[]:\n"); + for (i = 0; i <= f->Nc; i++) + fprintf(stderr, " %1.3f", (double)cabsolute(f->phase_tx[i])); + fprintf(stderr, "\nfreq[]:\n"); + for (i = 0; i <= f->Nc; i++) + fprintf(stderr, " %1.3f", (double)cabsolute(f->freq[i])); + fprintf(stderr, "\nfoff_phase_rect: %1.3f", + (double)cabsolute(f->foff_phase_rect)); + fprintf(stderr, "\nphase_rx[]:\n"); + for (i = 0; i <= f->Nc; i++) + fprintf(stderr, " %1.3f", (double)cabsolute(f->phase_rx[i])); + fprintf(stderr, "\n\n"); } - /*---------------------------------------------------------------------------*\ FUNCTION....: randn() @@ -2007,25 +1968,24 @@ void fdmdv_dump_osc_mags(struct FDMDV *f) \*---------------------------------------------------------------------------*/ -#define RANDN_IT 12 /* This magic number of iterations gives us a - unit variance. I think because var = - (b-a)^2/12 for one uniform random variable, so - for a sum of n random variables it's - n(b-a)^2/12, or for b=1, a = 0, n=12, we get - var = 12(1-0)^2/12 = 1 */ +#define RANDN_IT \ + 12 /* This magic number of iterations gives us a \ + unit variance. I think because var = \ + (b-a)^2/12 for one uniform random variable, so \ + for a sum of n random variables it's \ + n(b-a)^2/12, or for b=1, a = 0, n=12, we get \ + var = 12(1-0)^2/12 = 1 */ static float randn() { - int i; - float rn = 0.0; + int i; + float rn = 0.0; - for(i=0; i<RANDN_IT; i++) - rn += (float)rand()/RAND_MAX; + for (i = 0; i < RANDN_IT; i++) rn += (float)rand() / RAND_MAX; - rn -= (float)RANDN_IT/2.0; - return rn; + rn -= (float)RANDN_IT / 2.0; + return rn; } - /*---------------------------------------------------------------------------*\ FUNCTION....: fdmdv_simulate_channel() @@ -2043,40 +2003,45 @@ static float randn() { \*---------------------------------------------------------------------------*/ -void fdmdv_simulate_channel(float *sig_pwr_av, COMP samples[], int nin, float target_snr) -{ - float sig_pwr, target_snr_linear, noise_pwr, noise_pwr_1Hz, noise_pwr_4000Hz, noise_gain; - int i; +void fdmdv_simulate_channel(float *sig_pwr_av, COMP samples[], int nin, + float target_snr) { + float sig_pwr, target_snr_linear, noise_pwr, noise_pwr_1Hz, noise_pwr_4000Hz, + noise_gain; + int i; - /* prevent NAN when we divide by nin below */ - if (nin == 0) return; + /* prevent NAN when we divide by nin below */ + if (nin == 0) return; - /* estimate signal power */ + /* estimate signal power */ - sig_pwr = 0.0; - for(i=0; i<nin; i++) - sig_pwr += samples[i].real*samples[i].real + samples[i].imag*samples[i].imag; + sig_pwr = 0.0; + for (i = 0; i < nin; i++) + sig_pwr += + samples[i].real * samples[i].real + samples[i].imag * samples[i].imag; - sig_pwr /= nin; + sig_pwr /= nin; - *sig_pwr_av = 0.9**sig_pwr_av + 0.1*sig_pwr; + *sig_pwr_av = 0.9 * *sig_pwr_av + 0.1 * sig_pwr; - /* det noise to meet target SNR */ + /* det noise to meet target SNR */ - target_snr_linear = POW10F(target_snr/10.0); - noise_pwr = *sig_pwr_av/target_snr_linear; /* noise pwr in a 3000 Hz BW */ - noise_pwr_1Hz = noise_pwr/3000.0; /* noise pwr in a 1 Hz bandwidth */ - noise_pwr_4000Hz = noise_pwr_1Hz*4000.0; /* noise pwr in a 4000 Hz BW, which - due to fs=8000 Hz in our simulation noise BW */ + target_snr_linear = POW10F(target_snr / 10.0); + noise_pwr = *sig_pwr_av / target_snr_linear; /* noise pwr in a 3000 Hz BW */ + noise_pwr_1Hz = noise_pwr / 3000.0; /* noise pwr in a 1 Hz bandwidth */ + noise_pwr_4000Hz = + noise_pwr_1Hz * 4000.0; /* noise pwr in a 4000 Hz BW, which + due to fs=8000 Hz in our simulation noise BW */ - noise_gain = sqrtf(0.5*noise_pwr_4000Hz); /* split noise pwr between real and imag sides */ + noise_gain = sqrtf( + 0.5 * noise_pwr_4000Hz); /* split noise pwr between real and imag sides */ - for(i=0; i<nin; i++) { - samples[i].real += noise_gain*randn(); - samples[i].imag += noise_gain*randn(); - } - /* - fprintf(stderr, "sig_pwr: %f f->sig_pwr_av: %e target_snr_linear: %f noise_pwr_4000Hz: %e noise_gain: %e\n", - sig_pwr, *sig_pwr_av, target_snr_linear, noise_pwr_4000Hz, noise_gain); - */ + for (i = 0; i < nin; i++) { + samples[i].real += noise_gain * randn(); + samples[i].imag += noise_gain * randn(); + } + /* + fprintf(stderr, "sig_pwr: %f f->sig_pwr_av: %e target_snr_linear: %f + noise_pwr_4000Hz: %e noise_gain: %e\n", sig_pwr, *sig_pwr_av, + target_snr_linear, noise_pwr_4000Hz, noise_gain); + */ } |