From 0e80e1009e5ec300891617ef2dae92fdc3225c9a Mon Sep 17 00:00:00 2001 From: "Charles J. Cliffe" Date: Fri, 5 Jun 2015 00:28:32 -0400 Subject: [PATCH 1/3] First pass at FM Stereo improve via pilot tone PLL --- src/demod/DemodDefs.h | 3 +- src/demod/DemodulatorPreThread.cpp | 15 +++++++- src/demod/DemodulatorPreThread.h | 1 + src/demod/DemodulatorThread.cpp | 50 +++++++++++++++++++++++++-- src/demod/DemodulatorWorkerThread.cpp | 9 +++++ src/demod/DemodulatorWorkerThread.h | 3 +- 6 files changed, 76 insertions(+), 5 deletions(-) diff --git a/src/demod/DemodDefs.h b/src/demod/DemodDefs.h index 5f3e3fb..a3db3e0 100644 --- a/src/demod/DemodDefs.h +++ b/src/demod/DemodDefs.h @@ -84,9 +84,10 @@ public: firfilt_rrrf firStereoLeft; firfilt_rrrf firStereoRight; + iirfilt_crcf iirStereoPilot; DemodulatorThreadPostIQData() : - sampleRate(0), audioResampler(NULL), stereoResampler(NULL), audioResampleRatio(0), audioSampleRate(0), firStereoLeft(NULL), firStereoRight(NULL) { + sampleRate(0), audioResampler(NULL), stereoResampler(NULL), audioResampleRatio(0), audioSampleRate(0), firStereoLeft(NULL), firStereoRight(NULL), iirStereoPilot(NULL) { } diff --git a/src/demod/DemodulatorPreThread.cpp b/src/demod/DemodulatorPreThread.cpp index 1dad193..3a0d819 100644 --- a/src/demod/DemodulatorPreThread.cpp +++ b/src/demod/DemodulatorPreThread.cpp @@ -11,7 +11,7 @@ DemodulatorPreThread::DemodulatorPreThread(DemodulatorThreadInputQueue* iqInputQueue, DemodulatorThreadPostInputQueue* iqOutputQueue, DemodulatorThreadControlCommandQueue *threadQueueControl, DemodulatorThreadCommandQueue* threadQueueNotify) : iqInputQueue(iqInputQueue), iqOutputQueue(iqOutputQueue), terminated(false), initialized(false), audioResampler(NULL), stereoResampler(NULL), iqResampleRatio( - 1), audioResampleRatio(1), firStereoRight(NULL), firStereoLeft(NULL), iqResampler(NULL), commandQueue(NULL), threadQueueNotify(threadQueueNotify), threadQueueControl( + 1), audioResampleRatio(1), firStereoRight(NULL), firStereoLeft(NULL), iirStereoPilot(NULL), iqResampler(NULL), commandQueue(NULL), threadQueueNotify(threadQueueNotify), threadQueueControl( threadQueueControl) { freqShifter = nco_crcf_create(LIQUID_VCO); @@ -56,6 +56,14 @@ void DemodulatorPreThread::initialize() { firStereoLeft = firfilt_rrrf_create(h, h_len); firStereoRight = firfilt_rrrf_create(h, h_len); + // stereo pilot filter + unsigned int order = 5; // filter order + float f0 = ((double) 19000 / (double) params.bandwidth); + float fc = f0 + ((double) 3000 / (double) params.bandwidth); + float Ap = 1.0f; + As = 60.0f; + iirStereoPilot = iirfilt_crcf_create_prototype(LIQUID_IIRDES_CHEBY2, LIQUID_IIRDES_BANDPASS, LIQUID_IIRDES_SOS, order, fc, f0, Ap, As); + initialized = true; lastParams = params; } @@ -253,6 +261,7 @@ void DemodulatorPreThread::threadMain() { resamp->stereoResampler = stereoResampler; resamp->firStereoLeft = firStereoLeft; resamp->firStereoRight = firStereoRight; + resamp->iirStereoPilot = iirStereoPilot; resamp->sampleRate = params.bandwidth; iqOutputQueue->push(resamp); @@ -282,6 +291,10 @@ void DemodulatorPreThread::threadMain() { firStereoRight = result.firStereoRight; } + if (result.iirStereoPilot) { + iirStereoPilot = result.iirStereoPilot; + } + if (result.audioResampler) { audioResampler = result.audioResampler; audioResampleRatio = result.audioResamplerRatio; diff --git a/src/demod/DemodulatorPreThread.h b/src/demod/DemodulatorPreThread.h index dde1239..632c4c9 100644 --- a/src/demod/DemodulatorPreThread.h +++ b/src/demod/DemodulatorPreThread.h @@ -60,6 +60,7 @@ protected: firfilt_rrrf firStereoLeft; firfilt_rrrf firStereoRight; + iirfilt_crcf iirStereoPilot; DemodulatorThreadParameters params; DemodulatorThreadParameters lastParams; diff --git a/src/demod/DemodulatorThread.cpp b/src/demod/DemodulatorThread.cpp index 57031e5..9385373 100644 --- a/src/demod/DemodulatorThread.cpp +++ b/src/demod/DemodulatorThread.cpp @@ -45,8 +45,9 @@ void DemodulatorThread::threadMain() { msresamp_rrrf stereoResampler = NULL; firfilt_rrrf firStereoLeft = NULL; firfilt_rrrf firStereoRight = NULL; + iirfilt_crcf iirStereoPilot = NULL; - liquid_float_complex x, y, z[2]; + liquid_float_complex u, v, w, x, y, z[2]; float rz[2]; firhilbf firStereoR2C = firhilbf_create(5, 60.0f); @@ -55,6 +56,10 @@ void DemodulatorThread::threadMain() { nco_crcf stereoShifter = nco_crcf_create(LIQUID_NCO); double stereoShiftFrequency = 0; + nco_crcf stereoPilot = nco_crcf_create(LIQUID_NCO); + nco_crcf_pll_set_bandwidth(stereoPilot, 0.05f); + nco_crcf_reset(stereoPilot); + // half band filter used for side-band elimination resamp2_cccf ssbFilt = resamp2_cccf_create(12,-0.25f,60.0f); @@ -103,6 +108,7 @@ void DemodulatorThread::threadMain() { stereoResampler = inp->stereoResampler; firStereoLeft = inp->firStereoLeft; firStereoRight = inp->firStereoRight; + iirStereoPilot = inp->iirStereoPilot; audioSampleRate = inp->audioSampleRate; } else if (audioResampler != inp->audioResampler) { msresamp_rrrf_destroy(audioResampler); @@ -115,6 +121,7 @@ void DemodulatorThread::threadMain() { ampmodem_reset(demodAM); } freqdem_reset(demodFM); + nco_crcf_reset(stereoPilot); } if (firStereoLeft != inp->firStereoLeft) { @@ -131,6 +138,13 @@ void DemodulatorThread::threadMain() { firStereoRight = inp->firStereoRight; } + if (iirStereoPilot != inp->iirStereoPilot) { + if (iirStereoPilot != NULL) { + iirfilt_crcf_destroy(iirStereoPilot); + } + iirStereoPilot = inp->iirStereoPilot; + } + if (agcData.size() != bufSize) { if (agcData.capacity() < bufSize) { agcData.reserve(bufSize); @@ -229,13 +243,34 @@ void DemodulatorThread::threadMain() { stereoShiftFrequency = freq; } + float phase_error = 0; for (int i = 0; i < bufSize; i++) { firhilbf_r2c_execute(firStereoR2C, demodOutputData[i], &x); nco_crcf_mix_down(stereoShifter, x, &y); nco_crcf_step(stereoShifter); + nco_crcf_step(stereoPilot); + + iirfilt_crcf_execute(iirStereoPilot, x, &v); + nco_crcf_cexpf(stereoPilot, &w); + + u.real = v.real * w.real - v.imag * (-w.imag); + u.imag = v.real * (-w.imag) + v.imag * w.real; + phase_error = atan2f(u.imag,u.real); + + nco_crcf_pll_step(stereoPilot, phase_error); firhilbf_c2r_execute(firStereoC2R, y, &demodStereoData[i]); } - + +// if (fabs(fabs((((nco_crcf_get_frequency(stereoPilot) / (2.0 * M_PI)) * inp->sampleRate)))-19000) > 2000) { +// nco_crcf_reset(stereoPilot); +// nco_crcf_set_frequency(stereoPilot, freq/2); +// } +// + nco_crcf_set_frequency(stereoShifter, nco_crcf_get_frequency(stereoPilot)*2); + nco_crcf_set_phase(stereoShifter, nco_crcf_get_phase(stereoPilot)); + std::cout << "[PLL] phase error: " << phase_error; + std::cout << " freq:" << (((nco_crcf_get_frequency(stereoPilot) / (2.0 * M_PI)) * inp->sampleRate)) << std::endl; + if (audio_out_size != resampledStereoData.size()) { if (resampledStereoData.capacity() < audio_out_size) { resampledStereoData.reserve(audio_out_size); @@ -286,6 +321,11 @@ void DemodulatorThread::threadMain() { firfilt_rrrf_push(firStereoRight, (resampledOutputData[i] + (resampledStereoData[i]))); firfilt_rrrf_execute(firStereoRight, &r); +// firfilt_rrrf_push(firStereoLeft, (resampledStereoData[i])); +// firfilt_rrrf_execute(firStereoLeft, &l); +// +// firfilt_rrrf_push(firStereoRight, (resampledStereoData[i])); +// firfilt_rrrf_execute(firStereoRight, &r); ati->data[i * 2] = l; ati->data[i * 2 + 1] = r; @@ -314,6 +354,8 @@ void DemodulatorThread::threadMain() { int num_vis = DEMOD_VIS_SIZE; if (stereo) { +// ati_vis->channels = 1; +// ati_vis->data.assign(demodStereoData.begin(), demodStereoData.begin() + num_vis); ati_vis->channels = 2; int stereoSize = ati->data.size(); if (stereoSize > DEMOD_VIS_SIZE) { @@ -411,11 +453,15 @@ void DemodulatorThread::threadMain() { if (firStereoRight != NULL) { firfilt_rrrf_destroy(firStereoRight); } + if (iirStereoPilot != NULL) { + iirfilt_crcf_destroy(iirStereoPilot); + } agc_crcf_destroy(iqAutoGain); firhilbf_destroy(firStereoR2C); firhilbf_destroy(firStereoC2R); nco_crcf_destroy(stereoShifter); + nco_crcf_destroy(stereoPilot); resamp2_cccf_destroy(ssbFilt); while (!outputBuffers.empty()) { diff --git a/src/demod/DemodulatorWorkerThread.cpp b/src/demod/DemodulatorWorkerThread.cpp index 99199d1..d9c459c 100644 --- a/src/demod/DemodulatorWorkerThread.cpp +++ b/src/demod/DemodulatorWorkerThread.cpp @@ -69,6 +69,15 @@ void DemodulatorWorkerThread::threadMain() { result.firStereoLeft = firfilt_rrrf_create(h, h_len); result.firStereoRight = firfilt_rrrf_create(h, h_len); + + + unsigned int order = 5; + float f0 = ((double) 19000 / (double) filterCommand.bandwidth); + float fc = f0 + ((double) 3000 / (double) filterCommand.bandwidth); + float Ap = 1.0f; + As = 60.0f; + + result.iirStereoPilot = iirfilt_crcf_create_prototype(LIQUID_IIRDES_CHEBY2, LIQUID_IIRDES_BANDPASS, LIQUID_IIRDES_SOS, order, fc, f0, Ap, As); } if (filterCommand.bandwidth) { diff --git a/src/demod/DemodulatorWorkerThread.h b/src/demod/DemodulatorWorkerThread.h index a14f375..989b048 100644 --- a/src/demod/DemodulatorWorkerThread.h +++ b/src/demod/DemodulatorWorkerThread.h @@ -16,7 +16,7 @@ public: DemodulatorWorkerThreadResult() : cmd(DEMOD_WORKER_THREAD_RESULT_NULL), iqResampler(NULL), iqResampleRatio(0), audioResampler(NULL), stereoResampler(NULL), audioResamplerRatio( - 0), firStereoLeft(NULL), firStereoRight(NULL), sampleRate(0), bandwidth(0), audioSampleRate(0) { + 0), firStereoLeft(NULL), firStereoRight(NULL), iirStereoPilot(NULL), sampleRate(0), bandwidth(0), audioSampleRate(0) { } @@ -35,6 +35,7 @@ public: firfilt_rrrf firStereoLeft; firfilt_rrrf firStereoRight; + iirfilt_crcf iirStereoPilot; long long sampleRate; unsigned int bandwidth; From 0ccd5b415628b1e6acc1580bb2c44647c271d76d Mon Sep 17 00:00:00 2001 From: "Charles J. Cliffe" Date: Fri, 5 Jun 2015 00:35:18 -0400 Subject: [PATCH 2/3] Comment PLL debug --- src/demod/DemodulatorThread.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/demod/DemodulatorThread.cpp b/src/demod/DemodulatorThread.cpp index 9385373..bfcb2de 100644 --- a/src/demod/DemodulatorThread.cpp +++ b/src/demod/DemodulatorThread.cpp @@ -268,8 +268,8 @@ void DemodulatorThread::threadMain() { // nco_crcf_set_frequency(stereoShifter, nco_crcf_get_frequency(stereoPilot)*2); nco_crcf_set_phase(stereoShifter, nco_crcf_get_phase(stereoPilot)); - std::cout << "[PLL] phase error: " << phase_error; - std::cout << " freq:" << (((nco_crcf_get_frequency(stereoPilot) / (2.0 * M_PI)) * inp->sampleRate)) << std::endl; +// std::cout << "[PLL] phase error: " << phase_error; +// std::cout << " freq:" << (((nco_crcf_get_frequency(stereoPilot) / (2.0 * M_PI)) * inp->sampleRate)) << std::endl; if (audio_out_size != resampledStereoData.size()) { if (resampledStereoData.capacity() < audio_out_size) { From 234ac5bd155fe2705ef8826a16c31c732cc48805 Mon Sep 17 00:00:00 2001 From: "Charles J. Cliffe" Date: Fri, 5 Jun 2015 20:58:10 -0400 Subject: [PATCH 3/3] Simplify/fix PLL, tweak parameters --- src/demod/DemodulatorPreThread.cpp | 6 +-- src/demod/DemodulatorThread.cpp | 69 +++++++++++---------------- src/demod/DemodulatorWorkerThread.cpp | 10 ++-- 3 files changed, 36 insertions(+), 49 deletions(-) diff --git a/src/demod/DemodulatorPreThread.cpp b/src/demod/DemodulatorPreThread.cpp index 3a0d819..ac92c65 100644 --- a/src/demod/DemodulatorPreThread.cpp +++ b/src/demod/DemodulatorPreThread.cpp @@ -37,8 +37,8 @@ void DemodulatorPreThread::initialize() { stereoResampler = msresamp_rrrf_create(audioResampleRatio, As); // Stereo filters / shifters - double firStereoCutoff = 0.5 * ((double) 36000 / (double) params.audioSampleRate); // filter cutoff frequency - float ft = 0.05f; // filter transition + double firStereoCutoff = ((double) 16000 / (double) params.audioSampleRate); + float ft = ((double) 1000 / (double) params.audioSampleRate); // filter transition float mu = 0.0f; // fractional timing offset if (firStereoCutoff < 0) { @@ -59,7 +59,7 @@ void DemodulatorPreThread::initialize() { // stereo pilot filter unsigned int order = 5; // filter order float f0 = ((double) 19000 / (double) params.bandwidth); - float fc = f0 + ((double) 3000 / (double) params.bandwidth); + float fc = ((double) 19500 / (double) params.bandwidth); float Ap = 1.0f; As = 60.0f; iirStereoPilot = iirfilt_crcf_create_prototype(LIQUID_IIRDES_CHEBY2, LIQUID_IIRDES_BANDPASS, LIQUID_IIRDES_SOS, order, fc, f0, Ap, As); diff --git a/src/demod/DemodulatorThread.cpp b/src/demod/DemodulatorThread.cpp index bfcb2de..3541b61 100644 --- a/src/demod/DemodulatorThread.cpp +++ b/src/demod/DemodulatorThread.cpp @@ -47,18 +47,14 @@ void DemodulatorThread::threadMain() { firfilt_rrrf firStereoRight = NULL; iirfilt_crcf iirStereoPilot = NULL; - liquid_float_complex u, v, w, x, y, z[2]; - float rz[2]; + liquid_float_complex u, v, w, x, y; firhilbf firStereoR2C = firhilbf_create(5, 60.0f); firhilbf firStereoC2R = firhilbf_create(5, 60.0f); - nco_crcf stereoShifter = nco_crcf_create(LIQUID_NCO); - double stereoShiftFrequency = 0; - - nco_crcf stereoPilot = nco_crcf_create(LIQUID_NCO); - nco_crcf_pll_set_bandwidth(stereoPilot, 0.05f); + nco_crcf stereoPilot = nco_crcf_create(LIQUID_VCO); nco_crcf_reset(stereoPilot); + nco_crcf_pll_set_bandwidth(stereoPilot, 0.25f); // half band filter used for side-band elimination resamp2_cccf ssbFilt = resamp2_cccf_create(12,-0.25f,60.0f); @@ -121,7 +117,6 @@ void DemodulatorThread::threadMain() { ampmodem_reset(demodAM); } freqdem_reset(demodFM); - nco_crcf_reset(stereoPilot); } if (firStereoLeft != inp->firStereoLeft) { @@ -236,38 +231,38 @@ void DemodulatorThread::threadMain() { demodStereoData.resize(bufSize); } - double freq = (2.0 * M_PI) * ((double) 38000) / ((double) inp->sampleRate); - - if (stereoShiftFrequency != freq) { - nco_crcf_set_frequency(stereoShifter, freq); - stereoShiftFrequency = freq; - } - + float phase_error = 0; - for (int i = 0; i < bufSize; i++) { - firhilbf_r2c_execute(firStereoR2C, demodOutputData[i], &x); - nco_crcf_mix_down(stereoShifter, x, &y); - nco_crcf_step(stereoShifter); - nco_crcf_step(stereoPilot); + for (int i = 0; i < bufSize; i++) { + // real -> complex + firhilbf_r2c_execute(firStereoR2C, demodOutputData[i], &x); + + // 19khz pilot band-pass iirfilt_crcf_execute(iirStereoPilot, x, &v); nco_crcf_cexpf(stereoPilot, &w); + + w.imag = -w.imag; // conjf(w) - u.real = v.real * w.real - v.imag * (-w.imag); - u.imag = v.real * (-w.imag) + v.imag * w.real; + // multiply u = v * conjf(w) + u.real = v.real * w.real - v.imag * w.imag; + u.imag = v.real * w.imag + v.imag * w.real; + + // cargf(u) phase_error = atan2f(u.imag,u.real); + // step pll nco_crcf_pll_step(stereoPilot, phase_error); - firhilbf_c2r_execute(firStereoC2R, y, &demodStereoData[i]); + nco_crcf_step(stereoPilot); + + // 38khz down-mix + nco_crcf_mix_down(stereoPilot, x, &y); + nco_crcf_mix_down(stereoPilot, y, &x); + + // complex -> real + firhilbf_c2r_execute(firStereoC2R, x, &demodStereoData[i]); } - -// if (fabs(fabs((((nco_crcf_get_frequency(stereoPilot) / (2.0 * M_PI)) * inp->sampleRate)))-19000) > 2000) { -// nco_crcf_reset(stereoPilot); -// nco_crcf_set_frequency(stereoPilot, freq/2); -// } -// - nco_crcf_set_frequency(stereoShifter, nco_crcf_get_frequency(stereoPilot)*2); - nco_crcf_set_phase(stereoShifter, nco_crcf_get_phase(stereoPilot)); + // std::cout << "[PLL] phase error: " << phase_error; // std::cout << " freq:" << (((nco_crcf_get_frequency(stereoPilot) / (2.0 * M_PI)) * inp->sampleRate)) << std::endl; @@ -316,16 +311,11 @@ void DemodulatorThread::threadMain() { for (int i = 0; i < numAudioWritten; i++) { float l, r; - firfilt_rrrf_push(firStereoLeft, (resampledOutputData[i] - (resampledStereoData[i]))); + firfilt_rrrf_push(firStereoLeft, 0.568 * (resampledOutputData[i] - (resampledStereoData[i]))); firfilt_rrrf_execute(firStereoLeft, &l); - firfilt_rrrf_push(firStereoRight, (resampledOutputData[i] + (resampledStereoData[i]))); + firfilt_rrrf_push(firStereoRight, 0.568 * (resampledOutputData[i] + (resampledStereoData[i]))); firfilt_rrrf_execute(firStereoRight, &r); -// firfilt_rrrf_push(firStereoLeft, (resampledStereoData[i])); -// firfilt_rrrf_execute(firStereoLeft, &l); -// -// firfilt_rrrf_push(firStereoRight, (resampledStereoData[i])); -// firfilt_rrrf_execute(firStereoRight, &r); ati->data[i * 2] = l; ati->data[i * 2 + 1] = r; @@ -354,8 +344,6 @@ void DemodulatorThread::threadMain() { int num_vis = DEMOD_VIS_SIZE; if (stereo) { -// ati_vis->channels = 1; -// ati_vis->data.assign(demodStereoData.begin(), demodStereoData.begin() + num_vis); ati_vis->channels = 2; int stereoSize = ati->data.size(); if (stereoSize > DEMOD_VIS_SIZE) { @@ -460,7 +448,6 @@ void DemodulatorThread::threadMain() { agc_crcf_destroy(iqAutoGain); firhilbf_destroy(firStereoR2C); firhilbf_destroy(firStereoC2R); - nco_crcf_destroy(stereoShifter); nco_crcf_destroy(stereoPilot); resamp2_cccf_destroy(ssbFilt); diff --git a/src/demod/DemodulatorWorkerThread.cpp b/src/demod/DemodulatorWorkerThread.cpp index d9c459c..6e421df 100644 --- a/src/demod/DemodulatorWorkerThread.cpp +++ b/src/demod/DemodulatorWorkerThread.cpp @@ -51,8 +51,8 @@ void DemodulatorWorkerThread::threadMain() { result.audioSampleRate = filterCommand.audioSampleRate; // Stereo filters / shifters - double firStereoCutoff = 0.5 * ((double) 36000 / (double) filterCommand.audioSampleRate); // filter cutoff frequency - float ft = 0.05f; // filter transition + double firStereoCutoff = ((double) 16000 / (double) filterCommand.audioSampleRate); + float ft = ((double) 1000 / (double) filterCommand.audioSampleRate); // filter transition float mu = 0.0f; // fractional timing offset if (firStereoCutoff < 0) { @@ -70,10 +70,10 @@ void DemodulatorWorkerThread::threadMain() { result.firStereoLeft = firfilt_rrrf_create(h, h_len); result.firStereoRight = firfilt_rrrf_create(h, h_len); - - unsigned int order = 5; + // stereo pilot filter + unsigned int order = 5; // filter order float f0 = ((double) 19000 / (double) filterCommand.bandwidth); - float fc = f0 + ((double) 3000 / (double) filterCommand.bandwidth); + float fc = ((double) 19500 / (double) filterCommand.bandwidth); float Ap = 1.0f; As = 60.0f;