From: drowe67 Date: Tue, 24 Feb 2015 00:48:48 +0000 (+0000) Subject: first pass FM demod working in C X-Git-Url: http://git.whiteaudio.com/gitweb/?a=commitdiff_plain;h=306053aa5f38a39c5560d9ad281e5e6f07e405d6;p=freetel-svn-tracking.git first pass FM demod working in C git-svn-id: https://svn.code.sf.net/p/freetel/code@2045 01035d8c-6547-0410-b346-abe4f91aad63 --- diff --git a/codec2-dev/README b/codec2-dev/README index 97f3c1e6..58994747 100644 --- a/codec2-dev/README +++ b/codec2-dev/README @@ -81,8 +81,8 @@ Debugging $ cd ~/codec2 $ rm -Rf build_linux && mkdir build_linux - $ CFLAGS=-DDUMP cmake .. $ cd build_linux + $ CFLAGS=-DDUMP cmake .. $ make Directories diff --git a/codec2-dev/octave/adcres.m b/codec2-dev/octave/adcres.m index 6a3f376d..f4f04ac0 100644 --- a/codec2-dev/octave/adcres.m +++ b/codec2-dev/octave/adcres.m @@ -52,5 +52,5 @@ title('After Decimation'); subplot(212) plot(20*log10(abs(fft(s4)/fs))) grid -title('After Flattening Filter'); +title('After Equaliser'); axis([0 fs/2/M -10 50]) diff --git a/codec2-dev/octave/bandpasssampling.m b/codec2-dev/octave/bandpasssampling.m new file mode 100644 index 00000000..9a2a4efc --- /dev/null +++ b/codec2-dev/octave/bandpasssampling.m @@ -0,0 +1,34 @@ +% bandpasssampling.m +% David Rowe 23 Feb 2015 +% +% Band pass sampling example + +graphics_toolkit ("gnuplot"); + +t = 0:1E-3:1-1E-3; +f1 = 5; +f2 = 105; + +x = 1:10:length(s1); + +s1 = cos(2*pi*f1*t); +s1_sampled = s1(x); + +s2 = cos(2*pi*f2*t); +s2_sampled = s2(x); + +figure(1) +subplot(211) +plot(t,s1) +title('5Hz signal sampled at 100 Hz'); +subplot(212) +stem(x*1E-3, s1_sampled,'r') +xlabel('Time (s)'); + +figure(2) +subplot(211) +plot(t,s2) +title('105Hz signal sampled at 100 Hz'); +subplot(212) +stem(x*1E-3, s2_sampled,'r') +xlabel('Time (s)'); diff --git a/codec2-dev/octave/fm.m b/codec2-dev/octave/fm.m index 543b633c..765aae12 100644 --- a/codec2-dev/octave/fm.m +++ b/codec2-dev/octave/fm.m @@ -5,6 +5,8 @@ 1; +graphics_toolkit ("gnuplot"); + function fm_states = analog_fm_init(fm_states) % FM modulator constants @@ -47,6 +49,29 @@ function fm_states = analog_fm_init(fm_states) endfunction +function fm_fir_coeff_file(fm_states, filename) + global gt_alpha5_root; + global Nfilter; + + f=fopen(filename,"wt"); + + fprintf(f,"/* Generated by fm_fir_coeff_file() Octave function in fm.m */\n\n"); + fprintf(f,"const float bin[]={\n"); + for m=1:length(fm_states.bin)-1 + fprintf(f," %g,\n", fm_states.bin(m)); + endfor + fprintf(f," %g\n};\n\n", fm_states.bin(length(fm_states.bin))); + + fprintf(f,"const float bout[]={\n"); + for m=1:length(fm_states.bout)-1 + fprintf(f," %g,\n", fm_states.bout(m)); + endfor + fprintf(f," %g\n};\n", fm_states.bout(length(fm_states.bout))); + + fclose(f); +endfunction + + function tx = analog_fm_mod(fm_states, mod) Fs = fm_states.Fs; fc = fm_states.fc; wc = 2*pi*fc/Fs; @@ -86,7 +111,7 @@ function [rx_out rx_bb] = analog_fm_demod(fm_states, rx) rx_bb_diff = [ 1 rx_bb(2:nsam) .* conj(rx_bb(1:nsam-1))]; rx_out = atan2(imag(rx_bb_diff),real(rx_bb_diff)); - % limit maximum phase jumps, to remove staticc type noise at low SNRs + % limit maximum phase jumps, to remove static type noise at low SNRs rx_out(find(rx_out > wd)) = wd; rx_out(find(rx_out < -wd)) = -wd; @@ -141,7 +166,7 @@ function sim_out = analog_fm_test(sim_in) mod = sin(wm*t); tx = analog_fm_mod(fm_states, mod); - + % Channel --------------------------------- noise = sqrt(variance/2)*(randn(1,nsam) + j*randn(1,nsam)); @@ -260,11 +285,35 @@ function run_fm_single sim_in.pre_emp = 0; sim_in.de_emp = 0; - sim_in.CNdB = 25; + sim_in.CNdB = 20; sim_out = analog_fm_test(sim_in); end +function fm_mod_file(file_name_out) + fm_states.Fs = 44400; + fm_states.fm_max = 3E3; + fm_states.fd = 5E3; + fm_states.fc = fm_states.Fs/4; + fm_states.pre_emp = 0; + fm_states.de_emp = 0; + fm_states.Ts = 1; + fm_states.output_filter = 1; + fm_states = analog_fm_init(fm_states); + + nsam = fm_states.Fs * 10; + t = 0:(nsam-1); + fm = 1000; wm = 2*pi*fm/fm_states.Fs; + mod = sin(wm*t); + tx = analog_fm_mod(fm_states, mod); + + tx_out = tx*16384; + fout = fopen(file_name_out,"wb"); + fwrite(fout, tx_out, "short"); + fclose(fout); +endfunction + + function fm_demod_file(file_name_out, file_name_in) fin = fopen(file_name_in,"rb"); rx = fread(fin,"short")'; @@ -345,10 +394,30 @@ function fm_demod_file(file_name_out, file_name_in) endfunction + +% generate filter coeffs for C implementation of FM demod + +function make_coeff_file + fm_states.Fs = 44400; + fm_states.fm_max = 3E3; + fm_states.fd = 5E3; + fm_states.fc = fm_states.Fs/4; + + fm_states.pre_emp = 0; + fm_states.de_emp = 0; + fm_states.Ts = 1; + fm_states.output_filter = 1; + fm_states = analog_fm_init(fm_states); + + fm_fir_coeff_file(fm_states, "fm_fir_coeff.h") +endfunction + + more off; %run_fm_curves %fm_demod_file("ssb_fm_out.raw","~/Desktop/ssb_fm.wav") %fm_demod_file("ssb25_fm_de.raw", "~/Desktop/ssb25db.wav") %run_fm_single - +%make_coeff_file +fm_mod_file("fm_1000.raw"); diff --git a/codec2-dev/octave/gmsk.m b/codec2-dev/octave/gmsk.m index 9d35f8e9..2092a593 100644 --- a/codec2-dev/octave/gmsk.m +++ b/codec2-dev/octave/gmsk.m @@ -269,6 +269,12 @@ function [rx_bits rx_int rx_filt] = gmsk_demod(gmsk_states, rx) k++; end + figure + subplot(211) + stem(re_syms) + subplot(211) + stem(im_syms) + figure; clf subplot(211) @@ -831,7 +837,7 @@ function gmsk_rx(rx_file_name, err_file_name) Rs = 1200; framesize = 480; npreamble = 480; - fc = 1500; + fc = 1900; gmsk_states.npreamble = npreamble; gmsk_states.verbose = 1; @@ -856,6 +862,9 @@ function gmsk_rx(rx_file_name, err_file_name) w = exp(-j*wc*(1:nsam)); rxbb = rx .* w; + figure; + plot(rx); + % find preamble [preamble_location freq_offset_est] = find_preamble(gmsk_states, M, npreamble, rxbb); @@ -875,11 +884,11 @@ function gmsk_rx(rx_file_name, err_file_name) rx_power_dB = 10*log10(rx_power); figure; subplot(211) - plot(rx_filt); + plot(rx_filt(1000:5*Fs)); title('GMSK Power (narrow filter)'); subplot(212) plot(rx_power_dB); - axis([1 length(rx_power) max(rx_power_dB)-9 max(rx_power_dB)+1]) + axis([1 length(rx_power) max(rx_power_dB)-29 max(rx_power_dB)+1]) grid("minor") % Work out where to sample N, and S+N @@ -954,7 +963,9 @@ function gmsk_rx(rx_file_name, err_file_name) w_est = (0:nsam-1)*2*pi*freq_offset_est/Fs; rxbb = rxbb.*exp(-j*w_est); st = preamble_location+npreamble*M; - en = min(nsam,st + 22*framesize*M); + en = min(nsam,st + 4*framesize*M); + %en = nsam; + gmsk_statres.verbose = 2; [rx_bits rx_out rx_filt] = gmsk_demod(gmsk_states, rxbb(st:en)); nframes_rx = length(rx_bits)/framesize; @@ -1000,6 +1011,7 @@ endfunction %run_test_channel_impairments %gmsk_tx("test_gmsk.raw") %gmsk_rx("ssb-ber5.wav") -gmsk_rx("~/Desktop/ssb25db.wav") +%gmsk_rx("ssb25db.wav") %gmsk_rx("~/Desktop/ssb_fm_gmsk_high.wav") +gmsk_rx("~/Desktop/test_gmsk_28BER.raw") diff --git a/codec2-dev/src/CMakeLists.txt b/codec2-dev/src/CMakeLists.txt index 86dada4e..6ea9d755 100644 --- a/codec2-dev/src/CMakeLists.txt +++ b/codec2-dev/src/CMakeLists.txt @@ -167,6 +167,7 @@ set(CODEC2_SRCS codec2.c fifo.c fdmdv.c + fm.c kiss_fft.c interp.c lsp.c @@ -190,6 +191,7 @@ set(CODEC2_PUBLIC_HEADERS golay23.h codec2.h codec2_fdmdv.h + codec2_fm.h codec2_fifo.h comp.h ) @@ -250,6 +252,9 @@ target_link_libraries(freedv_tx ${CMAKE_REQUIRED_LIBRARIES} codec2) add_executable(freedv_rx freedv_rx.c) target_link_libraries(freedv_rx ${CMAKE_REQUIRED_LIBRARIES} codec2) +add_executable(fm_demod fm_demod.c fm.c) +target_link_libraries(fm_demod ${CMAKE_REQUIRED_LIBRARIES}) + install(TARGETS codec2 EXPORT codec2-config LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} @@ -269,6 +274,7 @@ install(TARGETS c2sim fdmdv_get_test_bits fdmdv_mod fdmdv_demod + fm_demod fdmdv_put_test_bits fdmdv_interleave insert_errors diff --git a/codec2-dev/src/c2sim.c b/codec2-dev/src/c2sim.c index 5dc1f400..1e294a7b 100644 --- a/codec2-dev/src/c2sim.c +++ b/codec2-dev/src/c2sim.c @@ -708,7 +708,7 @@ int main(int argc, char *argv[]) //printf("Wo %f e %f\n", model.Wo, e); } - + aks_to_M2(fft_fwd_cfg, ak, order, &model, e, &snr, 1, simlpcpf, lpcpf, 1, LPCPF_BETA, LPCPF_GAMMA, Aw); apply_lpc_correction(&model); diff --git a/codec2-dev/src/codec2_fm.h b/codec2-dev/src/codec2_fm.h new file mode 100644 index 00000000..92457ecf --- /dev/null +++ b/codec2-dev/src/codec2_fm.h @@ -0,0 +1,50 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: codec2_fm.h + AUTHOR......: David Rowe + DATE CREATED: February 2015 + + Functions that implement analog FM, see also octave/fm.m. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2015 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 . +*/ + +#ifndef __CODEC2_FM__ +#define __CODEC2_FM__ + +#include "comp.h" + +struct FM { + float Fs; /* setme: sample rate */ + float fm_max; /* setme: maximum modulation frequency */ + float fd; /* setme: maximum deviation */ + float fc; /* setme: carrier frequency */ + COMP *rx_bb; + COMP rx_bb_filt_prev; + float *rx_dem_mem; + int nsam; + COMP lo_phase; +}; + +struct FM *fm_create(int nsam); +void fm_destroy(struct FM *fm_states); +void fm_demod(struct FM *fm, float rx_out[], float rx[]); + +#endif + diff --git a/codec2-dev/src/fm.c b/codec2-dev/src/fm.c new file mode 100644 index 00000000..8d5e5be0 --- /dev/null +++ b/codec2-dev/src/fm.c @@ -0,0 +1,224 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: fm.c + AUTHOR......: David Rowe + DATE CREATED: February 2015 + + Functions that implement analog FM modulation and demodulation, see + also octave/fm.m. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2015 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 . +*/ + +/*---------------------------------------------------------------------------*\ + + DEFINES + +\*---------------------------------------------------------------------------*/ + +#define FILT_MEM 200 + +/*---------------------------------------------------------------------------*\ + + INCLUDES + +\*---------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include + +#include "codec2_fm.h" +#include "fm_fir_coeff.h" + +/*---------------------------------------------------------------------------*\ + + FUNCTIONS + +\*---------------------------------------------------------------------------*/ + +static COMP cconj(COMP a) +{ + COMP res; + + res.real = a.real; + res.imag = -a.imag; + + return res; +} + +static COMP cmult(COMP a, COMP b) +{ + COMP res; + + res.real = a.real*b.real - a.imag*b.imag; + res.imag = a.real*b.imag + a.imag*b.real; + + return res; +} + +static COMP fcmult(float a, COMP b) +{ + COMP res; + + res.real = a*b.real; + res.imag = a*b.imag; + + return res; +} + +static float cabsolute(COMP a) +{ + return sqrtf(powf(a.real, 2.0) + powf(a.imag, 2.0)); +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: fm_create + AUTHOR......: David Rowe + DATE CREATED: 24 Feb 2015 + + Create and initialise an instance of the "modem". Returns a pointer + to the modem states or NULL on failure. One set of states is + sufficient for a full duplex modem. + +\*---------------------------------------------------------------------------*/ + +struct FM *fm_create(int nsam) +{ + struct FM *fm; + + fm = (struct FM*)malloc(sizeof(struct FM)); + if (fm == NULL) + return NULL; + fm->rx_bb = (COMP*)malloc(sizeof(COMP)*(FILT_MEM+nsam)); + assert(fm->rx_bb != NULL); + + fm->rx_bb_filt_prev.real = 0.0; + fm->rx_bb_filt_prev.imag = 0.0; + fm->lo_phase.real = 1.0; + fm->lo_phase.imag = 0.0; + + fm->rx_dem_mem = (float*)malloc(sizeof(float)*(FILT_MEM+nsam)); + assert(fm->rx_dem_mem != NULL); + + fm->nsam = nsam; + + return fm; +} + + +void fm_destroy(struct FM *fm_states) +{ + free(fm_states->rx_bb); + free(fm_states->rx_dem_mem); + free(fm_states); +} + +/*---------------------------------------------------------------------------*\ + + FUNCTION....: fm_demod + AUTHOR......: David Rowe + DATE CREATED: 24 Feb 2015 + + Demodulate a FM signal to baseband audio. + +\*---------------------------------------------------------------------------*/ + +void fm_demod(struct FM *fm_states, float rx_out[], float rx[]) +{ + float Fs = fm_states->Fs; + float fc = fm_states->fc; + float wc = 2*M_PI*fc/Fs; + float fd = fm_states->fd; + float wd = 2*M_PI*fd/Fs; + COMP *rx_bb = fm_states->rx_bb + FILT_MEM; + COMP wc_rect, rx_bb_filt, rx_bb_diff; + float rx_dem, acc; + float *rx_dem_mem = fm_states->rx_dem_mem + FILT_MEM; + int nsam = fm_states->nsam; + float mag; + int i,k; + + wc_rect.real = cos(wc); wc_rect.imag = -sin(wc); + + for(i=0; ilo_phase = cmult(fm_states->lo_phase, wc_rect); + rx_bb[i] = fcmult(rx[i], fm_states->lo_phase); + + /* input FIR filter */ + + rx_bb_filt.real = 0.0; rx_bb_filt.imag = 0.0; + for(k=0; klo_phase.real, fm_states->lo_phase.imag); + //printf("%f %f %f\n", rx[i], rx_bb[i].real, rx_bb[i].imag); + //printf("%f %f\n", rx_bb_filt.real, rx_bb_filt.imag); + /* + Differentiate first, in rect domain, then find angle, this + puts signal on the positive side of the real axis and helps + atan2() behaive. + */ + + rx_bb_diff = cmult(rx_bb_filt, cconj(fm_states->rx_bb_filt_prev)); + fm_states->rx_bb_filt_prev = rx_bb_filt; + + rx_dem = atan2(rx_bb_diff.imag, rx_bb_diff.real); + + /* limit maximum phase jumps, to remove static type noise at low SNRs */ + + if (rx_dem > wd) + rx_dem = wd; + if (rx_dem < -wd) + rx_dem = -wd; + + rx_dem *= (1/wd); + //printf("%f %f\n", rx_bb_diff.real, rx_bb_diff.imag); + rx_dem_mem[i] = rx_dem; + acc = 0; + for(k=0; klo_phase); + fm_states->lo_phase.real /= mag; + fm_states->lo_phase.imag /= mag; + +} diff --git a/codec2-dev/src/fm_demod.c b/codec2-dev/src/fm_demod.c new file mode 100644 index 00000000..8042df65 --- /dev/null +++ b/codec2-dev/src/fm_demod.c @@ -0,0 +1,92 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: fm_demod.c + AUTHOR......: David Rowe + DATE CREATED: Feb 24 2015 + + Given an input raw file (44.4 kHz, 16 bit shorts) with a FM signal centered + 11.1 kHz, outputs a file of demodulated audio samples. + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2015 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, 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 . +*/ + +#include +#include +#include +#include +#include +#include + +#include "codec2_fm.h" +#include "octave.h" + +#define N 160 + +int main(int argc, char *argv[]) +{ + FILE *fin, *fout; + struct FM *fm; + short buf[N]; + float rx[N]; + float rx_out[N]; + int i; + + if (argc < 2) { + printf("usage: %s InputFMRawFile OutputSpeechRawFile\n", argv[0]); + printf("e.g %s fm.raw fm_demodulated.raw\n", argv[0]); + exit(1); + } + + if (strcmp(argv[1], "-") == 0) fin = stdin; + else if ( (fin = fopen(argv[1],"rb")) == NULL ) { + fprintf(stderr, "Error opening input file: %s: %s.\n", + argv[1], strerror(errno)); + exit(1); + } + + if (strcmp(argv[2], "-") == 0) fout = stdout; + else if ( (fout = fopen(argv[2],"wb")) == NULL ) { + fprintf(stderr, "Error opening output file: %s: %s.\n", + argv[2], strerror(errno)); + exit(1); + } + + fm = fm_create(N); + fm->Fs = 44400.0; + fm->fm_max = 3000.0; + fm->fd = 5000.0; + fm->fc = fm->Fs/4; + + while(fread(buf, sizeof(short), N, fin) == N) { + for(i=0; i