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');
%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<tol;
printf('\n*** vcompare failed %s in test %s. Diff: %f Tol: %f\n\n',vname,tname,maxdvec,tol);
titlestr = sprintf('Diff between C and Octave of %s for %s',vname,tname)
- figure(12)
+ figure(10+pnum*2)
plot(abs(dvec))
title(titlestr)
- figure(13)
- plot(abs(va))
+ figure(11+pnum*2)
+ plot((1:length(vc)),abs(vc),(1:length(voct)),abs(voct))
- figure(14)
- plot(abs(vb))
end
- assert(pass);
endfunction
-function test_stats = fsk_demod_xt(Fs,Rs,f1,f2,mod,tname)
+function test_stats = fsk_demod_xt(Fs,Rs,f1,fsp,mod,tname,M=2)
global tfsk_location;
%Name of executable containing the modulator
fsk_demod_ex_file = '../build/unittest/tfsk';
tvecfilename = sprintf('fsk_demod_ut_tracevec_%d.txt',getpid());
%command to be run by system to launch the demod
- command = sprintf('%s D %d %d %d %d %s %s %s',tfsk_location,f1,f2,Fs,Rs,modvecfilename,bitvecfilename,tvecfilename);
+ command = sprintf('%s D %d %d %d %d %d %s %s %s',tfsk_location,M,f1,fsp,Fs,Rs,modvecfilename,bitvecfilename,tvecfilename);
%save modulated input into a file
modvecfile = fopen(modvecfilename,'wb+');
o_f1_dc = [];
o_f2_dc = [];
+ o_f3_dc = [];
+ o_f4_dc = [];
o_f1_int = [];
o_f2_int = [];
+ o_f3_int = [];
+ o_f4_int = [];
+ o_f1 = [];
+ o_f2 = [];
+ o_f3 = [];
+ o_f4 = [];
o_EbNodB = [];
o_ppm = [];
+ o_Sf = [];
+ o_fest = [];
o_rx_timing = [];
%Run octave demod, dump some test vectors
- states = fsk_horus_init(Fs,Rs);
+ states = fsk_horus_init(Fs,Rs,M);
Ts = states.Ts;
P = states.P;
- states.f1_tx = f1;
- states.f2_tx = f2;
+ states.ftx(1) = f1;
+ states.ftx(2) = f1+fsp;
+ states.ftx(3) = f1+fsp*2;
+ states.ftx(4) = f1+fsp*3;
states.dA = 1;
states.dF = 0;
modin = mod;
obits = [];
while length(modin)>=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'));
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));
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
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;
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
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
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
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);
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;
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;
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))
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);
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);
end
close all
- figure(2);
clf;
semilogy(ebnodbrange, berinc,'r;2FSK non-coherent theory;')
hold on;
endfunction
-plot_fsk_bers
-test_fsk_battery
+%plot_fsk_bers
+%test_fsk_battery
#include <stdint.h>
#include <math.h>
-#include "fsk.h"
+#include "fsk4.h"
#include "comp_prim.h"
#include "kiss_fftr.h"
#include "modem_probe.h"
\*---------------------------------------------------------------------------*/
+/*
+ * 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
\*---------------------------------------------------------------------------*/
-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;
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;
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;
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);
return NULL;
}
- for(i=0;i<memold;i++) fsk->samp_old[i]=0;
+ for(int i=0;i<memold;i++)fsk->samp_old[i]=0;
fsk->fft_cfg = kiss_fftr_alloc(Ndft,0,NULL,NULL);
if(fsk->fft_cfg == NULL){
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;i<Ndft/2;i++)fsk->fft_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;
}
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
* 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;
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?nin:Ndft;
+ fft_samps = Ndft;
+ f_min = (FEST_MIN*Ndft)/Fs;
+ f_max = (FEST_MAX*Ndft)/Fs;
+ f_zero = (FEST_MIN_SPACING*Ndft)/(2*Fs);
+
+ int fft_loops = nin/Ndft;
+ for(j=0; j<fft_loops; j++){
/* Copy FSK buffer into reals of FFT buffer and apply a hann window */
- for(i=0; i<fft_samps ; i++){
- /* Note : This is a sort of bug copied from fsk_horus */
- /* if N+Ts/2 > 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(; i<Ndft;i++){
- fftin[i] = 0;
- }
+ for(i=0; i<fft_samps; i++){
+ hann = 1-cosf((2*M_PI*(float)i)/((float)fft_samps-1));
+
+ fftin[i] = (kiss_fft_scalar)0.5*hann*fsk_in[i+Ndft*j];
+ }
+ /* Zero out the remaining slots */
+ for(; i<Ndft;i++){
+ fftin[i] = 0;
+ }
+
+ /* Do the FFT */
+ kiss_fftr(fft_cfg,fftin,fftout);
+
+ /* 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; i<Ndft/2; i++){
+ fftout[i].r = (fftout[i].r*fftout[i].r) + (fftout[i].i*fftout[i].i) ;
+ }
+
+ /* Zero out the minimum and maximum ends */
+ for(i=0; i<f_min; i++){
+ fftout[i].r = 0;
+ }
+ for(i=f_max-1; i<Ndft/2; i++){
+ fftout[i].r = 0;
+ }
+ /* Mix back in with the previous fft block */
+ /* Copy new fft est into imag of fftout for frequency divination below */
+ for(i=0; i<Ndft/2; i++){
+ fsk->fft_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<M; i++){
+ imax = 0;
+ max = 0;
+ for(j=0;j<Ndft/2;j++){
+ if(fftout[j].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<f_max; j++)
+ fftout[j].i = 0;
+
+ /* Stick the freq index on the list */
+ freqi[i] = imax;
+ }
+
+ /* 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);
+ }
+
+
+
+}
+
+void fsk2_demod(struct FSK *fsk, uint8_t rx_bits[], float 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;
+ 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; i<Ndft/2; i++){
- fftout[i].r = (fftout[i].r*fftout[i].r) + (fftout[i].i*fftout[i].i) ;
+ /* 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];
+ 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<Ts-(Ts/P); 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[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<Ndft/2; i++){
- if(fftout[i].r > 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; j<Ts; j++){
+ t1 = cadd(t1,f1_intbuf[j]);
+ t2 = cadd(t2,f2_intbuf[j]);
+ }
+ f1_int[i] = t1;
+ f2_int[i] = t2;
+
}
+
+ fsk->phi1_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<j;i++)
- fftout[i].r = 0;
+ /* Estimate sample clock offset */
+ d_norm_rx_timing = norm_rx_timing - old_norm_rx_timing;
- /* Find the other maximum */
- max = 0;
- m2 = 0;
- for(i=0; i<Ndft/2; i++){
- if(fftout[i].r > 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; i<nsym; i++){
+ int st = (i+1)*P;
+ t1 = fcmult(1-fract,f1_int[st+ low_sample]);
+ t1 = cadd(t1,fcmult( fract,f1_int[st+high_sample]));
+ t2 = fcmult(1-fract,f2_int[st+ low_sample]);
+ t2 = cadd(t2,fcmult( fract,f2_int[st+high_sample]));
+
+ /* Figure mag^2 of each resampled fx_int */
+ tmax[0] = (t1.real*t1.real) + (t1.imag*t1.imag);
+ tmax[1] = (t2.real*t2.real) + (t2.imag*t2.imag);
+ rx_bits[i] = (tmax[1]>tmax[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; i<nsym; i++){
+ int st = (i+1)*P;
+ t1 = fcmult(1-fract,f1_int[st+ low_sample]);
+ t1 = cadd(t1,fcmult( fract,f1_int[st+high_sample]));
+ t2 = fcmult(1-fract,f2_int[st+ low_sample]);
+ t2 = cadd(t2,fcmult( fract,f2_int[st+high_sample]));
+
+ /* Accumulate resampled int magnitude for EbNodB estimation */
+ ft1 = sqrtf(t1.real*t1.real + t1.imag*t1.imag);
+ ft2 = sqrtf(t2.real*t2.real + t2.imag*t2.imag);
+ ft1 = fabsf(ft1-ft2);
+ ft2 = abs(meanebno-ft1);
+ stdebno += ft2*ft2;
+ }
+ /* Finish figuring std. dev. */
+ stdebno = sqrtf(stdebno/(float)(nsym-1));
+ fsk->EbNodB = 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;
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;
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;
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);
}
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; j<Ts; j++){
t1 = cadd(t1,f1_intbuf[j]);
t2 = cadd(t2,f2_intbuf[j]);
+ t3 = cadd(t3,f3_intbuf[j]);
+ t4 = cadd(t4,f4_intbuf[j]);
}
f1_int[i] = t1;
f2_int[i] = t2;
+ f3_int[i] = t3;
+ f4_int[i] = t4;
}
+ phi1_c = comp_normalize(phi1_c);
+ phi2_c = comp_normalize(phi2_c);
+ phi3_c = comp_normalize(phi3_c);
+ phi4_c = comp_normalize(phi4_c);
fsk->phi1_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);
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;
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;
#endif
/* FINALLY, THE BITS */
- /* also, resample f1_int,f2_int */
+ /* also, resample fx_int */
for(i=0; i<nsym; i++){
int st = (i+1)*P;
t1 = fcmult(1-fract,f1_int[st+ low_sample]);
t1 = cadd(t1,fcmult( fract,f1_int[st+high_sample]));
t2 = fcmult(1-fract,f2_int[st+ low_sample]);
t2 = cadd(t2,fcmult( fract,f2_int[st+high_sample]));
+ t3 = fcmult(1-fract,f3_int[st+ low_sample]);
+ t3 = cadd(t3,fcmult( fract,f3_int[st+high_sample]));
+ t4 = fcmult(1-fract,f4_int[st+ low_sample]);
+ t4 = cadd(t4,fcmult( fract,f4_int[st+high_sample]));
+
+ /* Figure mag^2 of each resampled fx_int */
+ tmax[0] = (t1.real*t1.real) + (t1.imag*t1.imag);
+ tmax[1] = (t2.real*t2.real) + (t2.imag*t2.imag);
+ tmax[2] = (t3.real*t3.real) + (t3.imag*t3.imag);
+ tmax[3] = (t4.real*t4.real) + (t4.imag*t4.imag);
+
+ /* Find the maximum symbol */
+ max = 0;
+ sym = 0;
+ for(j=0; j<4; j++){
+ if(tmax[j]>max){
+ 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 */
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
/* 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; i<fsk->Nsym; 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; j<Ts; j++){
- tx_phase_c = cmult(tx_phase_c,dph);
- fsk_out[i*Ts+j] = 2*tx_phase_c.real;
- }
- }
+ dosc_f[2] = comp_exp_j(2*M_PI*((float)(f1_tx+fs_tx*2)/(float)(Fs)));
+ dosc_f[3] = comp_exp_j(2*M_PI*((float)(f1_tx+fs_tx*3)/(float)(Fs)));
+
+ if(fsk->mode == 2){
+ /* Outer loop through bits */
+ for(i=0; i<fsk->Nsym; i++){
+ /* select current bit phase shift */
+ dph = tx_bits[i]==0?dosc_f[0]:dosc_f[1];
+ for(j=0; j<Ts; j++){
+ tx_phase_c = cmult(tx_phase_c,dph);
+ fsk_out[i*Ts+j] = 2*tx_phase_c.real;
+ }
+ }
+ }else {
+ /* Same thing as above, but with more bits and phases */
+ for(i=0; i<fsk->Nsym; 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; j<Ts; j++){
+ tx_phase_c = cmult(tx_phase_c,dph);
+ fsk_out[i*Ts+j] = 2*tx_phase_c.real;
+ }
+ }
+ }
- /* Log the modulated samples */
- modem_probe_samp_f("t_txmod",fsk_out,fsk->N);
+ /* 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;
}
projects / freetel-svn-tracking.git / commitdiff