+/*---------------------------------------------------------------------------*\

+

+ FILE........: fdmdv.c

+ AUTHOR......: David Rowe

+ DATE CREATED: April 14 2012

+

+ Functions that implement the FDMDV modem.

+

+\*---------------------------------------------------------------------------*/

+

+/*

+ Copyright (C) 2012 David Rowe

+

+ All rights reserved.

+

+ This program is free software; you can redistribute it and/or modify

+ it under the terms of the GNU Lesser General Public License version 2.1, as

+ published by the Free Software Foundation. This program is

+ distributed in the hope that it will be useful, but WITHOUT ANY

+ WARRANTY; without even the implied warranty of MERCHANTABILITY or

+ FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public

+ License for more details.

+

+ You should have received a copy of the GNU Lesser General Public License

+ along with this program; if not, see <http://www.gnu.org/licenses/>.

+*/

+

+/*---------------------------------------------------------------------------*\

+

+ INCLUDES

+

+\*---------------------------------------------------------------------------*/

+

+#include <assert.h>

+#include <stdlib.h>

+#include <stdio.h>

+#include <string.h>

+#include <math.h>

+

+#include "fdmdv_internal.h"

+#include "codec2_fdmdv.h"

+#include "rn.h"

+#include "test_bits.h"

+#include "pilot_coeff.h"

+#include "kiss_fft.h"

+#include "hanning.h"

+#include "os.h"

+

+static int sync_uw[] = {1,-1,1,-1,1,-1};

+

+/*---------------------------------------------------------------------------* \

+

+ FUNCTIONS

+

+\*---------------------------------------------------------------------------*/

+

+static COMP cneg(COMP a)

+{

+ COMP res;

+

+ res.real = -a.real;

+ res.imag = -a.imag;

+

+ return res;

+}

+

+static COMP cconj(COMP a)

+{

+ COMP res;

+

+ res.real = a.real;

+ res.imag = -a.imag;

+

+ return res;

+}

+

+static COMP cmult(COMP a, COMP b)

+{

+ COMP res;

+

+ res.real = a.real*b.real - a.imag*b.imag;

+ res.imag = a.real*b.imag + a.imag*b.real;

+

+ return res;

+}

+

+static COMP fcmult(float a, COMP b)

+{

+ COMP res;

+

+ res.real = a*b.real;

+ res.imag = a*b.imag;

+

+ return res;

+}

+

+static COMP cadd(COMP a, COMP b)

+{

+ COMP res;

+

+ res.real = a.real + b.real;

+ res.imag = a.imag + b.imag;

+

+ return res;

+}

+

+static float cabsolute(COMP a)

+{

+ return sqrt(pow(a.real, 2.0) + pow(a.imag, 2.0));

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_create

+ AUTHOR......: David Rowe

+ DATE CREATED: 16/4/2012

+

+ Create and initialise an instance of the modem. Returns a pointer

+ to the modem states or NULL on failure. One set of states is

+ sufficient for a full duplex modem.

+

+\*---------------------------------------------------------------------------*/

+

+struct FDMDV * CODEC2_WIN32SUPPORT 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);

+ assert(FDMDV_MAX_SAMPLES_PER_FRAME == (M+M/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;

+ }

+

+ for(k=0; k<NFILTER; k++) {

+ f->rx_filter_memory[c][k].real = 0.0;

+ f->rx_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. */

+

+ f->phase_tx[c].real = cos(2.0*PI*c/(Nc+1));

+ f->phase_tx[c].imag = sin(2.0*PI*c/(Nc+1));

+

+ f->phase_rx[c].real = 1.0;

+ f->phase_rx[c].imag = 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;

+ }

+ for(k=0; k<NFILTERTIMING; k++) {

+ f->rx_baseband_mem_timing[c][k].real = 0.0;

+ f->rx_baseband_mem_timing[c][k].imag = 0.0;

+ }

+ }

+

+ fdmdv_set_fsep(f, FSEP);

+ f->freq[Nc].real = cos(2.0*PI*FDMDV_FCENTRE/FS);

+ f->freq[Nc].imag = sin(2.0*PI*FDMDV_FCENTRE/FS);

+

+ /* Generate DBPSK pilot Look Up Table (LUT) */

+

+ generate_pilot_lut(f->pilot_lut, &f->freq[Nc]);

+

+ /* freq Offset estimation states */

+

+ f->fft_pilot_cfg = kiss_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;

+

+ 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_rect.real = 1.0;

+ f->foff_rect.imag = 0.0;

+ f->foff_phase_rect.real = 1.0;

+ f->foff_phase_rect.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;

+ }

+

+ for(i=0; i<2*FDMDV_NSPEC; i++)

+ f->fft_buf[i] = 0.0;

+ f->fft_cfg = kiss_fft_alloc (2*FDMDV_NSPEC, 0, NULL, NULL);

+ assert(f->fft_cfg != NULL);

+

+

+ return f;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_destroy

+ AUTHOR......: David Rowe

+ DATE CREATED: 16/4/2012

+

+ Destroy an instance of the modem.

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT fdmdv_destroy(struct FDMDV *fdmdv)

+{

+ assert(fdmdv != NULL);

+ KISS_FFT_FREE(fdmdv->fft_pilot_cfg);

+ KISS_FFT_FREE(fdmdv->fft_cfg);

+ free(fdmdv->rx_test_bits_mem);

+ free(fdmdv);

+}

+

+

+void CODEC2_WIN32SUPPORT fdmdv_use_old_qpsk_mapping(struct FDMDV *fdmdv) {

+ fdmdv->old_qpsk_mapping = 1;

+}

+

+

+int CODEC2_WIN32SUPPORT fdmdv_bits_per_frame(struct FDMDV *fdmdv)

+{

+ return (fdmdv->Nc * NB);

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_get_test_bits()

+ AUTHOR......: David Rowe

+ DATE CREATED: 16/4/2012

+

+ Generate a frame of bits from a repeating sequence of random data. OK so

+ it's not very random if it repeats but it makes syncing at the demod easier

+ for test purposes.

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT 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;

+ }

+ }

+

+float CODEC2_WIN32SUPPORT fdmdv_get_fsep(struct FDMDV *f)

+{

+ return f->fsep;

+}

+

+void CODEC2_WIN32SUPPORT 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 + FDMDV_FCENTRE;

+ f->freq[c].real = cos(2.0*PI*carrier_freq/FS);

+ f->freq[c].imag = sin(2.0*PI*carrier_freq/FS);

+ }

+

+ for(c=f->Nc/2; c<f->Nc; c++) {

+ carrier_freq = (-f->Nc/2 + c + 1)*f->fsep + FDMDV_FCENTRE;

+ f->freq[c].real = cos(2.0*PI*carrier_freq/FS);

+ f->freq[c].imag = sin(2.0*PI*carrier_freq/FS);

+ }

+}

+

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: bits_to_dqpsk_symbols()

+ AUTHOR......: David Rowe

+ DATE CREATED: 16/4/2012

+

+ Maps bits to parallel DQPSK symbols. Generate Nc+1 QPSK symbols from

+ vector of (1,Nc*Nb) input tx_bits. The Nc+1 symbol is the +1 -1 +1

+ .... BPSK sync carrier.

+

+\*---------------------------------------------------------------------------*/

+

+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 */

+

+ 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;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: tx_filter()

+ AUTHOR......: David Rowe

+ DATE CREATED: 17/4/2012

+

+ Given Nc*NB bits construct M samples (1 symbol) of Nc+1 filtered

+ symbols streams.

+

+\*---------------------------------------------------------------------------*/

+

+void tx_filter(COMP tx_baseband[NC+1][M], int Nc, COMP tx_symbols[], COMP tx_filter_memory[NC+1][NSYM])

+{

+ int c;

+ int i,j,k;

+ float acc;

+ COMP gain;

+

+ gain.real = sqrt(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 filtered output samples for each symbol.

+ Efficient polyphase filter techniques used as tx_filter_memory is sparse

+ */

+

+ for(i=0; i<M; i++) {

+ for(c=0; c<Nc+1; c++) {

+

+ /* filter real sample of symbol for carrier c */

+

+ acc = 0.0;

+ for(j=0,k=M-i-1; j<NSYM; j++,k+=M)

+ acc += M * tx_filter_memory[c][j].real * gt_alpha5_root[k];

+ tx_baseband[c][i].real = acc;

+

+ /* filter imag sample of symbol for carrier c */

+

+ acc = 0.0;

+ for(j=0,k=M-i-1; j<NSYM; j++,k+=M)

+ acc += M * tx_filter_memory[c][j].imag * gt_alpha5_root[k];

+ tx_baseband[c][i].imag = acc;

+

+ }

+ }

+

+ /* 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;

+ }

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdm_upconvert()

+ AUTHOR......: David Rowe

+ DATE CREATED: 17/4/2012

+

+ Construct FDM signal by frequency shifting each filtered symbol

+ stream. Returns complex signal so we can apply frequency offsets

+ easily.

+

+\*---------------------------------------------------------------------------*/

+

+void fdm_upconvert(COMP tx_fdm[], int Nc, COMP tx_baseband[NC+1][M], COMP phase_tx[], COMP freq[])

+{

+ int i,c;

+ COMP two = {2.0, 0.0};

+ COMP pilot;

+

+ for(i=0; i<M; i++) {

+ tx_fdm[i].real = 0.0;

+ tx_fdm[i].imag = 0.0;

+ }

+

+ /* Nc/2 tones below centre freq */

+

+ for (c=0; c<Nc/2; c++)

+ for (i=0; i<M; 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]));

+ }

+

+ /* Nc/2 tones above centre freq */

+

+ for (c=Nc/2; c<Nc; c++)

+ for (i=0; i<M; 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]));

+ }

+

+ /* add centre pilot tone */

+

+ c = Nc;

+ for (i=0; i<M; i++) {

+ phase_tx[c] = cmult(phase_tx[c], freq[c]);

+ pilot = cmult(cmult(two, tx_baseband[c][i]), phase_tx[c]);

+ tx_fdm[i] = cadd(tx_fdm[i], pilot);

+ }

+

+ /*

+ 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; i++)

+ tx_fdm[i] = cmult(two, tx_fdm[i]);

+

+ /* normalise digital oscilators as the magnitude can drfift over time */

+

+ for (c=0; c<Nc+1; c++) {

+ phase_tx[c].real /= cabsolute(phase_tx[c]);

+ phase_tx[c].imag /= cabsolute(phase_tx[c]);

+ }

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_mod()

+ AUTHOR......: David Rowe

+ DATE CREATED: 26/4/2012

+

+ FDMDV modulator, take a frame of FDMDV_BITS_PER_FRAME bits and

+ generates a frame of FDMDV_SAMPLES_PER_FRAME modulated symbols.

+ Sync bit is returned to aid alignment of your next frame.

+

+ The sync_bit value returned will be used for the _next_ frame.

+

+ The output signal is complex to support single sided frequency

+ shifting, for example when testing frequency offsets in channel

+ simulation.

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT fdmdv_mod(struct FDMDV *fdmdv, COMP tx_fdm[],

+ int tx_bits[], int *sync_bit)

+{

+ COMP tx_symbols[NC+1];

+ COMP tx_baseband[NC+1][M];

+

+ 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));

+ tx_filter(tx_baseband, fdmdv->Nc, tx_symbols, fdmdv->tx_filter_memory);

+ fdm_upconvert(tx_fdm, fdmdv->Nc, tx_baseband, fdmdv->phase_tx, fdmdv->freq);

+

+ *sync_bit = fdmdv->tx_pilot_bit;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: generate_pilot_fdm()

+ AUTHOR......: David Rowe

+ DATE CREATED: 19/4/2012

+

+ Generate M samples of DBPSK pilot signal for Freq offset estimation.

+

+\*---------------------------------------------------------------------------*/

+

+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];

+

+ /* +1 -1 +1 -1 DBPSK sync carrier, once filtered becomes (roughly)

+ two spectral lines at +/- RS/2 */

+

+ if (*bit)

+ *symbol = -*symbol;

+ else

+ *symbol = *symbol;

+ if (*bit)

+ *bit = 0;

+ else

+ *bit = 1;

+

+ /* filter DPSK symbol to create M baseband samples */

+

+ filter_mem[NFILTER-1] = (sqrt(2)/2) * *symbol;

+ for(i=0; i<M; i++) {

+ tx_baseband[i] = 0.0;

+ for(j=M-1,k=M-i-1; j<NFILTER; j+=M,k+=M)

+ tx_baseband[i] += M * filter_mem[j] * gt_alpha5_root[k];

+ }

+

+ /* shift memory, inserting zeros at end */

+

+ for(i=0; i<NFILTER-M; i++)

+ filter_mem[i] = filter_mem[i+M];

+

+ for(i=NFILTER-M; i<NFILTER; i++)

+ filter_mem[i] = 0.0;

+

+ /* upconvert */

+

+ for(i=0; i<M; i++) {

+ *phase = cmult(*phase, *freq);

+ pilot_fdm[i].real = sqrt(2)*2*tx_baseband[i] * phase->real;

+ pilot_fdm[i].imag = sqrt(2)*2*tx_baseband[i] * phase->imag;

+ }

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: generate_pilot_lut()

+ AUTHOR......: David Rowe

+ DATE CREATED: 19/4/2012

+

+ Generate a 4M sample vector of DBPSK pilot signal. As the pilot signal

+ is periodic in 4M samples we can then use this vector as a look up table

+ for pilot signal generation in the demod.

+

+\*---------------------------------------------------------------------------*/

+

+void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq)

+{

+ int pilot_rx_bit = 0;

+ float pilot_symbol = sqrt(2.0);

+ COMP pilot_phase = {1.0, 0.0};

+ float pilot_filter_mem[NFILTER];

+ COMP pilot[M];

+ 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*(f-4)], pilot, M*sizeof(COMP));

+ }

+

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: lpf_peak_pick()

+ AUTHOR......: David Rowe

+ DATE CREATED: 20/4/2012

+

+ LPF and peak pick part of freq est, put in a function as we call it twice.

+

+\*---------------------------------------------------------------------------*/

+

+void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[],

+ COMP pilot_lpf[], kiss_fft_cfg fft_pilot_cfg, COMP S[], int nin)

+{

+ int i,j,k;

+ int mpilot;

+ COMP s[MPILOTFFT];

+ 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=0; i<NPILOTLPF; i++,j++) {

+ pilot_lpf[i].real = 0.0; pilot_lpf[i].imag = 0.0;

+ for(k=0; k<NPILOTCOEFF; k++)

+ pilot_lpf[i] = cadd(pilot_lpf[i], fcmult(pilot_coeff[k], pilot_baseband[j+k]));

+ }

+

+ /* 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; i<MPILOTFFT; i++) {

+ s[i].real = 0.0; s[i].imag = 0.0;

+ }

+ for(i=0,j=0; i<NPILOTLPF; i+=mpilot,j++) {

+ s[j] = fcmult(hanning[i], pilot_lpf[i]);

+ }

+

+ kiss_fft(fft_pilot_cfg, (kiss_fft_cpx *)s, (kiss_fft_cpx *)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;

+ *max = imax;

+

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: rx_est_freq_offset()

+ AUTHOR......: David Rowe

+ DATE CREATED: 19/4/2012

+

+ Estimate frequency offset of FDM signal using BPSK pilot. Note that

+ this algorithm is quite sensitive to pilot tone level wrt other

+ carriers, so test variations to the pilot amplitude carefully.

+

+\*---------------------------------------------------------------------------*/

+

+float rx_est_freq_offset(struct FDMDV *f, COMP rx_fdm[], int nin)

+{

+ int i,j;

+ COMP pilot[M+M/P];

+ COMP prev_pilot[M+M/P];

+ float foff, foff1, foff2;

+ float max1, max2;

+

+ assert(nin <= M+M/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)

+ 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)

+ f->prev_pilot_lut_index = 0;

+ }

+

+ /*

+ Down convert latest M 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];

+ }

+

+ for(i=0,j=NPILOTBASEBAND-nin; i<nin; i++,j++) {

+ f->pilot_baseband1[j] = cmult(rx_fdm[i], cconj(pilot[i]));

+ f->pilot_baseband2[j] = cmult(rx_fdm[i], cconj(prev_pilot[i]));

+ }

+

+ lpf_peak_pick(&foff1, &max1, f->pilot_baseband1, f->pilot_lpf1, f->fft_pilot_cfg, f->S1, nin);

+ lpf_peak_pick(&foff2, &max2, f->pilot_baseband2, f->pilot_lpf2, f->fft_pilot_cfg, f->S2, nin);

+

+ if (max1 > max2)

+ foff = foff1;

+ else

+ foff = foff2;

+

+ return foff;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_freq_shift()

+ AUTHOR......: David Rowe

+ DATE CREATED: 26/4/2012

+

+ Frequency shift modem signal. The use of complex input and output allows

+ single sided frequency shifting (no images).

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT fdmdv_freq_shift(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff,

+ COMP *foff_rect, COMP *foff_phase_rect, int nin)

+{

+ int i;

+

+ foff_rect->real = cos(2.0*PI*foff/FS);

+ foff_rect->imag = sin(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 oscilator as the magnitude can drfift over time */

+

+ foff_phase_rect->real /= cabsolute(*foff_phase_rect);

+ foff_phase_rect->imag /= cabsolute(*foff_phase_rect);

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdm_downconvert()

+ AUTHOR......: David Rowe

+ DATE CREATED: 22/4/2012

+

+ Frequency shift each modem carrier down to Nc+1 baseband signals.

+

+\*---------------------------------------------------------------------------*/

+

+void fdm_downconvert(COMP rx_baseband[NC+1][M+M/P], int Nc, COMP rx_fdm[], COMP phase_rx[], COMP freq[], int nin)

+{

+ int i,c;

+

+ /* maximum number of input samples to demod */

+

+ assert(nin <= (M+M/P));

+

+ /* Nc/2 tones below centre freq */

+

+ for (c=0; c<Nc/2; 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]));

+ }

+

+ /* Nc/2 tones above centre freq */

+

+ for (c=Nc/2; c<Nc; 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]));

+ }

+

+ /* centre pilot tone */

+

+ c = Nc;

+ 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 */

+

+ for (c=0; c<Nc+1; c++) {

+ phase_rx[c].real /= cabsolute(phase_rx[c]);

+ phase_rx[c].imag /= cabsolute(phase_rx[c]);

+ }

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: rx_filter()

+ AUTHOR......: David Rowe

+ DATE CREATED: 22/4/2012

+

+ Receive filter each baseband signal at oversample rate P. Filtering at

+ rate P lowers CPU compared to rate M.

+

+ Depending on the number of input samples to the demod nin, we

+ produce P-1, P (usually), or P+1 filtered samples at rate P. nin is

+ occasionally adjusted to compensate for timing slips due to

+ different tx and rx sample clocks.

+

+\*---------------------------------------------------------------------------*/

+

+void rx_filter(COMP rx_filt[NC+1][P+1], int Nc, COMP rx_baseband[NC+1][M+M/P], COMP rx_filter_memory[NC+1][NFILTER], int nin)

+{

+ int c, i,j,k,l;

+ int n=M/P;

+

+ /* rx filter each symbol, generate P filtered output samples for

+ each symbol. Note we keep filter memory at rate M, it's just

+ the filter output at rate P */

+

+ for(i=0, j=0; i<nin; i+=n,j++) {

+

+ /* 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];

+

+ /* 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]));

+ }

+

+ /* 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];

+ }

+

+ assert(j <= (P+1)); /* check for any over runs */

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: rx_est_timing()

+ AUTHOR......: David Rowe

+ DATE CREATED: 23/4/2012

+

+ Estimate optimum timing offset, re-filter receive symbols at optimum

+ timing estimate.

+

+\*---------------------------------------------------------------------------*/

+

+float rx_est_timing(COMP rx_symbols[],

+ int Nc,

+ COMP rx_filt[NC+1][P+1],

+ COMP rx_baseband[NC+1][M+M/P],

+ COMP rx_filter_mem_timing[NC+1][NT*P],

+ float env[],

+ COMP rx_baseband_mem_timing[NC+1][NFILTERTIMING],

+ int nin)

+{

+ int c,i,j,k;

+ int adjust, s;

+ COMP x, phase, freq;

+ float rx_timing;

+

+ /*

+ 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 = cos(2*PI/P);

+ freq.imag = sin(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 M. The

+ M/4 part was adjusted by experiment, I know not why.... */

+

+ rx_timing = atan2(x.imag, x.real)*M/(2*PI) + M/4;

+

+ if (rx_timing > M)

+ rx_timing -= M;

+ if (rx_timing < -M)

+ rx_timing += M;

+

+ /* rx_filt_mem_timing contains M + Nfilter + M samples of the

+ baseband signal at rate M this enables us to resample the

+ filtered rx symbol with M sample precision once we have

+ rx_timing */

+

+ for(c=0; c<Nc+1; c++)

+ for(i=0,j=nin; i<NFILTERTIMING-nin; i++,j++)

+ rx_baseband_mem_timing[c][i] = rx_baseband_mem_timing[c][j];

+ for(c=0; c<Nc+1; c++)

+ for(i=NFILTERTIMING-nin,j=0; i<NFILTERTIMING; i++,j++)

+ rx_baseband_mem_timing[c][i] = rx_baseband[c][j];

+

+ /* rx filter to get symbol for each carrier at estimated optimum

+ timing instant. We use rate M filter memory to get fine timing

+ resolution. */

+

+ s = round(rx_timing) + M;

+ for(c=0; c<Nc+1; c++) {

+ rx_symbols[c].real = 0.0;

+ rx_symbols[c].imag = 0.0;

+ for(k=s,j=0; k<s+NFILTER; k++,j++)

+ rx_symbols[c] = cadd(rx_symbols[c], fcmult(gt_alpha5_root[j], rx_baseband_mem_timing[c][k]));

+ }

+

+ return rx_timing;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: qpsk_to_bits()

+ AUTHOR......: David Rowe

+ DATE CREATED: 24/4/2012

+

+ Convert DQPSK symbols back to an array of bits, extracts sync bit

+ from DBPSK pilot, and also uses pilot to estimate fine frequency

+ error.

+

+\*---------------------------------------------------------------------------*/

+

+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 pi_on_4;

+ COMP d;

+ int msb=0, lsb=0;

+ float ferr, norm;

+

+ pi_on_4.real = cos(PI/4.0);

+ pi_on_4.imag = sin(PI/4.0);

+

+ /* 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;

+ }

+ 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;

+ }

+

+ /* 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;

+ }

+ else {

+ *sync_bit = 0;

+ ferr = -phase_difference[Nc].imag;

+ }

+

+ /* 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;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: snr_update()

+ AUTHOR......: David Rowe

+ DATE CREATED: 17 May 2012

+

+ Given phase differences update estimates of signal and noise levels.

+

+\*---------------------------------------------------------------------------*/

+

+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];

+ COMP pi_on_4;

+ int c;

+

+ pi_on_4.real = cos(PI/4.0);

+ pi_on_4.imag = sin(PI/4.0);

+

+ /* 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]);

+

+ /* 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];

+

+ /* 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 = fabs(phase_difference[c].real);

+ refl_symbols[c].imag = fabs(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. */

+

+ 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 CODEC2_WIN32SUPPORT fdmdv_error_pattern_size(struct FDMDV *f) {

+ return f->ntest_bits;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_put_test_bits()

+ AUTHOR......: David Rowe

+ DATE CREATED: 24/4/2012

+

+ Accepts nbits from rx and attempts to sync with test_bits sequence.

+ If sync OK measures bit errors.

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT 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]);

+ }

+

+ /* if less than a thresh we are aligned and in sync with test sequence */

+

+ ber = (float)*bit_errors/f->ntest_bits;

+

+ *sync = 0;

+ if (ber < 0.2)

+ *sync = 1;

+

+ *ntest_bits = f->ntest_bits;

+

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: freq_state(()

+ AUTHOR......: David Rowe

+ DATE CREATED: 24/4/2012

+

+ Freq offset state machine. Moves between coarse and fine states

+ based on BPSK pilot sequence. Freq offset estimator occasionally

+ makes mistakes when used continuously. So we use it until we have

+ acquired the BPSK pilot, then switch to a more robust "fine"

+ tracking algorithm. If we lose sync we switch back to coarse mode

+ for fast re-acquisition of large frequency offsets.

+

+ The sync state is also useful for higher layers to determine when

+ there is valid FDMDV data for decoding. We want to reliably and

+ quickly get into sync, stay in sync even on fading channels, and

+ fall out of sync quickly if tx stops or it's a false sync.

+

+ In multipath fading channels the BPSK sync carrier may be pushed

+ down in the noise, despite other carriers being at full strength.

+ We want to avoid loss of sync in these cases.

+

+\*---------------------------------------------------------------------------*/

+

+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 */

+

+ 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 */

+

+ 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;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_demod()

+ AUTHOR......: David Rowe

+ DATE CREATED: 26/4/2012

+

+ FDMDV demodulator, take an array of FDMDV_SAMPLES_PER_FRAME

+ modulated samples, returns an array of FDMDV_BITS_PER_FRAME bits,

+ plus the sync bit.

+

+ The input signal is complex to support single sided frequency shifting

+ before the demod input (e.g. fdmdv2 click to tune feature).

+

+ The number of input samples nin will normally be M ==

+ FDMDV_SAMPLES_PER_FRAME. However to adjust for differences in

+ transmit and receive sample clocks nin will occasionally be M-M/P,

+ or M+M/P.

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT 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+M/P];

+ COMP rx_baseband[NC+1][M+M/P];

+ COMP rx_filt[NC+1][P+1];

+ COMP rx_symbols[NC+1];

+ float env[NT*P];

+ int sync_bit;

+

+ /* freq offset estimation and correction */

+

+ foff_coarse = rx_est_freq_offset(fdmdv, rx_fdm, *nin);

+

+ if (fdmdv->sync == 0)

+ fdmdv->foff = foff_coarse;

+ fdmdv_freq_shift(rx_fdm_fcorr, rx_fdm, -fdmdv->foff, &fdmdv->foff_rect, &fdmdv->foff_phase_rect, *nin);

+

+ /* baseband processing */

+

+ fdm_downconvert(rx_baseband, fdmdv->Nc, rx_fdm_fcorr, fdmdv->phase_rx, fdmdv->freq, *nin);

+ rx_filter(rx_filt, fdmdv->Nc, rx_baseband, fdmdv->rx_filter_memory, *nin);

+ fdmdv->rx_timing = rx_est_timing(rx_symbols, fdmdv->Nc, rx_filt, rx_baseband, fdmdv->rx_filter_mem_timing, env, fdmdv->rx_baseband_mem_timing, *nin);

+

+ /* Adjust number of input samples to keep timing within bounds */

+

+ *nin = M;

+

+ if (fdmdv->rx_timing > 2*M/P)

+ *nin += M/P;

+

+ if (fdmdv->rx_timing < 0)

+ *nin -= M/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));

+ snr_update(fdmdv->sig_est, fdmdv->noise_est, fdmdv->Nc, fdmdv->phase_difference);

+

+ /* freq offset estimation state machine */

+

+ fdmdv->sync = freq_state(reliable_sync_bit, sync_bit, &fdmdv->fest_state, &fdmdv->timer, fdmdv->sync_mem);

+ fdmdv->foff -= TRACK_COEFF*foff_fine;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: calc_snr()

+ AUTHOR......: David Rowe

+ DATE CREATED: 17 May 2012

+

+ Calculate current SNR estimate (3000Hz noise BW)

+

+\*---------------------------------------------------------------------------*/

+

+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 += pow(sig_est[c], 2.0);

+ SdB = 10.0*log10(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) */

+

+ mean = 0.0;

+ for(c=0; c<Nc+1; c++)

+ mean += noise_est[c];

+ mean /= (Nc+1);

+ N50 = pow(mean, 2.0);

+ N50dB = 10.0*log10(N50+1E-12);

+

+ /* Now multiply by (3000 Hz)/(50 Hz) to find the total noise power

+ in 3000 Hz */

+

+ N3000dB = N50dB + 10.0*log10(3000.0/RS);

+

+ snr_dB = SdB - N3000dB;

+

+ return snr_dB;

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_get_demod_stats()

+ AUTHOR......: David Rowe

+ DATE CREATED: 1 May 2012

+

+ Fills stats structure with a bunch of demod information.

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT fdmdv_get_demod_stats(struct FDMDV *fdmdv,

+ struct FDMDV_STATS *fdmdv_stats)

+{

+ int c;

+

+ fdmdv_stats->Nc = fdmdv->Nc;

+ fdmdv_stats->snr_est = calc_snr(fdmdv->Nc, fdmdv->sig_est, fdmdv->noise_est);

+ fdmdv_stats->sync = fdmdv->sync;

+ fdmdv_stats->foff = fdmdv->foff;

+ fdmdv_stats->rx_timing = fdmdv->rx_timing;

+ fdmdv_stats->clock_offset = 0.0; /* TODO - implement clock offset estimation */

+

+ for(c=0; c<fdmdv->Nc+1; c++) {

+ fdmdv_stats->rx_symbols[c] = fdmdv->phase_difference[c];

+ }

+}

+

+/*---------------------------------------------------------------------------*\

+

+ 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. Experience

+ with PC based modems has shown that PC sound cards have a more

+ accurate sample clock when set for 48 kHz than 8 kHz.

+

+ n is the number of samples at the 8 kHz rate, there are FDMDV_OS*n samples

+ at the 48 kHz rate. A memory of FDMDV_OS_TAPS/FDMDV_OS samples is reqd for

+ in8k[] (see t48_8.c unit test as example).

+

+ This is a classic polyphase upsampler. We take the 8 kHz samples

+ and insert (FDMDV_OS-1) zeroes between each sample, then

+ FDMDV_OS_TAPS FIR low pass filter the signal at 4kHz. As most of

+ the input samples are zeroes, we only need to multiply non-zero

+ input samples by filter coefficients. The zero insertion and

+ filtering are combined in the code below and I'm too lazy to explain

+ it further right now....

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT fdmdv_8_to_48(float out48k[], float in8k[], int n)

+{

+ int i,j,k,l;

+

+ /* make sure n is an integer multiple of the oversampling rate, ow

+ this function breaks */

+

+ assert((n % FDMDV_OS) == 0);

+

+ for(i=0; i<n; i++) {

+ for(j=0; j<FDMDV_OS; j++) {

+ out48k[i*FDMDV_OS+j] = 0.0;

+ for(k=0,l=0; k<FDMDV_OS_TAPS; k+=FDMDV_OS,l++)

+ out48k[i*FDMDV_OS+j] += fdmdv_os_filter[k+j]*in8k[i-l];

+ out48k[i*FDMDV_OS+j] *= FDMDV_OS;

+

+ }

+ }

+

+ /* update filter memory */

+

+ for(i=-(FDMDV_OS_TAPS/FDMDV_OS); i<0; i++)

+ in8k[i] = in8k[i + n];

+

+}

+

+/*---------------------------------------------------------------------------*\

+

+ 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*n

+ samples at the 48 kHz rate. As above however a memory of

+ FDMDV_OS_TAPS samples is reqd for in48k[] (see t48_8.c unit test as example).

+

+ Low pass filter the 48 kHz signal at 4 kHz using the same filter as

+ the upsampler, then just output every FDMDV_OS-th filtered sample.

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT 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; j++)

+ out8k[i] += fdmdv_os_filter[j]*in48k[i*FDMDV_OS-j];

+ }

+

+ /* update filter memory */

+

+ for(i=-FDMDV_OS_TAPS; i<0; i++)

+ in48k[i] = in48k[i + n*FDMDV_OS];

+}

+

+/*---------------------------------------------------------------------------*\

+

+ FUNCTION....: fdmdv_get_rx_spectrum()

+ AUTHOR......: David Rowe

+ DATE CREATED: 9 June 2012

+

+ Returns the FDMDV_NSPEC point magnitude spectrum of the rx signal in

+ dB. The spectral samples are scaled so that 0dB is the peak, a good

+ range for plotting is 0 to -40dB.

+

+ Note only the real part of the complex input signal is used at

+ present. A complex variable is used for input for compatability

+ with the other rx signal procesing.

+

+ Successive calls can be used to build up a waterfall or spectrogram

+ plot, by mapping the received levels to colours.

+

+ The time-frequency resolution of the spectrum can be adjusted by varying

+ FDMDV_NSPEC. Note that a 2*FDMDV_NSPEC size FFT is reqd to get

+ FDMDV_NSPEC output points. FDMDV_NSPEC must be a power of 2.

+

+ See octave/tget_spec.m for a demo real time spectral display using

+ Octave. This demo averages the output over time to get a smoother

+ display:

+

+ av = 0.9*av + 0.1*mag_dB

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT fdmdv_get_rx_spectrum(struct FDMDV *f, float mag_spec_dB[],

+ COMP rx_fdm[], int nin)

+{

+ int i,j;

+ COMP fft_in[2*FDMDV_NSPEC];

+ COMP fft_out[2*FDMDV_NSPEC];

+ float full_scale_dB;

+

+ /* update buffer of input samples */

+

+ for(i=0; i<2*FDMDV_NSPEC-nin; i++)

+ f->fft_buf[i] = f->fft_buf[i+nin];

+ for(j=0; j<nin; j++,i++)

+ f->fft_buf[i] = rx_fdm[j].real;

+ assert(i == 2*FDMDV_NSPEC);

+

+ /* window and FFT */

+

+ for(i=0; i<2*FDMDV_NSPEC; i++) {

+ fft_in[i].real = f->fft_buf[i] * (0.5 - 0.5*cos((float)i*2.0*PI/(2*FDMDV_NSPEC)));

+ fft_in[i].imag = 0.0;

+ }

+

+ kiss_fft(f->fft_cfg, (kiss_fft_cpx *)fft_in, (kiss_fft_cpx *)fft_out);

+

+ /* FFT scales up a signal of level 1 FDMDV_NSPEC */

+

+ full_scale_dB = 20*log10(FDMDV_NSPEC);

+

+ /* scale and convert to dB */

+

+ for(i=0; i<FDMDV_NSPEC; i++) {

+ mag_spec_dB[i] = 10.0*log10(fft_out[i].real*fft_out[i].real + fft_out[i].imag*fft_out[i].imag + 1E-12);

+ mag_spec_dB[i] -= full_scale_dB;

+ }

+}

+

+/*---------------------------------------------------------------------------*\

+

+ Function used during development to test if magnitude of digital

+ oscillators was drifting. It was!

+

+\*---------------------------------------------------------------------------*/

+

+void CODEC2_WIN32SUPPORT 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", cabsolute(f->phase_tx[i]));

+ fprintf(stderr,"\nfreq[]:\n");

+ for(i=0; i<=f->Nc; i++)

+ fprintf(stderr," %1.3f", cabsolute(f->freq[i]));

+ fprintf(stderr,"\nfoff_rect %1.3f foff_phase_rect: %1.3f", cabsolute(f->foff_rect), cabsolute(f->foff_phase_rect));

+ fprintf(stderr,"\nphase_rx[]:\n");

+ for(i=0; i<=f->Nc; i++)

+ fprintf(stderr," %1.3f", cabsolute(f->phase_rx[i]));

+ fprintf(stderr, "\n\n");

+}