mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-08-24 06:46:33 -04:00
1996 lines
59 KiB
C++
1996 lines
59 KiB
C++
/*---------------------------------------------------------------------------*\
|
|
|
|
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 "fdv_arm_math.h"
|
|
|
|
#include "fdmdv_internal.h"
|
|
#include "codec2_fdmdv.h"
|
|
#include "comp_prim.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"
|
|
|
|
namespace FreeDV
|
|
{
|
|
|
|
static int sync_uw[] = {1,-1,1,-1,1,-1};
|
|
#ifdef __EMBEDDED__
|
|
#define printf gdb_stdio_printf
|
|
#endif
|
|
|
|
static const COMP pi_on_4 = { .70710678118654752439, .70710678118654752439 }; // COSF(PI/4) , SINF(PI/4)
|
|
|
|
|
|
/*--------------------------------------------------------------------------* \
|
|
|
|
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 * 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)) >= (std::size_t) 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. */
|
|
|
|
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->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;
|
|
}
|
|
}
|
|
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;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fdmdv_destroy
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 16/4/2012
|
|
|
|
Destroy an instance of the modem.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
int 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 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 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;
|
|
}
|
|
}
|
|
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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_FAC samples (1 symbol) of Nc+1 filtered
|
|
symbols streams.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
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;
|
|
|
|
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(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;
|
|
|
|
/* 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;
|
|
|
|
}
|
|
}
|
|
|
|
/* 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....: tx_filter_and_upconvert()
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 13 August 2014
|
|
|
|
Given Nc symbols construct M_FAC samples (1 symbol) of Nc+1 filtered
|
|
and upconverted symbols.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
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;
|
|
}
|
|
|
|
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]));
|
|
}
|
|
}
|
|
|
|
/* 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);
|
|
}
|
|
|
|
/*
|
|
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;
|
|
}
|
|
}
|
|
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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_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]));
|
|
}
|
|
|
|
/* 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);
|
|
}
|
|
|
|
/*
|
|
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 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;
|
|
}
|
|
|
|
mag = cabsolute(*fbb_phase);
|
|
fbb_phase->real /= mag;
|
|
fbb_phase->imag /= mag;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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 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;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: generate_pilot_fdm()
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 19/4/2012
|
|
|
|
Generate M_FAC 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_FAC];
|
|
|
|
/* +1 -1 +1 -1 DBPSK sync carrier, once filtered becomes (roughly)
|
|
two spectral lines at +/- RS/2 */
|
|
|
|
if (*bit)
|
|
*symbol = -*symbol;
|
|
|
|
if (*bit)
|
|
*bit = 0;
|
|
else
|
|
*bit = 1;
|
|
|
|
/* 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];
|
|
}
|
|
|
|
/* shift memory, inserting zeros at end */
|
|
|
|
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;
|
|
|
|
/* 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;
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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 = 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]);
|
|
}
|
|
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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[], 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]);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
*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 do_fft)
|
|
{
|
|
int i;
|
|
#ifndef FDV_ARM_MATH
|
|
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];
|
|
}
|
|
|
|
#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]);
|
|
}
|
|
#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);
|
|
#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);
|
|
|
|
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 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 oscilator as the magnitude can drfift over time */
|
|
|
|
mag = cabsolute(*foff_phase_rect);
|
|
foff_phase_rect->real /= mag;
|
|
foff_phase_rect->imag /= mag;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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_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 */
|
|
|
|
assert(nin <= (M_FAC+M_FAC/P));
|
|
|
|
/* 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]));
|
|
}
|
|
|
|
/* 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;
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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_FAC.
|
|
|
|
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_FAC+M_FAC/P], COMP rx_filter_memory[NC+1][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 */
|
|
|
|
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....: rxdec_filter()
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 31 July 2014
|
|
|
|
+/- 1000Hz low pass filter, allows us to filter at rate Q to save CPU load.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
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];
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fir_filter2()
|
|
AUTHOR......: Danilo Beuche
|
|
DATE CREATED: August 2016
|
|
|
|
Ths version submitted by Danilo for the STM32F4 platform. The idea
|
|
is to avoid reading the same value from the STM32F4 "slow" flash
|
|
twice. 2-4ms of savings per frame were measured by Danilo and the mcHF
|
|
team.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
static void fir_filter2(float acc[2], 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];
|
|
|
|
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;
|
|
|
|
m1 = inpCmplx[0];
|
|
m2 = inpCmplx[1];
|
|
|
|
inpCmplx+= dec_rate*2;
|
|
coeffPtr+= dec_rate;
|
|
|
|
c2 = *coeffPtr;
|
|
m3 = inpCmplx[0];
|
|
m4 = inpCmplx[1];
|
|
|
|
inpCmplx+= dec_rate*2;
|
|
coeffPtr+= dec_rate;
|
|
|
|
c3 = *coeffPtr;
|
|
m5 = inpCmplx[0];
|
|
m6 = inpCmplx[1];
|
|
|
|
inpCmplx+= dec_rate*2;
|
|
coeffPtr+= dec_rate;
|
|
|
|
c4 = *coeffPtr;
|
|
m7 = inpCmplx[0];
|
|
m8 = inpCmplx[1];
|
|
|
|
inpCmplx+= dec_rate*2;
|
|
coeffPtr+= dec_rate;
|
|
|
|
c5 = *coeffPtr;
|
|
m9 = inpCmplx[0];
|
|
m10 = inpCmplx[1];
|
|
|
|
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;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for(m=0; m<NFILTER; m+=dec_rate) {
|
|
c1 = *coeffPtr;
|
|
|
|
m1 = inpCmplx[0];
|
|
m2 = inpCmplx[1];
|
|
|
|
inpCmplx+= dec_rate*2;
|
|
coeffPtr+= dec_rate;
|
|
|
|
a1 = c1 * m1;
|
|
a2 = c1 * m2;
|
|
acc[0] += a1;
|
|
acc[1] += a2;
|
|
}
|
|
}
|
|
acc[0] *= dec_rate;
|
|
acc[1] *= dec_rate;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: down_convert_and_rx_filter()
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 30/6/2014
|
|
|
|
Combined down convert and rx filter, more memory efficient but less
|
|
intuitive design.
|
|
|
|
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.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
/*
|
|
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;
|
|
|
|
//PROFILE_VAR(windback_start, downconvert_start, filter_start);
|
|
|
|
/* update memory of rx_fdm */
|
|
|
|
#if 0
|
|
for(i=0; i<NRX_FDM_MEM-nin; i++)
|
|
rx_fdm_mem[i] = rx_fdm_mem[i+nin];
|
|
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));
|
|
#endif
|
|
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.
|
|
|
|
|
|
<--------------- Nrx_filt_mem ------->
|
|
nin
|
|
|--------------------------|---------|
|
|
1 |
|
|
phase_rx(c)
|
|
|
|
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");
|
|
|
|
/* 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 */
|
|
|
|
/* 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]);
|
|
|
|
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 */
|
|
|
|
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;
|
|
|
|
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);
|
|
#endif
|
|
}
|
|
//PROFILE_SAMPLE_AND_LOG2(filter_start, " filter");
|
|
|
|
/* 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;
|
|
|
|
//printf("phase_rx[%d] = %f %f\n", c, phase_rx[c].real, phase_rx[c].imag);
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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_filter_mem_timing[NC+1][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);
|
|
//fprintf(stderr,"%f %f norm_rx_timing: %f\n", x.real, x.imag, norm_rx_timing);
|
|
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;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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 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;
|
|
}
|
|
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*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;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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];
|
|
int c;
|
|
|
|
|
|
/* 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 = 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. */
|
|
|
|
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;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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 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_FAC ==
|
|
FDMDV_SAMPLES_PER_FRAME. However to adjust for differences in
|
|
transmit and receive sample clocks nin will occasionally be M_FAC-M_FAC/P,
|
|
or M_FAC+M_FAC/P.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
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;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
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 += powf(sig_est[c], 2.0);
|
|
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) */
|
|
|
|
mean = 0.0;
|
|
for(c=0; c<Nc+1; c++)
|
|
mean += noise_est[c];
|
|
mean /= (Nc+1);
|
|
N50 = powf(mean, 2.0);
|
|
N50dB = 10.0*log10f(N50+1E-12);
|
|
|
|
/* Now multiply by (3000 Hz)/(50 Hz) to find the total noise power
|
|
in 3000 Hz */
|
|
|
|
N3000dB = N50dB + 10.0*log10f(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 fdmdv_get_demod_stats(struct FDMDV *fdmdv, struct MODEM_STATS *stats)
|
|
{
|
|
int c;
|
|
|
|
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->nr = 1;
|
|
for(c=0; c<fdmdv->Nc+1; c++) {
|
|
stats->rx_symbols[0][c] = fdmdv->phase_difference[c];
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fdmdv_8_to_16()
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 9 May 2012
|
|
|
|
Changes the sample rate of a signal from 8 to 16 kHz. Support function for
|
|
SM1000.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
void fdmdv_8_to_16(float out16k[], float in8k[], int n)
|
|
{
|
|
int i,k,l;
|
|
float acc;
|
|
|
|
/* make sure n is an integer multiple of the oversampling rate, ow
|
|
this function breaks */
|
|
|
|
assert((n % FDMDV_OS) == 0);
|
|
|
|
/* this version unrolled for specific FDMDV_OS */
|
|
|
|
assert(FDMDV_OS == 2);
|
|
|
|
for(i=0; i<n; 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;
|
|
}
|
|
|
|
/* update filter memory */
|
|
|
|
for(i=-(FDMDV_OS_TAPS_8K); i<0; i++)
|
|
in8k[i] = in8k[i + n];
|
|
|
|
}
|
|
|
|
void fdmdv_8_to_16_short(short out16k[], short in8k[], int n)
|
|
{
|
|
int i,k,l;
|
|
float acc;
|
|
|
|
/* make sure n is an integer multiple of the oversampling rate, ow
|
|
this function breaks */
|
|
|
|
assert((n % FDMDV_OS) == 0);
|
|
|
|
/* this version unrolled for specific FDMDV_OS */
|
|
|
|
assert(FDMDV_OS == 2);
|
|
|
|
for(i=0; i<n; 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;
|
|
|
|
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;
|
|
}
|
|
|
|
/* update filter memory */
|
|
|
|
for(i=-(FDMDV_OS_TAPS_8K); i<0; i++)
|
|
in8k[i] = in8k[i + n];
|
|
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fdmdv_16_to_8()
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 9 May 2012
|
|
|
|
Changes the sample rate of a signal from 16 to 8 kHz.
|
|
|
|
n is the number of samples at the 8 kHz rate, there are FDMDV_OS*n
|
|
samples at the 16 kHz rate. As above however a memory of
|
|
FDMDV_OS_TAPS samples is reqd for in16k[] (see t16_8.c unit test as example).
|
|
|
|
Low pass filter the 16 kHz signal at 4 kHz using the same filter as
|
|
the upsampler, then just output every FDMDV_OS-th filtered sample.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
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;
|
|
}
|
|
|
|
/* update filter memory */
|
|
|
|
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;
|
|
|
|
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 */
|
|
|
|
for(i=-FDMDV_OS_TAPS_16K; i<0; i++)
|
|
in16k[i] = in16k[i + n*FDMDV_OS];
|
|
}
|
|
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
Function used during development to test if magnitude of digital
|
|
oscillators was drifting. It was!
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
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()
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 2 August 2014
|
|
|
|
Simple approximation to normal (gaussian) random number generator
|
|
with 0 mean and unit variance.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
#define RANDN_IT 12 /* This magic number of iterations gives us a
|
|
unit variance. I think beacuse 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;
|
|
|
|
for(i=0; i<RANDN_IT; i++)
|
|
rn += (float)rand()/RAND_MAX;
|
|
|
|
rn -= (float)RANDN_IT/2.0;
|
|
return rn;
|
|
}
|
|
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fdmdv_simulate_channel()
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: 10 July 2014
|
|
|
|
Simple channel simulation function to aid in testing. Target SNR
|
|
uses noise measured in a 3 kHz bandwidth.
|
|
|
|
Doesn't use fdmdv states so can be called from anywhere, e.g. non
|
|
fdmdv applications.
|
|
|
|
TODO: Measured SNR is coming out a few dB higher than target_snr, this
|
|
needs to be fixed.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
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;
|
|
|
|
/* 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 /= nin;
|
|
|
|
*sig_pwr_av = 0.9**sig_pwr_av + 0.1*sig_pwr;
|
|
|
|
/* det noise to meet target SNR */
|
|
|
|
target_snr_linear = powf(10.0, 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 */
|
|
|
|
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, f->sig_pwr_av, target_snr_linear, noise_pwr_4000Hz, noise_gain);
|
|
*/
|
|
}
|
|
|
|
} // FreeDV
|
|
|