kopia lustrzana https://github.com/projecthorus/horusdemodlib
1044 wiersze
32 KiB
C
1044 wiersze
32 KiB
C
/*---------------------------------------------------------------------------*\
|
|
|
|
FILE........: fsk.c
|
|
AUTHOR......: Brady O'Brien & David Rowe
|
|
DATE CREATED: 7 January 2016
|
|
|
|
C Implementation of 2/4FSK modulator/demodulator, based on octave/fsk_lib.m
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
/*
|
|
Copyright (C) 2016-2020 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/>.
|
|
*/
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
DEFINES
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
/* Define this to enable EbNodB estimate */
|
|
/* This needs square roots, may take more cpu time than it's worth */
|
|
#define EST_EBNO
|
|
|
|
/* This is a flag to make the mod/demod allocate their memory on the stack instead of the heap */
|
|
/* At large sample rates, there's not enough stack space to run the demod */
|
|
#define DEMOD_ALLOC_STACK
|
|
|
|
/* This is a flag for the freq. estimator to use a precomputed/rt computed hann window table
|
|
On platforms with slow cosf, this will produce a substantial speedup at the cost of a small
|
|
amount of memory
|
|
*/
|
|
#define USE_HANN_TABLE
|
|
|
|
/* This flag turns on run-time hann table generation. If USE_HANN_TABLE is unset,
|
|
this flag has no effect. If USE_HANN_TABLE is set and this flag is set, the
|
|
hann table will be allocated and generated when fsk_init or fsk_init_hbr is
|
|
called. If this flag is not set, a hann function table of size fsk->Ndft MUST
|
|
be provided. On small platforms, this can be used with a precomputed table to
|
|
save memory at the cost of flash space.
|
|
*/
|
|
#define GENERATE_HANN_TABLE_RUNTIME
|
|
|
|
/* Turn off table generation if on cortex M4 to save memory */
|
|
#ifdef CORTEX_M4
|
|
#undef USE_HANN_TABLE
|
|
#endif
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
INCLUDES
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
#include <assert.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <math.h>
|
|
|
|
#include "fsk.h"
|
|
#include "comp_prim.h"
|
|
#include "kiss_fftr.h"
|
|
#include "modem_probe.h"
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTIONS
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
static void stats_init(struct FSK *fsk);
|
|
|
|
#ifdef USE_HANN_TABLE
|
|
/*
|
|
This is used by fsk_create and fsk_create_hbr to generate a hann function
|
|
table
|
|
*/
|
|
static void fsk_generate_hann_table(struct FSK* fsk){
|
|
int Ndft = fsk->Ndft;
|
|
size_t i;
|
|
|
|
for(i=0; i<Ndft; i++){
|
|
fsk->hann_table[i] = 0.5 - 0.5 * cosf(2.0 * M_PI * (float)i / (float) (Ndft-1));
|
|
}
|
|
}
|
|
#endif
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fsk_create_core
|
|
AUTHOR......: Brady O'Brien
|
|
DATE CREATED: 7 January 2016
|
|
|
|
In this version of the demod the stdanard/hbr modes have been
|
|
largely combined at they shared so much common code. The
|
|
fsk_create/fsk_create_hbr function interface has been retained to
|
|
maximise compatability with existing applications.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
struct FSK * fsk_create_core(int Fs, int Rs, int M, int P, int Nsym, int tx_f1, int tx_fs)
|
|
{
|
|
struct FSK *fsk;
|
|
int i;
|
|
|
|
/* Check configuration validity */
|
|
assert(Fs > 0 );
|
|
assert(Rs > 0 );
|
|
assert(tx_f1 > 0);
|
|
assert(tx_fs > 0);
|
|
assert(P > 0);
|
|
assert(Nsym > 0);
|
|
/* Ts (Fs/Rs) must be an integer */
|
|
assert( (Fs%Rs) == 0 );
|
|
/* Ts/P (Fs/Rs/P) must be an integer */
|
|
assert( ((Fs/Rs)%P) == 0 );
|
|
assert( M==2 || M==4);
|
|
|
|
fsk = (struct FSK*) malloc(sizeof(struct FSK)); assert(fsk != NULL);
|
|
|
|
// Need enough bins to with 10% of tone centre
|
|
float bin_width_Hz = 0.1*Rs;
|
|
float Ndft = (float)Fs/bin_width_Hz;
|
|
Ndft = pow(2.0, ceil(log2(Ndft)));
|
|
|
|
/* Set constant config parameters */
|
|
fsk->Fs = Fs;
|
|
fsk->Rs = Rs;
|
|
fsk->Ts = Fs/Rs;
|
|
fsk->burst_mode = 0;
|
|
fsk->P = P;
|
|
fsk->Nsym = Nsym;
|
|
fsk->N = fsk->Ts*fsk->Nsym;
|
|
fsk->Ndft = Ndft;
|
|
fsk->tc = 0.1;
|
|
fsk->Nmem = fsk->N+(2*fsk->Ts);
|
|
fsk->f1_tx = tx_f1;
|
|
fsk->fs_tx = tx_fs;
|
|
fsk->nin = fsk->N;
|
|
fsk->lock_nin = 0;
|
|
fsk->mode = M==2 ? MODE_2FSK : MODE_4FSK;
|
|
fsk->Nbits = M==2 ? fsk->Nsym : fsk->Nsym*2;
|
|
fsk->est_min = 0;
|
|
fsk->est_max = Fs;
|
|
fsk->est_space = 0.75*Rs;
|
|
|
|
//printf("C.....: M: %d Fs: %d Rs: %d Ts: %d nsym: %d nbit: %d N: %d Ndft: %d fmin: %d fmax: %d\n",
|
|
// M, fsk->Fs, fsk->Rs, fsk->Ts, fsk->Nsym, fsk->Nbits, fsk->N, fsk->Ndft, fsk->est_min, fsk->est_max);
|
|
/* Set up rx state */
|
|
for(i=0; i<M; i++)
|
|
fsk->phi_c[i] = comp_exp_j(0);
|
|
fsk->f_dc = (COMP*)malloc(M*fsk->Nmem*sizeof(COMP)); assert(fsk->f_dc != NULL);
|
|
for(i=0; i<M*fsk->Nmem; i++)
|
|
fsk->f_dc[i] = comp0();
|
|
|
|
fsk->fft_cfg = kiss_fft_alloc(Ndft,0,NULL,NULL); assert(fsk->fft_cfg != NULL);
|
|
fsk->Sf = (float*)malloc(sizeof(float)*fsk->Ndft); assert(fsk->Sf != NULL);
|
|
|
|
#ifdef USE_HANN_TABLE
|
|
#ifdef GENERATE_HANN_TABLE_RUNTIME
|
|
fsk->hann_table = (float*)malloc(sizeof(float)*fsk->Ndft); assert(fsk->hann_table != NULL);
|
|
fsk_generate_hann_table(fsk);
|
|
#else
|
|
fsk->hann_table = NULL;
|
|
#endif
|
|
#endif
|
|
|
|
for(i=0;i<Ndft;i++)fsk->Sf[i] = 0;
|
|
|
|
fsk->norm_rx_timing = 0;
|
|
|
|
/* Set up tx state */
|
|
fsk->tx_phase_c = comp_exp_j(0);
|
|
|
|
/* Set up demod stats */
|
|
fsk->EbNodB = 0;
|
|
|
|
for( i=0; i<M; i++)
|
|
fsk->f_est[i] = 0;
|
|
|
|
fsk->ppm = 0;
|
|
|
|
fsk->stats = (struct MODEM_STATS*)malloc(sizeof(struct MODEM_STATS)); assert(fsk->stats != NULL);
|
|
stats_init(fsk);
|
|
fsk->normalise_eye = 1;
|
|
|
|
return fsk;
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------* \
|
|
|
|
FUNCTION....: fsk_create
|
|
AUTHOR......: Brady O'Brien
|
|
DATE CREATED: 7 January 2016
|
|
|
|
Create and initialize an instance of the FSK modem. Returns a pointer
|
|
to the modem state/config struct. One modem config struct may be used
|
|
for both mod and demod.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
struct FSK * fsk_create(int Fs, int Rs, int M, int tx_f1, int tx_fs) {
|
|
return fsk_create_core(Fs, Rs, M, FSK_DEFAULT_P, FSK_DEFAULT_NSYM, tx_f1, tx_fs);
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fsk_create_hbr
|
|
AUTHOR......: Brady O'Brien
|
|
DATE CREATED: 11 February 2016
|
|
|
|
Alternate version of create allows user defined decimation P and
|
|
Nsym. In the current version of the demod it's simply an alias for
|
|
the default core function.
|
|
|
|
P is the decimation rate, so the intermal demod processing happens
|
|
at Fs/P Hz. Nsym is the number of symbols we average demod
|
|
parameters like symbol timing over.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
struct FSK * fsk_create_hbr(int Fs, int Rs, int M, int P, int Nsym, int tx_f1, int tx_fs) {
|
|
return fsk_create_core(Fs, Rs, M, P, Nsym, tx_f1, tx_fs);
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fsk_destroy
|
|
AUTHOR......: Brady O'Brien
|
|
DATE CREATED: 11 February 2016
|
|
|
|
Call this to free all memory and shut down the modem.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
void fsk_destroy(struct FSK *fsk){
|
|
free(fsk->f_dc);
|
|
free(fsk->fft_cfg);
|
|
free(fsk->stats);
|
|
free(fsk->hann_table);
|
|
free(fsk);
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fsk_mod
|
|
AUTHOR......: Brady O'Brien
|
|
DATE CREATED: 11 February 2016
|
|
|
|
FSK modulator function, real valued output samples.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
void fsk_mod(struct FSK *fsk,float fsk_out[],uint8_t tx_bits[]){
|
|
COMP tx_phase_c = fsk->tx_phase_c; /* Current complex TX phase */
|
|
int f1_tx = fsk->f1_tx; /* '0' frequency */
|
|
int fs_tx = fsk->fs_tx; /* space between frequencies */
|
|
int Ts = fsk->Ts; /* samples-per-symbol */
|
|
int Fs = fsk->Fs; /* sample freq */
|
|
int M = fsk->mode;
|
|
COMP dosc_f[M]; /* phase shift per sample */
|
|
COMP dph; /* phase shift of current bit */
|
|
size_t i,j,m,bit_i,sym;
|
|
|
|
/* Init the per sample phase shift complex numbers */
|
|
for( m=0; m<M; m++){
|
|
dosc_f[m] = comp_exp_j(2*M_PI*((float)(f1_tx+(fs_tx*m))/(float)(Fs)));
|
|
}
|
|
|
|
bit_i = 0;
|
|
for( i=0; i<fsk->Nsym; i++){
|
|
sym = 0;
|
|
/* Pack the symbol number from the bit stream */
|
|
for( m=M; m>>=1; ){
|
|
uint8_t bit = tx_bits[bit_i];
|
|
bit = (bit==1)?1:0;
|
|
sym = (sym<<1)|bit;
|
|
bit_i++;
|
|
}
|
|
/* Look up symbol phase shift */
|
|
dph = dosc_f[sym];
|
|
/* Spin the oscillator for a symbol period */
|
|
for(j=0; j<Ts; j++){
|
|
tx_phase_c = cmult(tx_phase_c,dph);
|
|
fsk_out[i*Ts+j] = 2*tx_phase_c.real;
|
|
}
|
|
|
|
}
|
|
|
|
/* Normalize TX phase to prevent drift */
|
|
tx_phase_c = comp_normalize(tx_phase_c);
|
|
|
|
/* save TX phase */
|
|
fsk->tx_phase_c = tx_phase_c;
|
|
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fsk_mod_c
|
|
AUTHOR......: Brady O'Brien
|
|
DATE CREATED: 11 February 2016
|
|
|
|
FSK modulator function, complex valued output samples.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
void fsk_mod_c(struct FSK *fsk,COMP fsk_out[],uint8_t tx_bits[]){
|
|
COMP tx_phase_c = fsk->tx_phase_c; /* Current complex TX phase */
|
|
int f1_tx = fsk->f1_tx; /* '0' frequency */
|
|
int fs_tx = fsk->fs_tx; /* space between frequencies */
|
|
int Ts = fsk->Ts; /* samples-per-symbol */
|
|
int Fs = fsk->Fs; /* sample freq */
|
|
int M = fsk->mode;
|
|
COMP dosc_f[M]; /* phase shift per sample */
|
|
COMP dph; /* phase shift of current bit */
|
|
size_t i,j,bit_i,sym;
|
|
int m;
|
|
|
|
/* Init the per sample phase shift complex numbers */
|
|
for( m=0; m<M; m++){
|
|
dosc_f[m] = comp_exp_j(2*M_PI*((float)(f1_tx+(fs_tx*m))/(float)(Fs)));
|
|
}
|
|
|
|
bit_i = 0;
|
|
for( i=0; i<fsk->Nsym; i++){
|
|
sym = 0;
|
|
/* Pack the symbol number from the bit stream */
|
|
for( m=M; m>>=1; ){
|
|
uint8_t bit = tx_bits[bit_i];
|
|
bit = (bit==1)?1:0;
|
|
sym = (sym<<1)|bit;
|
|
bit_i++;
|
|
}
|
|
/* Look up symbol phase shift */
|
|
dph = dosc_f[sym];
|
|
/* Spin the oscillator for a symbol period */
|
|
for(j=0; j<Ts; j++){
|
|
tx_phase_c = cmult(tx_phase_c,dph);
|
|
fsk_out[i*Ts+j] = fcmult(2,tx_phase_c);
|
|
}
|
|
}
|
|
|
|
/* Normalize TX phase to prevent drift */
|
|
tx_phase_c = comp_normalize(tx_phase_c);
|
|
|
|
/* save TX phase */
|
|
fsk->tx_phase_c = tx_phase_c;
|
|
|
|
}
|
|
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fsk_mod_ext_vco
|
|
AUTHOR......: David Rowe
|
|
DATE CREATED: February 2018
|
|
|
|
Modulator that assume an external VCO. The output is a voltage
|
|
that changes for each symbol.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
void fsk_mod_ext_vco(struct FSK *fsk, float vco_out[], uint8_t tx_bits[]) {
|
|
int f1_tx = fsk->f1_tx; /* '0' frequency */
|
|
int fs_tx = fsk->fs_tx; /* space between frequencies */
|
|
int Ts = fsk->Ts; /* samples-per-symbol */
|
|
int M = fsk->mode;
|
|
int i, j, m, sym, bit_i;
|
|
|
|
bit_i = 0;
|
|
for(i=0; i<fsk->Nsym; i++) {
|
|
/* generate the symbol number from the bit stream,
|
|
e.g. 0,1 for 2FSK, 0,1,2,3 for 4FSK */
|
|
|
|
sym = 0;
|
|
|
|
/* unpack the symbol number from the bit stream */
|
|
|
|
for( m=M; m>>=1; ){
|
|
uint8_t bit = tx_bits[bit_i];
|
|
bit = (bit==1)?1:0;
|
|
sym = (sym<<1)|bit;
|
|
bit_i++;
|
|
}
|
|
|
|
/*
|
|
Map 'sym' to VCO frequency
|
|
Note: drive is inverted, a higher tone drives VCO voltage lower
|
|
*/
|
|
|
|
//fprintf(stderr, "i: %d sym: %d freq: %f\n", i, sym, f1_tx + fs_tx*(float)sym);
|
|
for(j=0; j<Ts; j++) {
|
|
vco_out[i*Ts+j] = f1_tx + fs_tx*(float)sym;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fsk_nin
|
|
AUTHOR......: Brady O'Brien
|
|
DATE CREATED: 11 February 2016
|
|
|
|
Call me before each call to fsk_demod() to determine how many new
|
|
samples you should pass in. the number of samples will vary due to
|
|
timing variations.
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
uint32_t fsk_nin(struct FSK *fsk){
|
|
return (uint32_t)fsk->nin;
|
|
}
|
|
|
|
/*
|
|
* Internal function to estimate the frequencies of the FSK tones.
|
|
* This is split off because it is fairly complicated, needs a bunch of memory, and probably
|
|
* takes more cycles than the rest of the demod.
|
|
* Parameters:
|
|
* fsk - FSK struct from demod containing FSK config
|
|
* fsk_in - block of samples in this demod cycles, must be nin long
|
|
* freqs - Array for the estimated frequencies
|
|
* M - number of frequency peaks to find
|
|
*/
|
|
void fsk_demod_freq_est(struct FSK *fsk, COMP fsk_in[], float *freqs, int M) {
|
|
int Ndft = fsk->Ndft;
|
|
int Fs = fsk->Fs;
|
|
int nin = fsk->nin;
|
|
size_t i,j;
|
|
float hann;
|
|
float max;
|
|
int imax;
|
|
kiss_fft_cfg fft_cfg = fsk->fft_cfg;
|
|
int freqi[M];
|
|
int st,en,f_zero;
|
|
|
|
/* Array to do complex FFT from using kiss_fft */
|
|
#ifdef DEMOD_ALLOC_STACK
|
|
kiss_fft_cpx *fftin = (kiss_fft_cpx*)alloca(sizeof(kiss_fft_cpx)*Ndft);
|
|
kiss_fft_cpx *fftout = (kiss_fft_cpx*)alloca(sizeof(kiss_fft_cpx)*Ndft);
|
|
#else
|
|
kiss_fft_cpx *fftin = (kiss_fft_cpx*)malloc(sizeof(kiss_fft_cpx)*Ndft);
|
|
kiss_fft_cpx *fftout = (kiss_fft_cpx*)malloc(sizeof(kiss_fft_cpx)*Ndft);
|
|
#endif
|
|
|
|
st = (fsk->est_min*Ndft)/Fs + Ndft/2; if (st < 0) st = 0;
|
|
en = (fsk->est_max*Ndft)/Fs + Ndft/2; if (en > Ndft) en = Ndft;
|
|
//fprintf(stderr, "min: %d max: %d st: %d en: %d\n", fsk->est_min, fsk->est_max, st, en);
|
|
|
|
f_zero = (fsk->est_space*Ndft)/Fs;
|
|
|
|
int numffts = floor((float)nin/(Ndft/2)) - 1;
|
|
for(j=0; j<numffts; j++){
|
|
int a = j*Ndft/2;
|
|
//fprintf(stderr, "numffts: %d j: %d a: %d\n", numffts, (int)j, a);
|
|
/* Copy FSK buffer into reals of FFT buffer and apply a hann window */
|
|
for(i=0; i<Ndft; i++){
|
|
#ifdef USE_HANN_TABLE
|
|
hann = fsk->hann_table[i];
|
|
#else
|
|
hann = 0.5 - 0.5 * cosf(2.0 * M_PI * (float)i / (float) (fft_samps-1));
|
|
#endif
|
|
fftin[i].r = hann*fsk_in[i+a].real;
|
|
fftin[i].i = hann*fsk_in[i+a].imag;
|
|
}
|
|
|
|
/* Do the FFT */
|
|
kiss_fft(fft_cfg,fftin,fftout);
|
|
|
|
/* FFT shift to put DC bin at Ndft/2 */
|
|
kiss_fft_cpx tmp;
|
|
for(i=0; i<Ndft/2; i++) {
|
|
tmp = fftout[i];
|
|
fftout[i] = fftout[i+Ndft/2];
|
|
fftout[i+Ndft/2] = tmp;
|
|
}
|
|
|
|
/* Find the magnitude^2 of each freq slot */
|
|
for(i=0; i<Ndft; i++) {
|
|
fftout[i].r = (fftout[i].r*fftout[i].r) + (fftout[i].i*fftout[i].i) ;
|
|
}
|
|
|
|
/* Mix back in with the previous fft block */
|
|
/* Copy new fft est into imag of fftout for frequency divination below */
|
|
float tc = fsk->tc;
|
|
for(i=0; i<Ndft; i++){
|
|
fsk->Sf[i] = (fsk->Sf[i]*(1-tc)) + (sqrtf(fftout[i].r)*tc);
|
|
fftout[i].i = fsk->Sf[i];
|
|
}
|
|
}
|
|
|
|
modem_probe_samp_f("t_Sf",fsk->Sf,Ndft);
|
|
|
|
max = 0;
|
|
/* Find the M frequency peaks here */
|
|
for(i=0; i<M; i++){
|
|
imax = 0;
|
|
max = 0;
|
|
for(j=st;j<en;j++){
|
|
if(fftout[j].i > max){
|
|
max = fftout[j].i;
|
|
imax = j;
|
|
}
|
|
}
|
|
/* Blank out FMax +/-Fspace/2 */
|
|
int f_min, f_max;
|
|
f_min = imax - f_zero;
|
|
f_min = f_min < 0 ? 0 : f_min;
|
|
f_max = imax + f_zero;
|
|
f_max = f_max > Ndft ? Ndft : f_max;
|
|
for(j=f_min; j<f_max; j++)
|
|
fftout[j].i = 0;
|
|
|
|
/* Stick the freq index on the list */
|
|
freqi[i] = imax - Ndft/2;
|
|
}
|
|
|
|
/* Gnome sort the freq list */
|
|
/* My favorite sort of sort*/
|
|
i = 1;
|
|
while(i<M){
|
|
if(freqi[i] >= freqi[i-1]) i++;
|
|
else{
|
|
j = freqi[i];
|
|
freqi[i] = freqi[i-1];
|
|
freqi[i-1] = j;
|
|
if(i>1) i--;
|
|
}
|
|
}
|
|
|
|
/* Convert freqs from indices to frequencies */
|
|
for(i=0; i<M; i++){
|
|
freqs[i] = (float)(freqi[i])*((float)Fs/(float)Ndft);
|
|
}
|
|
|
|
/* Search for each tone method 2 - correlate with mask with non-zero entries at tone spacings ----- */
|
|
|
|
/* construct mask */
|
|
float mask[Ndft];
|
|
for(i=0; i<Ndft; i++) mask[i] = 0.0;
|
|
for(i=0;i<3; i++) mask[i] = 1.0;
|
|
int bin=0;
|
|
for(int m=1; m<=M-1; m++) {
|
|
bin = round((float)m*fsk->fs_tx*Ndft/Fs)-1;
|
|
for(i=bin; i<=bin+2; i++) mask[i] = 1.0;
|
|
}
|
|
int len_mask = bin+2+1;
|
|
|
|
#ifdef MODEMPROBE_ENABLE
|
|
modem_probe_samp_f("t_mask",mask,len_mask);
|
|
#endif
|
|
|
|
/* drag mask over Sf, looking for peak in correlation */
|
|
int b_max = st; float corr_max = 0.0;
|
|
float *Sf = fsk->Sf;
|
|
for (int b=st; b<en-len_mask; b++) {
|
|
float corr = 0.0;
|
|
for(i=0; i<len_mask; i++)
|
|
corr += mask[i] * Sf[b+i];
|
|
if (corr > corr_max) {
|
|
corr_max = corr;
|
|
b_max = b;
|
|
}
|
|
}
|
|
float foff = (b_max-Ndft/2)*Fs/Ndft;
|
|
//fprintf(stderr, "fsk->fs_tx: %d\n",fsk->fs_tx);
|
|
for (int m=0; m<M; m++)
|
|
fsk->f2_est[m] = foff + m*fsk->fs_tx;
|
|
#ifdef MODEMPROBE_ENABLE
|
|
modem_probe_samp_f("t_f2_est",fsk->f2_est,M);
|
|
#endif
|
|
|
|
#ifndef DEMOD_ALLOC_STACK
|
|
free(fftin);
|
|
free(fftout);
|
|
#endif
|
|
}
|
|
|
|
/* core demodulator function */
|
|
void fsk_demod_core(struct FSK *fsk, uint8_t rx_bits[], float rx_sd[], COMP fsk_in[]){
|
|
int N = fsk->N;
|
|
int Ts = fsk->Ts;
|
|
int Rs = fsk->Rs;
|
|
int Fs = fsk->Fs;
|
|
int nsym = fsk->Nsym;
|
|
int nin = fsk->nin;
|
|
int P = fsk->P;
|
|
int Nmem = fsk->Nmem;
|
|
int M = fsk->mode;
|
|
size_t i,j,m;
|
|
float ft1;
|
|
|
|
COMP t[M]; /* complex number temps */
|
|
COMP t_c; /* another complex temp */
|
|
COMP *phi_c = fsk->phi_c;
|
|
COMP *f_dc = fsk->f_dc;
|
|
COMP phi_ft;
|
|
int nold = Nmem-nin;
|
|
|
|
COMP dphift;
|
|
float rx_timing,norm_rx_timing,old_norm_rx_timing,d_norm_rx_timing,appm;
|
|
|
|
float fc_avg,fc_tx;
|
|
float meanebno,stdebno,eye_max;
|
|
int neyesamp,neyeoffset;
|
|
|
|
#ifdef MODEMPROBE_ENABLE
|
|
#define NMP_NAME 26
|
|
char mp_name_tmp[NMP_NAME+1]; /* Temporary string for modem probe trace names */
|
|
#endif
|
|
|
|
/* Estimate tone frequencies */
|
|
fsk_demod_freq_est(fsk,fsk_in,fsk->f_est,M);
|
|
#ifdef MODEMPROBE_ENABLE
|
|
modem_probe_samp_f("t_f_est",fsk->f_est,M);
|
|
#endif
|
|
float *f_est;
|
|
|
|
if (fsk->freq_est_type){
|
|
f_est = fsk->f2_est;
|
|
}else{
|
|
f_est = fsk->f_est;
|
|
}
|
|
/* update filter (integrator) memory by shifting in nin samples */
|
|
for(m=0; m<M; m++) {
|
|
for(i=0,j=Nmem-nold; i<nold; i++,j++)
|
|
f_dc[m*Nmem+i] = f_dc[m*Nmem+j];
|
|
}
|
|
|
|
/* freq shift down to around DC, ensuring continuous phase from last frame */
|
|
COMP dphi_m;
|
|
for(m=0; m<M; m++) {
|
|
dphi_m = comp_exp_j(2*M_PI*((f_est[m])/(float)(Fs)));
|
|
for(i=nold,j=0; i<Nmem; i++,j++) {
|
|
phi_c[m] = cmult(phi_c[m],dphi_m);
|
|
f_dc[m*Nmem+i] = cmult(fsk_in[j],cconj(phi_c[m]));
|
|
//f_dc[m*Nmem+i] = cconj(phi_c[m]);
|
|
}
|
|
phi_c[m] = comp_normalize(phi_c[m]);
|
|
#ifdef MODEMPROBE_ENABLE
|
|
snprintf(mp_name_tmp,NMP_NAME,"t_f%zd_dc",m+1);
|
|
modem_probe_samp_c(mp_name_tmp,&f_dc[m*Nmem],Nmem);
|
|
#endif
|
|
}
|
|
|
|
/* integrate over symbol period at a variety of offsets */
|
|
COMP f_int[M][(nsym+1)*P];
|
|
for(i=0; i<(nsym+1)*P; i++) {
|
|
int st = i*Ts/P;
|
|
int en = st+Ts-1;
|
|
for(m=0; m<M; m++) {
|
|
f_int[m][i] = comp0();
|
|
for(j=st; j<=en; j++)
|
|
f_int[m][i] = cadd(f_int[m][i], f_dc[m*Nmem+j]);
|
|
}
|
|
}
|
|
|
|
#ifdef MODEMPROBE_ENABLE
|
|
for(m=0; m<M; m++) {
|
|
snprintf(mp_name_tmp,NMP_NAME,"t_f%zd_int",m+1);
|
|
modem_probe_samp_c(mp_name_tmp,&f_int[m][0],(nsym+1)*P);
|
|
}
|
|
#endif
|
|
|
|
/* Fine Timing Estimation */
|
|
/* Apply magic nonlinearity to f1_int and f2_int, shift down to 0,
|
|
* extract angle */
|
|
|
|
/* Figure out how much to spin the oscillator to extract magic spectral line */
|
|
dphift = comp_exp_j(2*M_PI*((float)(Rs)/(float)(P*Rs)));
|
|
phi_ft.real = 1;
|
|
phi_ft.imag = 0;
|
|
t_c=comp0();
|
|
for(i=0; i<(nsym+1)*P; i++){
|
|
/* Get abs^2 of fx_int[i], and add 'em */
|
|
ft1 = 0;
|
|
for( m=0; m<M; m++){
|
|
ft1 += (f_int[m][i].real*f_int[m][i].real) + (f_int[m][i].imag*f_int[m][i].imag);
|
|
}
|
|
|
|
/* Down shift and accumulate magic line */
|
|
t_c = cadd(t_c,fcmult(ft1,phi_ft));
|
|
|
|
/* Spin the oscillator for the magic line shift */
|
|
phi_ft = cmult(phi_ft,dphift);
|
|
}
|
|
|
|
/* Check for NaNs in the fine timing estimate, return if found */
|
|
/* otherwise segfaults happen */
|
|
if( isnan(t_c.real) || isnan(t_c.imag)){
|
|
return;
|
|
}
|
|
|
|
/* Get the magic angle */
|
|
norm_rx_timing = atan2f(t_c.imag,t_c.real)/(2*M_PI);
|
|
rx_timing = norm_rx_timing*(float)P;
|
|
|
|
old_norm_rx_timing = fsk->norm_rx_timing;
|
|
fsk->norm_rx_timing = norm_rx_timing;
|
|
|
|
/* Estimate sample clock offset */
|
|
d_norm_rx_timing = norm_rx_timing - old_norm_rx_timing;
|
|
|
|
/* Filter out big jumps in due to nin change */
|
|
if(fabsf(d_norm_rx_timing) < .2){
|
|
appm = 1e6*d_norm_rx_timing/(float)nsym;
|
|
fsk->ppm = .9*fsk->ppm + .1*appm;
|
|
}
|
|
|
|
/* Figure out how many samples are needed the next modem cycle */
|
|
/* Unless we're in burst mode or nin locked */
|
|
if(!fsk->burst_mode && !fsk->lock_nin) {
|
|
if(norm_rx_timing > 0.25)
|
|
fsk->nin = N+Ts/2;
|
|
else if(norm_rx_timing < -0.25)
|
|
fsk->nin = N-Ts/2;
|
|
else
|
|
fsk->nin = N;
|
|
}
|
|
|
|
modem_probe_samp_f("t_norm_rx_timing",&(norm_rx_timing),1);
|
|
modem_probe_samp_i("t_nin",&(fsk->nin),1);
|
|
|
|
/* Re-sample the integrators with linear interpolation magic */
|
|
int low_sample = (int)floorf(rx_timing);
|
|
float fract = rx_timing - (float)low_sample;
|
|
int high_sample = (int)ceilf(rx_timing);
|
|
|
|
/* Vars for finding the max-of-4 for each bit */
|
|
float tmax[M];
|
|
|
|
#ifdef EST_EBNO
|
|
meanebno = 0;
|
|
stdebno = 0;
|
|
#endif
|
|
|
|
/* FINALLY, THE BITS */
|
|
/* also, resample fx_int */
|
|
for(i=0; i<nsym; i++){
|
|
int st = (i+1)*P;
|
|
for( m=0; m<M; m++){
|
|
t[m] = fcmult(1-fract,f_int[m][st+ low_sample]);
|
|
t[m] = cadd(t[m],fcmult( fract,f_int[m][st+high_sample]));
|
|
/* Figure mag^2 of each resampled fx_int */
|
|
tmax[m] = (t[m].real*t[m].real) + (t[m].imag*t[m].imag);
|
|
}
|
|
|
|
float max = tmax[0]; /* Maximum for figuring correct symbol */
|
|
float min = tmax[0];
|
|
int sym = 0; /* Index of maximum */
|
|
for( m=0; m<M; m++){
|
|
if(tmax[m]>max){
|
|
max = tmax[m];
|
|
sym = m;
|
|
}
|
|
if(tmax[m]<min){
|
|
min = tmax[m];
|
|
}
|
|
}
|
|
|
|
/* Get the actual bit */
|
|
if(rx_bits != NULL){
|
|
/* Get bits for 2FSK and 4FSK */
|
|
/* TODO: Replace this with something more generic maybe */
|
|
if(M==2){
|
|
rx_bits[i] = sym==1; /* 2FSK. 1 bit per symbol */
|
|
}else if(M==4){
|
|
rx_bits[(i*2)+1] = (sym&0x1); /* 4FSK. 2 bits per symbol */
|
|
rx_bits[(i*2) ] = (sym&0x2)>>1;
|
|
}
|
|
}
|
|
|
|
/* Produce soft decision symbols */
|
|
if(rx_sd != NULL){
|
|
/* Convert symbols from max^2 into max */
|
|
for( m=0; m<M; m++)
|
|
tmax[m] = sqrtf(tmax[m]);
|
|
|
|
if(M==2){
|
|
rx_sd[i] = tmax[0] - tmax[1];
|
|
}else if(M==4){
|
|
/* TODO: Find a soft-decision mode that works for 4FSK */
|
|
min = sqrtf(min);
|
|
rx_sd[(i*2)+1] = - tmax[0] ; /* Bits=00 */
|
|
rx_sd[(i*2) ] = - tmax[0] ;
|
|
rx_sd[(i*2)+1]+= tmax[1] ; /* Bits=01 */
|
|
rx_sd[(i*2) ]+= - tmax[1] ;
|
|
rx_sd[(i*2)+1]+= - tmax[2] ; /* Bits=10 */
|
|
rx_sd[(i*2) ]+= tmax[2] ;
|
|
rx_sd[(i*2)+1]+= tmax[3] ; /* Bits=11 */
|
|
rx_sd[(i*2) ]+= tmax[3] ;
|
|
}
|
|
}
|
|
/* Accumulate resampled int magnitude for EbNodB estimation */
|
|
/* Standard deviation is calculated by algorithm devised by crafty soviets */
|
|
#ifdef EST_EBNO
|
|
/* Accumulate the square of the sampled value */
|
|
ft1 = max;
|
|
stdebno += ft1;
|
|
|
|
/* Figure the abs value of the max tone */
|
|
meanebno += sqrtf(ft1);
|
|
#endif
|
|
/* Soft output goes here */
|
|
}
|
|
|
|
#ifdef EST_EBNO
|
|
|
|
/* Calculate mean for EbNodB estimation */
|
|
meanebno = meanebno/(float)nsym;
|
|
|
|
/* Calculate the std. dev for EbNodB estimate */
|
|
stdebno = (stdebno/(float)nsym) - (meanebno*meanebno);
|
|
/* trap any negative numbers to avoid NANs flowing through */
|
|
if (stdebno > 0.0) {
|
|
stdebno = sqrt(stdebno);
|
|
} else {
|
|
stdebno = 0.0;
|
|
}
|
|
|
|
fsk->EbNodB = -6+(20*log10f((1e-6+meanebno)/(1e-6+stdebno)));
|
|
#else
|
|
fsk->EbNodB = 1;
|
|
#endif
|
|
|
|
/* Write some statistics to the stats struct */
|
|
|
|
/* Save clock offset in ppm */
|
|
fsk->stats->clock_offset = fsk->ppm;
|
|
|
|
/* Calculate and save SNR from EbNodB estimate */
|
|
|
|
fsk->stats->snr_est = .5*fsk->stats->snr_est + .5*fsk->EbNodB;//+ 10*log10f(((float)Rs)/((float)Rs*M));
|
|
|
|
/* Save rx timing */
|
|
fsk->stats->rx_timing = (float)rx_timing;
|
|
|
|
/* Estimate and save frequency offset */
|
|
fc_avg = fc_tx = 0.0;
|
|
for(int m=0; m<M; m++) {
|
|
fc_avg += f_est[m]/M;
|
|
fc_tx += (fsk->f1_tx + m*fsk->fs_tx)/M;
|
|
}
|
|
fsk->stats->foff = fc_tx-fc_avg;
|
|
|
|
/* Take a sample for the eye diagrams ---------------------------------- */
|
|
|
|
/* due to oversample rate P, we have too many samples for eye
|
|
trace. So lets output a decimated version. We use 2P
|
|
as we want two symbols worth of samples in trace */
|
|
|
|
int neyesamp_dec = ceil(((float)P*2)/MODEM_STATS_EYE_IND_MAX);
|
|
neyesamp = (P*2)/neyesamp_dec;
|
|
assert(neyesamp <= MODEM_STATS_EYE_IND_MAX);
|
|
fsk->stats->neyesamp = neyesamp;
|
|
|
|
neyeoffset = high_sample+1;
|
|
|
|
int eye_traces = MODEM_STATS_ET_MAX/M;
|
|
int ind;
|
|
|
|
fsk->stats->neyetr = fsk->mode*eye_traces;
|
|
for( i=0; i<eye_traces; i++){
|
|
for ( m=0; m<M; m++){
|
|
for(j=0; j<neyesamp; j++) {
|
|
/*
|
|
2*P*i...........: advance two symbols for next trace
|
|
neyeoffset......: centre trace on ideal timing offset, peak eye opening
|
|
j*neweyesamp_dec: For 2*P>MODEM_STATS_EYE_IND_MAX advance through integrated
|
|
samples newamp_dec at a time so we dont overflow rx_eye[][]
|
|
*/
|
|
ind = 2*P*i + neyeoffset + j*neyesamp_dec;
|
|
assert((i*M+m) < MODEM_STATS_ET_MAX);
|
|
assert(ind < (nsym+1)*P);
|
|
fsk->stats->rx_eye[i*M+m][j] = cabsolute(f_int[m][ind]);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (fsk->normalise_eye) {
|
|
eye_max = 0;
|
|
/* Normalize eye to +/- 1 */
|
|
for(i=0; i<M*eye_traces; i++)
|
|
for(j=0; j<neyesamp; j++)
|
|
if(fabsf(fsk->stats->rx_eye[i][j])>eye_max)
|
|
eye_max = fabsf(fsk->stats->rx_eye[i][j]);
|
|
|
|
for(i=0; i<M*eye_traces; i++)
|
|
for(j=0; j<neyesamp; j++)
|
|
fsk->stats->rx_eye[i][j] = fsk->stats->rx_eye[i][j]/eye_max;
|
|
}
|
|
|
|
fsk->stats->nr = 0;
|
|
fsk->stats->Nc = 0;
|
|
|
|
for(i=0; i<M; i++)
|
|
fsk->stats->f_est[i] = f_est[i];
|
|
|
|
/* Dump some internal samples */
|
|
modem_probe_samp_f("t_EbNodB",&(fsk->EbNodB),1);
|
|
modem_probe_samp_f("t_ppm",&(fsk->ppm),1);
|
|
modem_probe_samp_f("t_rx_timing",&(rx_timing),1);
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*\
|
|
|
|
FUNCTION....: fsk_demod/fsk_demod_sd
|
|
AUTHOR......: Brady O'Brien
|
|
DATE CREATED: 11 February 2016
|
|
|
|
FSK demodulator functions:
|
|
|
|
fsk_demod...: complex samples in, bits out
|
|
fsk_demos_sd: complex samples in, soft decision symbols out
|
|
|
|
\*---------------------------------------------------------------------------*/
|
|
|
|
void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], COMP fsk_in[]){
|
|
fsk_demod_core(fsk,rx_bits,NULL,fsk_in);
|
|
}
|
|
|
|
void fsk_demod_sd(struct FSK *fsk, float rx_sd[], COMP fsk_in[]){
|
|
fsk_demod_core(fsk,NULL,rx_sd,fsk_in);
|
|
}
|
|
|
|
/* make sure stats have known values in case monitoring process reads stats before they are set */
|
|
static void stats_init(struct FSK *fsk) {
|
|
/* Take a sample for the eye diagrams */
|
|
int i,j,m;
|
|
int P = fsk->P;
|
|
int M = fsk->mode;
|
|
|
|
/* due to oversample rate P, we have too many samples for eye
|
|
trace. So lets output a decimated version */
|
|
|
|
/* asserts below as we found some problems over-running eye matrix */
|
|
|
|
/* TODO: refactor eye tracing code here and in fsk_demod */
|
|
|
|
int neyesamp_dec = ceil(((float)P*2)/MODEM_STATS_EYE_IND_MAX);
|
|
int neyesamp = (P*2)/neyesamp_dec;
|
|
assert(neyesamp <= MODEM_STATS_EYE_IND_MAX);
|
|
fsk->stats->neyesamp = neyesamp;
|
|
|
|
int eye_traces = MODEM_STATS_ET_MAX/M;
|
|
|
|
fsk->stats->neyetr = fsk->mode*eye_traces;
|
|
for(i=0; i<eye_traces; i++) {
|
|
for (m=0; m<M; m++){
|
|
for(j=0; j<neyesamp; j++) {
|
|
assert((i*M+m) < MODEM_STATS_ET_MAX);
|
|
fsk->stats->rx_eye[i*M+m][j] = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
fsk->stats->rx_timing = fsk->stats->snr_est = 0;
|
|
|
|
}
|
|
|
|
|
|
/* Set the FSK modem into burst demod mode */
|
|
|
|
void fsk_enable_burst_mode(struct FSK *fsk){
|
|
fsk->nin = fsk->N;
|
|
fsk->burst_mode = 1;
|
|
}
|
|
|
|
void fsk_clear_estimators(struct FSK *fsk){
|
|
int i;
|
|
/* Clear freq estimator state */
|
|
for(i=0; i < (fsk->Ndft); i++){
|
|
fsk->Sf[i] = 0;
|
|
}
|
|
/* Reset timing diff correction */
|
|
fsk->nin = fsk->N;
|
|
}
|
|
|
|
void fsk_get_demod_stats(struct FSK *fsk, struct MODEM_STATS *stats){
|
|
/* copy from internal stats, note we can't overwrite stats completely
|
|
as it has other states rqd by caller, also we want a consistent
|
|
interface across modem types for the freedv_api.
|
|
*/
|
|
|
|
stats->clock_offset = fsk->stats->clock_offset;
|
|
stats->snr_est = fsk->stats->snr_est; // TODO: make this SNR not Eb/No
|
|
stats->rx_timing = fsk->stats->rx_timing;
|
|
stats->foff = fsk->stats->foff;
|
|
|
|
stats->neyesamp = fsk->stats->neyesamp;
|
|
stats->neyetr = fsk->stats->neyetr;
|
|
memcpy(stats->rx_eye, fsk->stats->rx_eye, sizeof(stats->rx_eye));
|
|
memcpy(stats->f_est, fsk->stats->f_est, fsk->mode*sizeof(float));
|
|
|
|
|
|
/* these fields not used for FSK so set to something sensible */
|
|
|
|
stats->sync = 0;
|
|
stats->nr = fsk->stats->nr;
|
|
stats->Nc = fsk->stats->Nc;
|
|
}
|
|
|
|
/*
|
|
* Set the minimum and maximum frequencies at which the freq. estimator can find tones
|
|
*/
|
|
void fsk_set_freq_est_limits(struct FSK *fsk, int est_min, int est_max){
|
|
assert(fsk != NULL);
|
|
assert(est_min >= -fsk->Fs/2);
|
|
assert(est_max <= fsk->Fs/2);
|
|
assert(est_max > est_min);
|
|
fsk->est_min = est_min;
|
|
fsk->est_max = est_max;
|
|
}
|
|
|
|
void fsk_stats_normalise_eye(struct FSK *fsk, int normalise_enable) {
|
|
assert(fsk != NULL);
|
|
fsk->normalise_eye = normalise_enable;
|
|
}
|
|
|
|
void fsk_set_freq_est_alg(struct FSK *fsk, int est_type) {
|
|
assert(fsk != NULL);
|
|
fsk->freq_est_type = est_type;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|