From: baobrien Date: Fri, 29 Jan 2016 16:25:19 +0000 (+0000) Subject: Port of 4FSK almost complete X-Git-Url: http://git.whiteaudio.com/gitweb/?a=commitdiff_plain;h=4052520cca4bfe0591e79b613938e2c6c303b3fc;p=freetel-svn-tracking.git Port of 4FSK almost complete git-svn-id: https://svn.code.sf.net/p/freetel/code@2668 01035d8c-6547-0410-b346-abe4f91aad63 --- diff --git a/codec2-dev/octave/tfsk.m b/codec2-dev/octave/tfsk.m index 68f91604..ca397695 100644 --- a/codec2-dev/octave/tfsk.m +++ b/codec2-dev/octave/tfsk.m @@ -56,18 +56,19 @@ global tfsk_location = '../build_linux/unittest/tfsk'; fsk_horus_as_a_lib = 1; % make sure calls to test functions at bottom are disabled -fsk_horus_2fsk; +%fsk_horus_2fsk; +fsk_horus pkg load signal; pkg load parallel; graphics_toolkit('gnuplot'); -global mod_pass_fail_maxdiff = 1e-3/50000; +global mod_pass_fail_maxdiff = 1e-3/5000; -function mod = fsk_mod_c(Fs,Rs,f1,f2,bits) +function mod = fsk_mod_c(Fs,Rs,f1,fsp,bits,M) global tfsk_location; %command to be run by system to launch the modulator - command = sprintf('%s M %d %d %d %d fsk_mod_ut_bitvec fsk_mod_ut_modvec fsk_mod_ut_log.txt',tfsk_location,f1,f2,Fs,Rs); + command = sprintf('%s M %d %d %d %d %d fsk_mod_ut_bitvec fsk_mod_ut_modvec fsk_mod_ut_log.txt',tfsk_location,M,f1,fsp,Fs,Rs); %save input bits into a file bitvecfile = fopen('fsk_mod_ut_bitvec','wb+'); fwrite(bitvecfile,bits,'uint8'); @@ -84,13 +85,13 @@ endfunction %Compare 2 vectors, fail if they are not close enough -function pass = vcompare(vc,voct,vname,tname,tol) +function pass = vcompare(vc,voct,vname,tname,tol,pnum) %Get delta of vectors dvec = abs(abs(vc)-abs(voct)); %Normalize difference - dvec = dvec ./ abs(max(abs(voct))); + dvec = dvec ./ abs(max(abs(voct))+1e-8); maxdvec = abs(max(dvec)); pass = maxdvec=states.nin ninold = states.nin; + states = est_freq(states, modin(1:states.nin), states.M); [bitbuf,states] = fsk_horus_demod(states, modin(1:states.nin)); modin=modin(ninold+1:length(modin)); obits = [obits bitbuf]; %Save other parameters - o_f1_dc = [o_f1_dc states.f1_dc(1:states.Nmem-Ts/P)]; - o_f2_dc = [o_f2_dc states.f2_dc(1:states.Nmem-Ts/P)]; - o_f1_int = [o_f1_int states.f1_int]; - o_f2_int = [o_f2_int states.f2_int]; + o_f1_dc = [o_f1_dc states.f_dc(1,1:states.Nmem-Ts/P)]; + o_f2_dc = [o_f2_dc states.f_dc(2,1:states.Nmem-Ts/P)]; + o_f1_int = [o_f1_int states.f_int(1,:)]; + o_f2_int = [o_f2_int states.f_int(2,:)]; o_EbNodB = [o_EbNodB states.EbNodB]; o_ppm = [o_ppm states.ppm]; o_rx_timing = [o_rx_timing states.rx_timing]; + o_Sf = [o_Sf states.Sf']; + o_f1 = [o_f1 states.f(1)]; + o_f2 = [o_f1 states.f(2)]; + o_fest = [o_fest states.f]; + if M==4 + o_f3_dc = [o_f3_dc states.f_dc(3,1:states.Nmem-Ts/P)]; + o_f4_dc = [o_f4_dc states.f_dc(4,1:states.Nmem-Ts/P)]; + o_f3_int = [o_f3_int states.f_int(3,:)]; + o_f4_int = [o_f4_int states.f_int(4,:)]; + o_f3 = [o_f1 states.f(3)]; + o_f4 = [o_f1 states.f(4)]; + end end + close all + % One part-per-thousand allowed on important parameters - pass = vcompare(o_f1_dc, t_f1_dc, 'f1_dc', tname,.001); - pass = pass && vcompare(o_f2_dc, t_f2_dc, 'f2_dc', tname,.001); - pass = pass && vcompare(o_f1_int, t_f1_int, 'f1_int', tname,.001); - pass = pass && vcompare(o_f2_int, t_f2_int, 'f2_int', tname,.001); - pass = pass && vcompare(o_rx_timing, t_rx_timing,'rx_timing',tname,.001); + pass = 1; + - % Much larger tolerances on unimportant statistics - pass = pass && vcompare(o_EbNodB, t_EbNodB, 'EbNodB', tname,.05); - pass = pass && vcompare(o_ppm , t_ppm, 'ppm', tname,.02); + pass = vcompare(o_Sf, t_fft_est,'fft est',tname,.001,9) && pass; + pass = vcompare(o_fest, t_f_est,'f est',tname,.001,9) && pass; + pass = vcompare(o_rx_timing, t_rx_timing,'rx timing',tname,.001,10) && pass; + + if M==4 + pass = vcompare(o_f3_dc, t_f3_dc, 'f3 dc', tname,.001,3) && pass; + pass = vcompare(o_f4_dc, t_f4_dc, 'f4 dc', tname,.001,4) && pass; + pass = vcompare(o_f3_int, t_f3_int, 'f3 int', tname,.001,8) && pass; + pass = vcompare(o_f4_int, t_f4_int, 'f4 int', tname,.001,7) && pass; + % pass = vcompare(o_f3, t_f3, 'f3', tname,.001,15) && pass; + % pass = vcompare(o_f4, t_f4, 'f4', tname,.001,16) && pass; + end + + + pass = vcompare(o_f1_dc, t_f1_dc, 'f1 dc', tname,.001,1) && pass; + pass = vcompare(o_f2_dc, t_f2_dc, 'f2 dc', tname,.001,2) && pass; + pass = vcompare(o_f2_int, t_f2_int, 'f2 int', tname,.001,6) && pass; + pass = vcompare(o_f1_int, t_f1_int, 'f1 int', tname,.001,5) && pass; + %pass = vcompare(o_f1, t_f1, 'f1', tname,.001,15) && pass; + %pass = vcompare(o_f2, t_f2, 'f2', tname,.001,16) && pass; + + % Much larger tolerances on unimportant statistics + %pass = vcompare(o_EbNodB, t_EbNodB, 'EbNodB', tname,.05,12) && pass; + pass = vcompare(o_ppm , t_ppm, 'ppm', tname,.02,11) && pass; + assert(pass); diffpass = sum(xor(obits,bits'))<4; diffbits = sum(xor(obits,bits')); @@ -211,17 +255,25 @@ function test_stats = fsk_demod_xt(Fs,Rs,f1,f2,mod,tname) endfunction -function [dmod,cmod,omod,pass] = fsk_mod_test(Fs,Rs,f1,f2,bits,tname) +function [dmod,cmod,omod,pass] = fsk_mod_test(Fs,Rs,f1,fsp,bits,tname,M=2) global mod_pass_fail_maxdiff; %Run the C modulator - cmod = fsk_mod_c(Fs,Rs,f1,f2,bits); + cmod = fsk_mod_c(Fs,Rs,f1,fsp,bits,M); %Set up and run the octave modulator - states = fsk_horus_init(Fs,Rs); - states.f1_tx = f1; - states.f2_tx = f2; - states.dA = 1; + states.M = M; + states = fsk_horus_init(Fs,Rs,M); + + states.ftx(1) = f1; + states.ftx(2) = f1+fsp; + + if states.M == 4 + states.ftx(3) = f1+fsp*2; + states.ftx(4) = f1+fsp*3; + end + + states.dA = [1 1 1 1]; states.dF = 0; - omod = fsk_horus_mod(states,bits'); + omod = fsk_horus_mod(states,bits); dmod = cmod-omod; pass = max(dmod)<(mod_pass_fail_maxdiff*length(dmod)); @@ -246,11 +298,27 @@ function pass = test_mod_horuscfg_randbits endfunction +% Random bit modulator test +% Pass random bits through the modulators and compare +function pass = test_mod_horuscfgm4_randbits + rand('state',1); + randn('state',1); + bits = rand(1,10000)>.5; + [dmod,cmod,omod,pass] = fsk_mod_test(8000,100,1200,1600,bits,"mod horuscfg randbits",4); + + if(!pass) + figure(1) + plot(dmod) + title("Difference between octave and C mod impl"); + end + +endfunction + % A big ol' channel impairment tester % Shamlessly taken from fsk_horus % This throws some channel imparment or another at the C and octave modem so they % may be compared. -function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA) +function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA,M=2) frames = 60; %EbNodB = 10; %timing_offset = 2.0; % see resample() for clock offset below @@ -272,7 +340,7 @@ function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA) if test_frame_mode == 2 % horus rtty config --------------------- - states = fsk_horus_init(8000, 100); + states = fsk_horus_init(8000, 100, M); states.f1_tx = 1200; states.f2_tx = 1600; @@ -280,7 +348,7 @@ function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA) if test_frame_mode == 4 % horus rtty config --------------------- - states = fsk_horus_init(8000, 100); + states = fsk_horus_init(8000, 100, M); states.f1_tx = 1200; states.f2_tx = 1600; states.tx_bits_file = "horus_tx_bits_rtty.txt"; % Octave file of bits we FSK modulate @@ -289,7 +357,7 @@ function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA) if test_frame_mode == 5 % horus binary config --------------------- - states = fsk_horus_init(8000, 100); + states = fsk_horus_init(8000, 100, M); states.f1_tx = 1200; states.f2_tx = 1600; %%%states.tx_bits_file = "horus_tx_bits_binary.txt"; % Octave file of bits we FSK modulate @@ -305,10 +373,11 @@ function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA) nsym = states.nsym; Fs = states.Fs; states.df = df; - states.dA = dA; + states.dA = [dA dA dA dA]; + states.M = M; EbNo = 10^(EbNodB/10); - variance = states.Fs/(states.Rs*EbNo); + variance = states.Fs/(states.Rs*EbNo*states.bitspersymbol); % set up tx signal with payload bits based on test mode @@ -342,6 +411,17 @@ function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA) tx_bits = [tx_bits test_frame]; end end + + f1 = states.f1_tx; + fsp = states.f2_tx-f1 + states.dA = [dA dA dA dA]; + states.ftx(1) = f1; + states.ftx(2) = f1+fsp; + + if states.M == 4 + states.ftx(3) = f1+fsp*2; + states.ftx(4) = f1+fsp*3; + end tx = fsk_horus_mod(states, tx_bits); @@ -357,8 +437,8 @@ function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA) noise = sqrt(variance)*randn(length(tx),1); rx = tx + noise; - test_name = sprintf("tfsk_run_sim EbNodB:%d frames:%d timing_offset:%d fading:%d df:%d",EbNodB,frames,timing_offset,fading,df); - tstats = fsk_demod_xt(Fs,Rs,states.f1_tx,states.f2_tx,rx,test_name); + test_name = sprintf("tfsk run sim EbNodB:%d frames:%d timing_offset:%d fading:%d df:%d",EbNodB,frames,timing_offset,fading,df); + tstats = fsk_demod_xt(Fs,Rs,states.f1_tx,fsp,rx,test_name,M); printf("Test %s done\n",test_name); pass = tstats.pass; @@ -419,9 +499,9 @@ function stats = tfsk_run_sim(test_frame_mode,EbNodB,timing_offset,fading,df,dA) endfunction -function pass = ebno_battery_test(timing_offset,fading,df,dA) +function pass = ebno_battery_test(timing_offset,fading,df,dA,M) %Range of EbNodB over which to test - ebnodbrange = (5:13); + ebnodbrange = fliplr(5:13); ebnodbs = length(ebnodbrange); mode = 5; @@ -431,9 +511,9 @@ function pass = ebno_battery_test(timing_offset,fading,df,dA) fadingv = repmat(fading,1,ebnodbs); dfv = repmat(df,1,ebnodbs); dav = repmat(dA,1,ebnodbs); - - statv = pararrayfun(floor(1.25*nproc()),@tfsk_run_sim,modev,ebnodbrange,timingv,fadingv,dfv,dav); - %statv = arrayfun(@tfsk_run_sim,modev,ebnodbrange,timingv,fadingv,dfv,dav); + mv = repmat(M,1,ebnodbs); + %statv = pararrayfun(floor(1.25*nproc()),@tfsk_run_sim,modev,ebnodbrange,timingv,fadingv,dfv,dav,mv); + statv = arrayfun(@tfsk_run_sim,modev,ebnodbrange,timingv,fadingv,dfv,dav,mv); passv = zeros(1,length(statv)); for ii=(1:length(statv)) @@ -449,41 +529,44 @@ function pass = ebno_battery_test(timing_offset,fading,df,dA) endfunction %Test with and without channel fading -function pass = test_fading_var(timing_offset,df,dA) - pass = ebno_battery_test(timing_offset,1,df,dA) +function pass = test_fading_var(timing_offset,df,dA,M) + pass = ebno_battery_test(timing_offset,1,df,dA,M) assert(pass) - pass = pass && ebno_battery_test(timing_offset,0,df,dA) + pass = pass && ebno_battery_test(timing_offset,0,df,dA,M) assert(pass) endfunction %Test with and without sample clock offset -function pass = test_timing_var(df,dA) - pass = test_fading_var(1,df,dA) +function pass = test_timing_var(df,dA,M) + pass = test_fading_var(1,df,dA,M) assert(pass) - pass = pass && test_fading_var(0,df,dA) + pass = pass && test_fading_var(0,df,dA,M) assert(pass) endfunction %Test with and without 1 Hz/S freq drift -function pass = test_drift_var() - pass = test_timing_var(1,1) +function pass = test_drift_var(M) + pass = test_timing_var(1,1,M) assert(pass) - pass = pass && test_timing_var(0,1) + pass = pass && test_timing_var(0,1,M) assert(pass) endfunction function pass = test_fsk_battery() - pass = test_mod_horuscfg_randbits + pass = test_mod_horuscfg_randbits; assert(pass) - pass = pass && test_drift_var(); + pass = pass && test_mod_horuscfgm4_randbits; + assert(pass) + pass = pass && test_drift_var(2); + assert(pass) + pass = pass && test_drift_var(4); assert(pass) - if pass printf("***** All tests passed! *****\n"); end endfunction -function plot_fsk_bers() +function plot_fsk_bers(M=2) %Range of EbNodB over which to plot ebnodbrange = (5:13); @@ -498,7 +581,9 @@ function plot_fsk_bers() fadingv = repmat(0,1,ebnodbs); dfv = repmat(0,1,ebnodbs); dav = repmat(1,1,ebnodbs); - statv = pararrayfun(floor(1.25*nproc()),@tfsk_run_sim,modev,ebnodbrange,timingv,fadingv,dfv,dav); + Mv = repmat(M,1,ebnodbs); + %statv = pararrayfun(floor(1.25*nproc()),@tfsk_run_sim,modev,ebnodbrange,timingv,fadingv,dfv,dav,Mv); + statv = arrayfun(@tfsk_run_sim,modev,ebnodbrange,timingv,fadingv,dfv,dav,Mv); for ii = (1:length(statv)) stat = statv(ii); @@ -508,7 +593,6 @@ function plot_fsk_bers() end close all - figure(2); clf; semilogy(ebnodbrange, berinc,'r;2FSK non-coherent theory;') hold on; @@ -523,5 +607,5 @@ function plot_fsk_bers() endfunction -plot_fsk_bers -test_fsk_battery +%plot_fsk_bers +%test_fsk_battery diff --git a/codec2-dev/src/fsk.c b/codec2-dev/src/fsk.c index f4a02c90..50ae00ed 100644 --- a/codec2-dev/src/fsk.c +++ b/codec2-dev/src/fsk.c @@ -49,7 +49,7 @@ #include #include -#include "fsk.h" +#include "fsk4.h" #include "comp_prim.h" #include "kiss_fftr.h" #include "modem_probe.h" @@ -60,6 +60,57 @@ \*---------------------------------------------------------------------------*/ +/* + * Euler's formula in a new convenient function + */ +static inline COMP comp_exp_j(float phi){ + COMP res; + res.real = cosf(phi); + res.imag = sinf(phi); + return res; +} + +/* + * Quick and easy complex 0 + */ +static inline COMP comp0(){ + COMP res; + res.real = 0; + res.imag = 0; + return res; +} + +/* + * Quick and easy complex subtract + */ +static inline COMP csub(COMP a, COMP b){ + COMP res; + res.real = a.real-b.real; + res.imag = a.imag-b.imag; + return res; +} + +/* + * Compare the magnitude of a and b. if |a|>|b|, return true, otw false. + * This needs no square roots + */ +static inline int comp_mag_gt(COMP a,COMP b){ + return ((a.real*a.real)+(a.imag*a.imag)) > ((b.real*b.real)+(b.imag*b.imag)); +} + +/* + * Normalize a complex number's magnitude to 1 + */ +static inline COMP comp_normalize(COMP a){ + COMP b; + float av = sqrtf((a.real*a.real)+(a.imag*a.imag)); + b.real = a.real/av; + b.imag = a.imag/av; + return b; +} + + + /*---------------------------------------------------------------------------*\ FUNCTION....: fsk_create @@ -72,7 +123,7 @@ \*---------------------------------------------------------------------------*/ -struct FSK * fsk_create(int Fs, int Rs, int tx_f1,int tx_f2) +struct FSK * fsk_create(int Fs, int Rs,int M, int tx_f1, int tx_fs) { struct FSK *fsk; int i; @@ -83,12 +134,13 @@ struct FSK * fsk_create(int Fs, int Rs, int tx_f1,int tx_f2) assert(Fs > 0 ); assert(Rs > 0 ); assert(tx_f1 > 0); - assert(tx_f2 > 0); + assert(tx_fs > 0); assert(ct_P > 0); /* Ts (Fs/Rs) must be an integer */ assert( (Fs%Rs) == 0 ); /* Ts/P (Fs/Rs/P) must be an integer */ assert( ((Fs/Rs)%ct_P) == 0 ); + assert( M==2 || M==4); fsk = (struct FSK*) malloc(sizeof(struct FSK)); if(fsk == NULL) return NULL; @@ -98,7 +150,10 @@ struct FSK * fsk_create(int Fs, int Rs, int tx_f1,int tx_f2) for(i=1; i; i<<=1) if(Fs&i) Ndft = i<<1; + + Ndft = 1024; + //Ndft = 4096; /* Set constant config parameters */ fsk->Fs = Fs; fsk->Rs = Rs; @@ -109,14 +164,16 @@ struct FSK * fsk_create(int Fs, int Rs, int tx_f1,int tx_f2) fsk->Ndft = Ndft; fsk->Nmem = fsk->N+(2*fsk->Ts); fsk->f1_tx = tx_f1; - fsk->f2_tx = tx_f2; + fsk->fs_tx = tx_fs; fsk->nin = fsk->N; + fsk->mode = M==2 ? MODE_2FSK : MODE_4FSK; + fsk->Nbits = M==2 ? fsk->Nsym : fsk->Nsym*2; /* Set up rx state */ - fsk->phi1_c.real = 1; - fsk->phi1_c.imag = 0; - fsk->phi2_c.real = 1; - fsk->phi2_c.imag = 0; + fsk->phi1_c = comp_exp_j(0); + fsk->phi2_c = comp_exp_j(0); + fsk->phi3_c = comp_exp_j(0); + fsk->phi4_c = comp_exp_j(0); memold = (4*fsk->Ts); @@ -127,7 +184,7 @@ struct FSK * fsk_create(int Fs, int Rs, int tx_f1,int tx_f2) return NULL; } - for(i=0;isamp_old[i]=0; + for(int i=0;isamp_old[i]=0; fsk->fft_cfg = kiss_fftr_alloc(Ndft,0,NULL,NULL); if(fsk->fft_cfg == NULL){ @@ -136,19 +193,29 @@ struct FSK * fsk_create(int Fs, int Rs, int tx_f1,int tx_f2) return NULL; } + fsk->fft_est = (float*)malloc(sizeof(float)*fsk->Ndft/2); + if(fsk->fft_est == NULL){ + free(fsk->samp_old); + free(fsk->fft_cfg); + free(fsk); + return NULL; + } + + for(int i=0;ifft_est[i] = 0; + fsk->norm_rx_timing = 0; /* Set up tx state */ - fsk->tx_phase_c.imag = 0; - fsk->tx_phase_c.real = 1; + fsk->tx_phase_c = comp_exp_j(0); /* Set up demod stats */ fsk->EbNodB = 0; fsk->f1_est = 0; fsk->f2_est = 0; - fsk->twist_est = 0; + fsk->f3_est = 0; + fsk->f4_est = 0; fsk->ppm = 0; - + return fsk; } @@ -162,6 +229,10 @@ void fsk_destroy(struct FSK *fsk){ free(fsk); } +#define FEST_MIN 800 +#define FEST_MAX 2500 +#define FEST_MIN_SPACING 200 + /* * Internal function to estimate the frequencies of the two tones within a block of samples. * This is split off because it is fairly complicated, needs a bunch of memory, and probably @@ -169,11 +240,10 @@ void fsk_destroy(struct FSK *fsk){ * Parameters: * fsk - FSK struct from demod containing FSK config * fsk_in - block of samples in this demod cycles, must be nin long - * f1_est - pointer to f1 estimate variable in demod - * f2_est - pointer to f2 estimate variable in demod - * twist - pointer to twist estimate in demod + * freqs - Array for the estimated frequencies + * M - number of frequency peaks to find */ -void fsk_demod_freq_est(struct FSK *fsk, float fsk_in[],float *f1_est,float *f2_est,float *twist){ +void fsk_demod_freq_est(struct FSK *fsk, float fsk_in[],float *freqs,int M){ int Ndft = fsk->Ndft; int Fs = fsk->Fs; int nin = fsk->nin; @@ -181,136 +251,368 @@ void fsk_demod_freq_est(struct FSK *fsk, float fsk_in[],float *f1_est,float *f2_ int fft_samps; float hann; float max; - int m1,m2; - float m1v,m2v,t; + int imax; kiss_fftr_cfg fft_cfg = fsk->fft_cfg; + int freqi[M]; + int f_min,f_max,f_zero; /* Array to do complex FFT from using kiss_fft */ /* It'd probably make more sense here to use kiss_fftr */ kiss_fft_scalar *fftin = (kiss_fft_scalar*)alloca(sizeof(kiss_fft_scalar)*Ndft); kiss_fft_cpx *fftout = (kiss_fft_cpx*)alloca(sizeof(kiss_fft_cpx)*(Ndft/2)+1); - fft_samps = nin Ndft, the end of the hann window may be cut off */ - /* resulting in a dirty FFT */ - /* An easy fix would be to ensure that Ndft is always greater than N+Ts/2 - * instead of N */ - /* Another easy fix would be to apply the hann window over fft_samps - * instead of nin */ - /* This bug isn't a big deal and can't show up in the balloon config */ - /* as 8192 > 8040 */ - hann = sinf((M_PI*(float)i)/((float)nin-1)); - - fftin[i] = (kiss_fft_scalar)hann*hann*fsk_in[i]; - } - /* Zero out the remaining slots */ - for(; ifft_est[i] = fsk->fft_est[i]*.95 + sqrtf(fftout[i].r)*.05; + fftout[i].i = fsk->fft_est[i]; + } + } + + modem_probe_samp_f("t_fft_est",fsk->fft_est,Ndft/2); - /* Do the FFT */ - kiss_fftr(fft_cfg,fftin,fftout); + max = 0; + /* Find the M frequency peaks here */ + for(i=0; i max){ + max = fftout[j].i; + imax = j; + } + } + /* Blank out FMax +/-Fspace/2 */ + 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 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; iN; + 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; + int i,j,dc_i,cbuf_i; + float ft1,ft2; + int nstash = fsk->nstash; + COMP *f1_int, *f2_int; + COMP t1,t2; + COMP phi1_c = fsk->phi1_c; + COMP phi2_c = fsk->phi2_c; + COMP phi_ft; + int nold = Nmem-nin; + COMP dphi1,dphi2; + COMP dphift; + float rx_timing,norm_rx_timing,old_norm_rx_timing,d_norm_rx_timing,appm; + int using_old_samps; + float *sample_src; + COMP *f1_intbuf,*f2_intbuf; + float f_est[M]; + float meanebno,stdebno; + + /* Estimate tone frequencies */ + fsk_demod_freq_est(fsk,fsk_in,f_est,M); + modem_probe_samp_f("t_f_est",f_est,M); + + /* allocate memory for the integrated samples */ + /* Note: This must be kept after fsk_demod_freq_est for memory usage reasons */ + f1_int = (COMP*) alloca(sizeof(COMP)*(nsym+1)*P); + f2_int = (COMP*) alloca(sizeof(COMP)*(nsym+1)*P); + + /* Allocate circular buffers for integration */ + f1_intbuf = (COMP*) alloca(sizeof(COMP)*Ts); + f2_intbuf = (COMP*) alloca(sizeof(COMP)*Ts); - /* Find the magnitude^2 of each freq slot and stash away in the real - * value, so this only has to be done once. Since we're only comparing - * these values and only need the mag of 2 points, we don't need to do - * a sqrt to each value */ - for(i=0; if1_est<1){ + fsk->f1_est = f_est[0]; + fsk->f2_est = f_est[1]; + printf("using fest\n"); + } + + /* Figure out how much to nudge each sample downmixer for every sample */ + dphi1 = comp_exp_j(-2*M_PI*((fsk->f1_est)/(float)(Fs))); + dphi2 = comp_exp_j(-2*M_PI*((fsk->f2_est)/(float)(Fs))); + + //dphi1 = comp_exp_j(-2*M_PI*((float)(f_est[0])/(float)(Fs))); + //dphi2 = comp_exp_j(-2*M_PI*((float)(f_est[1])/(float)(Fs))); + + dc_i = 0; + cbuf_i = 0; + sample_src = &(fsk->samp_old[nstash-nold]); + using_old_samps = 1; + /* Pre-fill integration buffer */ + for(dc_i=0; dc_i=nold && using_old_samps){ + sample_src = &fsk_in[0]; + dc_i = 0; + using_old_samps = 0; + + /* Recalculate delta-phi after switching to new sample source */ + phi1_c = comp_normalize(phi1_c); + phi2_c = comp_normalize(phi2_c); + dphi1 = comp_exp_j(-2*M_PI*(f_est[0]/(float)(Fs))); + dphi2 = comp_exp_j(-2*M_PI*(f_est[1]/(float)(Fs))); + } + /* Downconvert and place into integration buffer */ + f1_intbuf[dc_i]=fcmult(sample_src[dc_i],phi1_c); + f2_intbuf[dc_i]=fcmult(sample_src[dc_i],phi2_c); + + modem_probe_samp_c("t_f1_dc",&f1_intbuf[dc_i],1); + modem_probe_samp_c("t_f2_dc",&f2_intbuf[dc_i],1); + /* Spin downconversion phases */ + phi1_c = cmult(phi1_c,dphi1); + phi2_c = cmult(phi2_c,dphi2); } + cbuf_i = dc_i; - /* Find the maximum */ - max = 0; - m1 = 0; - for(i=0; i max){ - max = fftout[i].r; - m1 = i; + /* Integrate over Ts at offsets of Ts/P */ + for(i=0; i<(nsym+1)*P; i++){ + /* Downconvert and Place Ts/P samples in the integration buffers */ + for(j=0; j<(Ts/P); j++,dc_i++){ + /* Switch sample source to new samples when we run out of old ones */ + if(dc_i>=nold && using_old_samps){ + sample_src = &fsk_in[0]; + dc_i = 0; + using_old_samps = 0; + + /* Recalculate delta-phi after switching to new sample source */ + phi1_c = comp_normalize(phi1_c); + phi2_c = comp_normalize(phi2_c); + dphi1 = comp_exp_j(-2*M_PI*((f_est[0])/(float)(Fs))); + dphi2 = comp_exp_j(-2*M_PI*((f_est[1])/(float)(Fs))); + } + /* Downconvert and place into integration buffer */ + f1_intbuf[cbuf_i+j]=fcmult(sample_src[dc_i],phi1_c); + f2_intbuf[cbuf_i+j]=fcmult(sample_src[dc_i],phi2_c); + + modem_probe_samp_c("t_f1_dc",&f1_intbuf[cbuf_i+j],1); + modem_probe_samp_c("t_f2_dc",&f2_intbuf[cbuf_i+j],1); + /* Spin downconversion phases */ + phi1_c = cmult(phi1_c,dphi1); + phi2_c = cmult(phi2_c,dphi2); + } + + /* Dump internal samples */ + + cbuf_i += Ts/P; + if(cbuf_i>=Ts) cbuf_i = 0; + + /* Integrate over the integration buffers, save samples */ + t1 = t2 = comp0(); + for(j=0; jphi1_c = phi1_c; + fsk->phi2_c = phi2_c; + + fsk->f1_est = f_est[0]; + fsk->f2_est = f_est[1]; + + /* Stash samples away in the old sample buffer for the next round of bit getting */ + memcpy((void*)&(fsk->samp_old[0]),(void*)&(fsk_in[nin-nstash]),sizeof(float)*nstash); - m1v = sqrtf(fftout[m1].r); + /* Fine Timing Estimation */ + /* Apply magic nonlinearity to f1_int and f2_int, shift down to 0, + * exract 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; + t1=comp0(); + for(i=0; i<(nsym+1)*P; i++){ + /* Get abs^2 of fx_int[i], and add 'em */ + ft1 = (f1_int[i].real*f1_int[i].real) + (f1_int[i].imag*f1_int[i].imag); + ft1 += (f2_int[i].real*f2_int[i].real) + (f2_int[i].imag*f2_int[i].imag); + + /* Down shift and accumulate magic line */ + t1 = cadd(t1,fcmult(ft1,phi_ft)); + + /* Spin the oscillator for the magic line shift */ + phi_ft = cmult(phi_ft,dphift); + } + /* Get the magic angle */ + norm_rx_timing = -atan2f(t1.imag,t1.real)/(2*M_PI); + rx_timing = norm_rx_timing*(float)P; - /* Zero out 100Hz around the maximum */ - i = m1 - 100*Ndft/Fs; - i = i<0 ? 0 : i; - j = m1 + 100*Ndft/Fs; - j = j>Ndft/2 ? Ndft/2 : j; + old_norm_rx_timing = fsk->norm_rx_timing; + fsk->norm_rx_timing = norm_rx_timing; - for(;i max){ - max = fftout[i].r; - m2 = i; - } + /* 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; } - m2v = sqrtf(fftout[m2].r); + /* Figure out how many samples are needed the next modem cycle */ + 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);; - /* f1 is always the lower tone */ - if(m1>m2){ - j=m1; - m1=m2; - m2=j; - t=m1v; - m1v=m2v; - m2v=t; + /* 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[2]; + + #ifdef EST_EBNO + meanebno = 0; + stdebno = 0; + #endif + + /* FINALLY, THE BITS */ + /* also, resample fx_int */ + for(i=0; itmax[0])?1:0; + + /* Accumulate resampled int magnitude for EbNodB estimation */ + /* Standard deviation is calculated by algorithm devised by crafty soviets */ + #ifdef EST_EBNO + + ft1 = sqrtf(t1.real*t1.real + t1.imag*t1.imag); + ft2 = sqrtf(t2.real*t2.real + t2.imag*t2.imag); + ft1 = fabsf(ft1-ft2); + meanebno += ft1; + + #endif + /* Soft output goes here */ } - *f1_est = (float)m1*(float)Fs/(float)Ndft; - *f2_est = (float)m2*(float)Fs/(float)Ndft; - *twist = 20*log10f(m1v/m2v); -} - -/* - * Euler's formula in a new convenient function - */ -static inline COMP comp_exp_j(float phi){ - COMP res; - res.real = cosf(phi); - res.imag = sinf(phi); - return res; -} - -/* - * Quick and easy complex 0 - */ -static inline COMP comp0(){ - COMP res; - res.real = 0; - res.imag = 0; - return res; -} - -/* - * Compare the magnitude of a and b. if |a|>|b|, return true, otw false. - * This needs no square roots - */ -static inline int comp_mag_gt(COMP a,COMP b){ - return ((a.real*a.real)+(a.imag*a.imag)) > ((b.real*b.real)+(b.imag*b.imag)); -} - -/* - * Normalize a complex number's magnitude to 1 - */ -static inline COMP comp_normalize(COMP a){ - COMP b; - float av = sqrtf((a.real*a.real)+(a.imag*a.imag)); - b.real = a.real/av; - b.imag = a.imag/av; - return b; + #ifdef EST_EBNO + /* Calculate mean for EbNodB estimation */ + meanebno = meanebno/(float)nsym; + stdebno = 0; + /* Go back through the data and figure the std dev */ + for(i=0; iEbNodB = 20*log10f((1e-6+meanebno)/(1e-6+stdebno)); + #else + fsk->EbNodB = 1; + #endif + + /* 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_f1",&f_est[0],1); + modem_probe_samp_f("t_f2",&f_est[1],1); + modem_probe_samp_c("t_f1_int",f1_int,(nsym+1)*P); + modem_probe_samp_c("t_f2_int",f2_int,(nsym+1)*P); + modem_probe_samp_f("t_rx_timing",&(rx_timing),1);; } - -void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ +void fsk4_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ int N = fsk->N; int Ts = fsk->Ts; int Rs = fsk->Rs; @@ -319,50 +621,57 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ int nin = fsk->nin; int P = fsk->P; int Nmem = fsk->Nmem; + int M = fsk->mode; int i,j,dc_i,cbuf_i; float ft1,ft2; - float twist; int nstash = fsk->nstash; - COMP *f1_int, *f2_int; - COMP t1,t2; + COMP *f1_int, *f2_int, *f3_int, *f4_int; + COMP t1,t2,t3,t4; COMP phi1_c = fsk->phi1_c; COMP phi2_c = fsk->phi2_c; + COMP phi3_c = fsk->phi3_c; + COMP phi4_c = fsk->phi4_c; COMP phi_ft; int nold = Nmem-nin; - COMP dphi1,dphi2; + COMP dphi1,dphi2,dphi3,dphi4; COMP dphift; - float f1,f2; float rx_timing,norm_rx_timing,old_norm_rx_timing,d_norm_rx_timing,appm; int using_old_samps; float *sample_src; - COMP *f1_intbuf,*f2_intbuf; - + COMP *f1_intbuf,*f2_intbuf,*f3_intbuf,*f4_intbuf; + float f_est[M]; float meanebno,stdebno; /* Estimate tone frequencies */ - fsk_demod_freq_est(fsk,fsk_in,&f1,&f2,&twist); + fsk_demod_freq_est(fsk,fsk_in,f_est,M); + modem_probe_samp_f("t_f_est",f_est,M); /* allocate memory for the integrated samples */ /* Note: This must be kept after fsk_demod_freq_est for memory usage reasons */ f1_int = (COMP*) alloca(sizeof(COMP)*(nsym+1)*P); f2_int = (COMP*) alloca(sizeof(COMP)*(nsym+1)*P); + f3_int = (COMP*) alloca(sizeof(COMP)*(nsym+1)*P); + f4_int = (COMP*) alloca(sizeof(COMP)*(nsym+1)*P); /* Allocate circular buffers for integration */ f1_intbuf = (COMP*) alloca(sizeof(COMP)*Ts); f2_intbuf = (COMP*) alloca(sizeof(COMP)*Ts); + f3_intbuf = (COMP*) alloca(sizeof(COMP)*Ts); + f4_intbuf = (COMP*) alloca(sizeof(COMP)*Ts); - /* If this is the first run and we haven't already estimated freq, save the new est */ - if(fsk->f1_est<1 || fsk->f2_est<1){ - fsk->f1_est = f1; - fsk->f2_est = f2; - fsk->twist_est = twist; - } + /* If this is the first run, we won't have any valid f_est */ + if(fsk->f1_est<1){ + fsk->f1_est = f_est[0]; + fsk->f2_est = f_est[1]; + fsk->f3_est = f_est[2]; + fsk->f4_est = f_est[3]; + } /* Figure out how much to nudge each sample downmixer for every sample */ - /* Use old freq. estimate here so that old samples will be converted at old - * frequency, to match behaviour of fsk_horus */ - dphi1 = comp_exp_j(-2*M_PI*((float)(fsk->f1_est)/(float)(Fs))); - dphi2 = comp_exp_j(-2*M_PI*((float)(fsk->f2_est)/(float)(Fs))); + dphi1 = comp_exp_j(-2*M_PI*((fsk->f1_est)/(float)(Fs))); + dphi2 = comp_exp_j(-2*M_PI*((fsk->f2_est)/(float)(Fs))); + dphi3 = comp_exp_j(-2*M_PI*((fsk->f3_est)/(float)(Fs))); + dphi4 = comp_exp_j(-2*M_PI*((fsk->f4_est)/(float)(Fs))); dc_i = 0; cbuf_i = 0; @@ -378,20 +687,30 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ using_old_samps = 0; /* Recalculate delta-phi after switching to new sample source */ - phi1_c = comp_normalize(phi1_c); - phi2_c = comp_normalize(phi2_c); - dphi1 = comp_exp_j(-2*M_PI*((float)(f1)/(float)(Fs))); - dphi2 = comp_exp_j(-2*M_PI*((float)(f2)/(float)(Fs))); + //phi1_c = comp_normalize(phi1_c); + //phi2_c = comp_normalize(phi2_c); + //phi3_c = comp_normalize(phi3_c); + //phi4_c = comp_normalize(phi4_c); + dphi1 = comp_exp_j(-2*M_PI*((f_est[0])/(float)(Fs))); + dphi2 = comp_exp_j(-2*M_PI*((f_est[1])/(float)(Fs))); + dphi3 = comp_exp_j(-2*M_PI*((f_est[2])/(float)(Fs))); + dphi4 = comp_exp_j(-2*M_PI*((f_est[3])/(float)(Fs))); } /* Downconvert and place into integration buffer */ f1_intbuf[dc_i]=fcmult(sample_src[dc_i],phi1_c); f2_intbuf[dc_i]=fcmult(sample_src[dc_i],phi2_c); + f3_intbuf[dc_i]=fcmult(sample_src[dc_i],phi3_c); + f4_intbuf[dc_i]=fcmult(sample_src[dc_i],phi4_c); modem_probe_samp_c("t_f1_dc",&f1_intbuf[dc_i],1); modem_probe_samp_c("t_f2_dc",&f2_intbuf[dc_i],1); + modem_probe_samp_c("t_f3_dc",&f3_intbuf[dc_i],1); + modem_probe_samp_c("t_f4_dc",&f4_intbuf[dc_i],1); /* Spin downconversion phases */ phi1_c = cmult(phi1_c,dphi1); phi2_c = cmult(phi2_c,dphi2); + phi3_c = cmult(phi3_c,dphi3); + phi4_c = cmult(phi4_c,dphi4); } cbuf_i = dc_i; @@ -405,23 +724,31 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ dc_i = 0; using_old_samps = 0; - /* Recalculate delta-phi after switching to new sample source */ - phi1_c = comp_normalize(phi1_c); - phi2_c = comp_normalize(phi2_c); - dphi1 = comp_exp_j(-2*M_PI*((float)(f1)/(float)(Fs))); - dphi2 = comp_exp_j(-2*M_PI*((float)(f2)/(float)(Fs))); + /* Recalculate delta-phi after switching to new sample source */ + //phi1_c = comp_normalize(phi1_c); + //phi2_c = comp_normalize(phi2_c); + //phi3_c = comp_normalize(phi3_c); + //phi4_c = comp_normalize(phi4_c); + dphi1 = comp_exp_j(-2*M_PI*((f_est[0])/(float)(Fs))); + dphi2 = comp_exp_j(-2*M_PI*((f_est[1])/(float)(Fs))); + dphi3 = comp_exp_j(-2*M_PI*((f_est[2])/(float)(Fs))); + dphi4 = comp_exp_j(-2*M_PI*((f_est[3])/(float)(Fs))); } /* Downconvert and place into integration buffer */ f1_intbuf[cbuf_i+j]=fcmult(sample_src[dc_i],phi1_c); f2_intbuf[cbuf_i+j]=fcmult(sample_src[dc_i],phi2_c); - - + f3_intbuf[cbuf_i+j]=fcmult(sample_src[dc_i],phi3_c); + f4_intbuf[cbuf_i+j]=fcmult(sample_src[dc_i],phi4_c); + modem_probe_samp_c("t_f1_dc",&f1_intbuf[cbuf_i+j],1); modem_probe_samp_c("t_f2_dc",&f2_intbuf[cbuf_i+j],1); - + modem_probe_samp_c("t_f3_dc",&f3_intbuf[cbuf_i+j],1); + modem_probe_samp_c("t_f4_dc",&f4_intbuf[cbuf_i+j],1); /* Spin downconversion phases */ phi1_c = cmult(phi1_c,dphi1); phi2_c = cmult(phi2_c,dphi2); + phi3_c = cmult(phi3_c,dphi3); + phi4_c = cmult(phi4_c,dphi4); } @@ -429,24 +756,34 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ cbuf_i += Ts/P; if(cbuf_i>=Ts) cbuf_i = 0; - /* Integrate over the integration buffers, save samples */ - t1 = t2 = comp0(); + t1 = t2 = t3 = t4 = comp0(); for(j=0; jphi1_c = phi1_c; - fsk->phi2_c = phi2_c; - - fsk->f1_est = f1; - fsk->f2_est = f2; - fsk->twist_est = twist; + fsk->phi2_c = phi2_c; + fsk->phi3_c = phi3_c; + fsk->phi4_c = phi4_c; + + fsk->f1_est = f_est[0]; + fsk->f2_est = f_est[1]; + fsk->f3_est = f_est[2]; + fsk->f4_est = f_est[3]; /* Stash samples away in the old sample buffer for the next round of bit getting */ memcpy((void*)&(fsk->samp_old[0]),(void*)&(fsk_in[nin-nstash]),sizeof(float)*nstash); @@ -460,20 +797,39 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ phi_ft.real = 1; phi_ft.imag = 0; t1=comp0(); + COMP c,y,t; + c = comp0(); + t2 = comp0(); for(i=0; i<(nsym+1)*P; i++){ - /* Get abs of f1_int[i] and f2_int[i] */ - ft1 = sqrtf( (f1_int[i].real*f1_int[i].real) + (f1_int[i].imag*f1_int[i].imag) ); - ft2 = sqrtf( (f2_int[i].real*f2_int[i].real) + (f2_int[i].imag*f2_int[i].imag) ); + /* Get abs^2 of fx_int[i], and add 'em */ + ft1 = (f1_int[i].real*f1_int[i].real) + (f1_int[i].imag*f1_int[i].imag); + ft1 += (f2_int[i].real*f2_int[i].real) + (f2_int[i].imag*f2_int[i].imag); + ft1 += (f3_int[i].real*f3_int[i].real) + (f3_int[i].imag*f3_int[i].imag); + ft1 += (f4_int[i].real*f4_int[i].real) + (f4_int[i].imag*f4_int[i].imag); - /* Add and square 'em */ - ft1 = ft1-ft2; - ft1 = ft1*ft1; /* Down shift and accumulate magic line */ - t1 = cadd(t1,fcmult(ft1,phi_ft)); + t1 = fcmult(ft1,phi_ft); + y.real = t1.real-c.real; + y.imag = t1.imag-c.imag; + + t.real = t2.real + y.real; + t.imag = t2.imag + y.imag; + + c.real = (t.real-t2.real) - y.real; + c.imag = (t.imag-t2.imag) - y.imag; + + //t2 = y; + t2 = cadd(t2,t1); + + /*y = csub(t1,c); + t = cadd(t2,y); + c = csub(csub(t,t2),y); + t2 = y;*/ /* Spin the oscillator for the magic line shift */ phi_ft = cmult(phi_ft,dphift); } + t1 = t2; /* Get the magic angle */ norm_rx_timing = -atan2f(t1.imag,t1.real)/(2*M_PI); rx_timing = norm_rx_timing*(float)P; @@ -504,6 +860,11 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ 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[4]; + float max = 0; + int sym = 0; #ifdef EST_EBNO meanebno = 0; @@ -511,13 +872,37 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ #endif /* FINALLY, THE BITS */ - /* also, resample f1_int,f2_int */ + /* also, resample fx_int */ for(i=0; imax){ + sym = j; + max = tmax[j]; + } + } + + /* Turn into bits */ + rx_bits[(i*2)+1] = (sym&0x1); + rx_bits[(i*2) ] = (sym&0x2)>>1; /* Accumulate resampled int magnitude for EbNodB estimation */ /* Standard deviation is calculated by algorithm devised by crafty soviets */ @@ -529,15 +914,7 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ meanebno += ft1; #endif - - /* THE BIT! */ - rx_bits[i] = comp_mag_gt(t2,t1)?1:0; /* Soft output goes here */ - - /* Log the bit */ - /* We must do some bit monkeying here, as rx_bits is uint8 while samp_i expects an int32 */ - j = rx_bits[i]>0; - modem_probe_samp_i("t_rxbit",&j,1); } #ifdef EST_EBNO @@ -569,47 +946,72 @@ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ /* 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_f1",&f1,1); - modem_probe_samp_f("t_f2",&f2,1); + modem_probe_samp_f("t_f1",&f_est[0],1); + modem_probe_samp_f("t_f2",&f_est[1],1); + modem_probe_samp_f("t_f3",&f_est[2],1); + modem_probe_samp_f("t_f4",&f_est[3],1); modem_probe_samp_c("t_f1_int",f1_int,(nsym+1)*P); modem_probe_samp_c("t_f2_int",f2_int,(nsym+1)*P); + modem_probe_samp_c("t_f3_int",f3_int,(nsym+1)*P); + modem_probe_samp_c("t_f4_int",f4_int,(nsym+1)*P); modem_probe_samp_f("t_rx_timing",&(rx_timing),1);; } + +void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], float fsk_in[]){ + if(fsk->mode == 4){ + fsk4_demod(fsk,rx_bits,fsk_in); + }else{ + fsk2_demod(fsk,rx_bits,fsk_in); + } +} + 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 f2_tx = fsk->f2_tx; /* '1' frequency */ + int fs_tx = fsk->fs_tx; /* space between frequencies */ int Ts = fsk->Ts; /* samples-per-symbol */ int Fs = fsk->Fs; /* sample freq */ - COMP dosc_f1, dosc_f2; /* phase shift per sample */ + COMP dosc_f[4]; /* phase shift per sample */ COMP dph; /* phase shift of current bit */ - int i,j; + int i,j,sym; /* Figure out the amount of phase shift needed per sample */ - dosc_f1 = comp_exp_j(2*M_PI*((float)(f1_tx)/(float)(Fs))); - dosc_f2 = comp_exp_j(2*M_PI*((float)(f2_tx)/(float)(Fs))); + dosc_f[0] = comp_exp_j(2*M_PI*((float)(f1_tx )/(float)(Fs))); + dosc_f[1] = comp_exp_j(2*M_PI*((float)(f1_tx+fs_tx )/(float)(Fs))); - /* Outer loop through bits */ - for(i=0; iNsym; i++){ - /* select current bit phase shift */ - dph = tx_bits[i]==0?dosc_f1:dosc_f2; - - /* Log the bit being modulated */ - j = tx_bits[i]>0; - modem_probe_samp_i("t_txbit",&j,1); - - for(j=0; jmode == 2){ + /* Outer loop through bits */ + for(i=0; iNsym; i++){ + /* select current bit phase shift */ + dph = tx_bits[i]==0?dosc_f[0]:dosc_f[1]; + for(j=0; jNsym; i++){ + /* select current bit phase shift */ + sym = tx_bits[ i*2 ]==0?0:2; + sym+= tx_bits[(i*2)+1]==0?0:1; + dph = dosc_f[sym]; + for(j=0; jN); + /* Normalize TX phase to prevent drift */ + tx_phase_c = comp_normalize(tx_phase_c); /* save TX phase */ - fsk->tx_phase_c = comp_normalize(tx_phase_c); + fsk->tx_phase_c = tx_phase_c; } diff --git a/codec2-dev/src/fsk.h b/codec2-dev/src/fsk.h index 90dcdfd2..bb92970d 100644 --- a/codec2-dev/src/fsk.h +++ b/codec2-dev/src/fsk.h @@ -32,6 +32,9 @@ #include "comp.h" #include "kiss_fftr.h" +#define MODE_2FSK 2 +#define MODE_4FSK 4 + struct FSK { /* Static parameters set up by fsk_init */ int Ndft; /* buffer size for freq offset est fft */ @@ -42,18 +45,24 @@ struct FSK { int Nmem; /* size of extra mem for timing adj */ int P; /* oversample rate for timing est/adj */ int Nsym; /* Number of symbols spat out in a processing frame */ + int Nbits; /* Number of bits spat out in a processing frame */ int f1_tx; /* f1 for modulator */ - int f2_tx; /* f2 for modulator */ + int fs_tx; /* Space between TX freqs for modulatosr */ + int mode; /* 2FSK or 4FSK */ /* Parameters used by demod */ COMP phi1_c; COMP phi2_c; + COMP phi3_c; + COMP phi4_c; kiss_fftr_cfg fft_cfg; /* Config for KISS FFT, used in freq est */ float norm_rx_timing; /* Normalized RX timing */ float *samp_old; /* Tail end of last batch of samples */ int nstash; /* How many elements are in there */ + float *fft_est; /* Freq est FFT magnitude */ + /* Memory used by demod but not important between demod frames */ /* Parameters used by mod */ @@ -63,7 +72,8 @@ struct FSK { float EbNodB; /* Estimated EbNo in dB */ float f1_est; /* Estimated f1 freq. */ float f2_est; /* Estimated f2 freq. */ - float twist_est; /* Estimaged 'twist' from freq est */ + float f3_est; /* Estimated f3 freq. */ + float f4_est; /* Estimated f4 freq. */ float ppm; /* Estimated PPM clock offset */ /* Parameters used by mod/demod and driving code */ @@ -79,7 +89,7 @@ struct FSK { * int tx_f1 - '0' frequency * int tx_f2 - '1' frequency */ -struct FSK * fsk_create(int Fs, int Rs, int tx_f1, int tx_f2); +struct FSK * fsk_create(int Fs, int Rs,int M, int tx_f1,int tx_fs); /* * Destroy an FSK state struct and free it's memory @@ -93,7 +103,7 @@ void fsk_destroy(struct FSK *fsk); * * struct FSK *fsk - FSK config/state struct, set up by fsk_create * float fsk_out[] - Buffer for N samples of modulated FSK - * uint8_t tx_bits[] - Buffer containing Nsym unpacked bits + * uint8_t tx_bits[] - Buffer containing Nbits unpacked bits */ void fsk_mod(struct FSK *fsk, float fsk_out[], uint8_t tx_bits[]); @@ -112,7 +122,7 @@ uint32_t fsk_nin(struct FSK *fsk); * demodulated can be found by calling fsk_nin(). * * struct FSK *fsk - FSK config/state struct, set up by fsk_create - * uint8_t rx_bits[] - Buffer for Nsym unpacked bits to be written + * uint8_t rx_bits[] - Buffer for Nbits unpacked bits to be written * float fsk_in[] - nin samples of modualted FSK */ void fsk_demod(struct FSK *fsk, uint8_t rx_bits[],float fsk_in[]); diff --git a/codec2-dev/src/fsk_demod.c b/codec2-dev/src/fsk_demod.c index 6fd243d9..cf0cecfc 100644 --- a/codec2-dev/src/fsk_demod.c +++ b/codec2-dev/src/fsk_demod.c @@ -36,41 +36,42 @@ int main(int argc,char *argv[]){ struct FSK *fsk; - int Fs,Rs; + int Fs,Rs,M; FILE *fin,*fout; uint8_t *bitbuf; int16_t *rawbuf; float *modbuf; int i,t; - if(argc<5){ - fprintf(stderr,"usage: %s SampleFreq SymbolFreq InputModemRawFile OutputOneBitPerCharFile [OctaveLogFile]\n",argv[0]); + if(argc<6){ + fprintf(stderr,"usage: %s Mode SampleFreq SymbolFreq InputModemRawFile OutputOneBitPerCharFile [OctaveLogFile]\n",argv[0]); exit(1); } /* Extract parameters */ - Fs = atoi(argv[1]); - Rs = atoi(argv[2]); + M = atoi(argv[1]); + Fs = atoi(argv[2]); + Rs = atoi(argv[3]); /* Open files */ - if(strcmp(argv[3],"-")==0){ + if(strcmp(argv[4],"-")==0){ fin = stdin; }else{ - fin = fopen(argv[3],"r"); + fin = fopen(argv[4],"r"); } - if(strcmp(argv[4],"-")==0){ + if(strcmp(argv[5],"-")==0){ fout = stdout; }else{ - fout = fopen(argv[4],"w"); + fout = fopen(argv[5],"w"); } - if(argc>5) - modem_probe_init("fsk2",argv[5]); + if(argc>6) + modem_probe_init("fsk2",argv[6]); /* set up FSK */ - fsk = fsk_create(Fs,Rs,1200,1600); + fsk = fsk_create(Fs,Rs,M,1200,400); if(fin==NULL || fout==NULL || fsk==NULL){ fprintf(stderr,"Couldn't open test vector files\n"); @@ -78,7 +79,7 @@ int main(int argc,char *argv[]){ } /* allocate buffers for processing */ - bitbuf = (uint8_t*)alloca(sizeof(uint8_t)*fsk->Nsym); + bitbuf = (uint8_t*)alloca(sizeof(uint8_t)*fsk->Nbits); rawbuf = (int16_t*)alloca(sizeof(int16_t)*(fsk->N+fsk->Ts*2)); modbuf = (float*)alloca(sizeof(float)*(fsk->N+fsk->Ts*2)); @@ -89,11 +90,11 @@ int main(int argc,char *argv[]){ } modem_probe_samp_f("t_d_sampin",modbuf,fsk_nin(fsk)); fsk_demod(fsk,bitbuf,modbuf); - for(i=0;iNsym;i++){ + for(i=0;iNbits;i++){ t = (int)bitbuf[i]; modem_probe_samp_i("t_d_bitout",&t,1); } - fwrite(bitbuf,sizeof(uint8_t),fsk->Nsym,fout); + fwrite(bitbuf,sizeof(uint8_t),fsk->Nbits,fout); if(fin == stdin || fout == stdin){ fflush(fin); diff --git a/codec2-dev/src/fsk_mod.c b/codec2-dev/src/fsk_mod.c index c5d66e75..c5a39991 100644 --- a/codec2-dev/src/fsk_mod.c +++ b/codec2-dev/src/fsk_mod.c @@ -35,39 +35,40 @@ int main(int argc,char *argv[]){ struct FSK *fsk; - int Fs,Rs,f1,f2; + int Fs,Rs,f1,fs,M; int i; FILE *fin,*fout; uint8_t *bitbuf; int16_t *rawbuf; float *modbuf; - if(argc<7){ - fprintf(stderr,"usage: %s SampleFreq SymbolFreq TxFreq1 TxFreq2 InputOneBitPerCharFile OutputModRawFile\n",argv[0]); + if(argc<8){ + fprintf(stderr,"usage: %s Mode SampleFreq SymbolFreq TxFreq1 TxFreqSpace InputOneBitPerCharFile OutputModRawFile\n",argv[0]); exit(1); } /* Extract parameters */ - Fs = atoi(argv[1]); - Rs = atoi(argv[2]); - f1 = atoi(argv[3]); - f2 = atoi(argv[4]); + M = atoi(argv[1]); + Fs = atoi(argv[2]); + Rs = atoi(argv[3]); + f1 = atoi(argv[4]); + fs = atoi(argv[5]); - if(strcmp(argv[5],"-")==0){ + if(strcmp(argv[6],"-")==0){ fin = stdin; }else{ - fin = fopen(argv[5],"r"); + fin = fopen(argv[6],"r"); } - if(strcmp(argv[6],"-")==0){ + if(strcmp(argv[7],"-")==0){ fout = stdout; }else{ - fout = fopen(argv[6],"w"); + fout = fopen(argv[7],"w"); } /* set up FSK */ - fsk = fsk_create(Fs,Rs,f1,f2); + fsk = fsk_create(Fs,Rs,M,f1,fs); if(fin==NULL || fout==NULL || fsk==NULL){ fprintf(stderr,"Couldn't open test vector files\n"); @@ -75,12 +76,12 @@ int main(int argc,char *argv[]){ } /* allocate buffers for processing */ - bitbuf = (uint8_t*)alloca(sizeof(uint8_t)*fsk->Nsym); + bitbuf = (uint8_t*)alloca(sizeof(uint8_t)*fsk->Nbits); rawbuf = (int16_t*)alloca(sizeof(int16_t)*fsk->N); modbuf = (float*)alloca(sizeof(float)*fsk->N); /* Modulate! */ - while( fread(bitbuf,sizeof(uint8_t),fsk->Nsym,fin) == fsk->Nsym ){ + while( fread(bitbuf,sizeof(uint8_t),fsk->Nbits,fin) == fsk->Nbits ){ fsk_mod(fsk,modbuf,bitbuf); for(i=0; iN; i++){ rawbuf[i] = (int16_t)(modbuf[i]*(float)FDMDV_SCALE); diff --git a/codec2-dev/unittest/tfsk.c b/codec2-dev/unittest/tfsk.c index 52195f69..67784068 100644 --- a/codec2-dev/unittest/tfsk.c +++ b/codec2-dev/unittest/tfsk.c @@ -40,8 +40,9 @@ #define ST_FS 8000 #define ST_RS 100 #define ST_F1 1200 -#define ST_F2 1600 +#define ST_Fs 400 #define ST_EBNO 8 +#define ST_M 2 #define TEST_SELF_FULL 1 /* No-arg self test */ #define TEST_MOD 2 /* Test modulator using in and out file */ @@ -50,7 +51,7 @@ int main(int argc,char *argv[]){ struct FSK *fsk; - int Fs,Rs,f1,f2; + int Fs,Rs,f1,fs,M; FILE *fin,*fout; uint8_t *bitbuf = NULL; @@ -75,11 +76,11 @@ int main(int argc,char *argv[]){ Fs = ST_FS; Rs = ST_RS; f1 = ST_F1; - f2 = ST_F2; - - } else if (argc<8){ + fs = ST_Fs; + M = ST_M; + } else if (argc<9){ /* Not running any test */ - printf("Usage: %s [(M|D) TXFreq1 TXFreq2 SampleRate BitRate InputFile OutputFile OctaveLogFile]\n",argv[0]); + printf("Usage: %s [(M|D) Mode TXFreq1 TXFreqSpace SampleRate BitRate InputFile OutputFile OctaveLogFile]\n",argv[0]); exit(1); } else { /* Running stim-drivin test */ @@ -94,29 +95,30 @@ int main(int argc,char *argv[]){ exit(1); } /* Extract parameters */ - Fs = atoi(argv[4]); - Rs = atoi(argv[5]); - f1 = atoi(argv[2]); - f2 = atoi(argv[3]); + M = atoi(argv[2]); + f1 = atoi(argv[3]); + fs = atoi(argv[4]); + Fs = atoi(argv[5]); + Rs = atoi(argv[6]); /* Open files */ - fin = fopen(argv[6],"r"); - fout = fopen(argv[7],"w"); + fin = fopen(argv[7],"r"); + fout = fopen(argv[8],"w"); if(fin == NULL || fout == NULL){ printf("Couldn't open test vector files\n"); exit(1); } /* Init modem probing */ - modem_probe_init("fsk2",argv[8]); + modem_probe_init("fsk",argv[9]); } srand(1); /* set up FSK */ - fsk = fsk_create(Fs,Rs,f1,f2); - + fsk = fsk_create(Fs,Rs,M,f1,fs); + fprintf(stderr,"Running in mode %d\n",M); /* Modulate! */ if(test_type == TEST_MOD || test_type == TEST_SELF_FULL){ /* Generate random bits for self test */ @@ -136,26 +138,26 @@ int main(int argc,char *argv[]){ i = 0; /* Read in some bits */ bitbufp = bitbuf; - while( fread(bitbufp,sizeof(uint8_t),fsk->Nsym,fin) == fsk->Nsym){ + while( fread(bitbufp,sizeof(uint8_t),fsk->Nbits,fin) == fsk->Nbits){ i++; - bitbufp+=fsk->Nsym; + bitbufp+=fsk->Nbits; /* Make sure we don't break the buffer */ - if(i*fsk->Nsym > bitbufsize){ - bitbuf = realloc(bitbuf,sizeof(uint8_t)*(bitbufsize+fsk->Nsym)); - bitbufsize += fsk->Nsym; + if(i*fsk->Nbits > bitbufsize){ + bitbuf = realloc(bitbuf,sizeof(uint8_t)*(bitbufsize+fsk->Nbits)); + bitbufsize += fsk->Nbits; } } } /* Allocate modulation buffer */ - modbuf = (float*)malloc(sizeof(float)*(bitbufsize/fsk->Nsym)*fsk->N*4); - modbufsize = (bitbufsize*fsk->Ts); + modbuf = (float*)malloc(sizeof(float)*(bitbufsize/fsk->Nbits)*fsk->N*4); + modbufsize = (bitbufsize/fsk->Nbits)*fsk->N; /* Do the modulation */ modbufp = modbuf; bitbufp = bitbuf; while( bitbufp < bitbuf+bitbufsize){ fsk_mod(fsk, modbufp, bitbufp); modbufp += fsk->N; - bitbufp += fsk->Nsym; + bitbufp += fsk->Nbits; } /* For a mod-only test, write out the result */ if(test_type == TEST_MOD){ @@ -173,12 +175,12 @@ int main(int argc,char *argv[]){ if(test_type == TEST_DEMOD || test_type == TEST_SELF_FULL){ free(modbuf); modbuf = malloc(sizeof(float)*(fsk->N+fsk->Ts*2)); - bitbuf = malloc(sizeof(uint8_t)*fsk->Nsym); + bitbuf = malloc(sizeof(uint8_t)*fsk->Nbits); /* Demod-only test */ if(test_type == TEST_DEMOD){ while( fread(modbuf,sizeof(float),fsk_nin(fsk),fin) == fsk_nin(fsk) ){ fsk_demod(fsk,bitbuf,modbuf); - fwrite(bitbuf,sizeof(uint8_t),fsk->Nsym,fout); + fwrite(bitbuf,sizeof(uint8_t),fsk->Nbits,fout); } } /* Demod after channel imp. and mod */