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;