comicsansg/CubicSDR
comicsansg
/
CubicSDR
mirror of https://github.com/cjcliffe/CubicSDR.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #87 from cjcliffe/stereo_pll 

Improve FM Stereo
This commit is contained in:
Charles J. Cliffe 2015-06-05 21:34:49 -04:00
parent e7297beaa3 234ac5bd15
commit 64ac186826
6 changed files with 81 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/demod/DemodDefs.h
View File

@ -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) {
}

19
src/demod/DemodulatorPreThread.cpp
View File

@ -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);
@ -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) {
@ -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 = ((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);
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;

1
src/demod/DemodulatorPreThread.h
View File

@ -60,6 +60,7 @@ protected:
firfilt_rrrf firStereoLeft;
firfilt_rrrf firStereoRight;
iirfilt_crcf iirStereoPilot;
DemodulatorThreadParameters params;
DemodulatorThreadParameters lastParams;

65
src/demod/DemodulatorThread.cpp
View File

@ -45,15 +45,16 @@ 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];
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_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);
@ -103,6 +104,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);
@ -131,6 +133,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);
@ -222,20 +231,41 @@ 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++) {
// real -> complex
firhilbf_r2c_execute(firStereoR2C, demodOutputData[i], &x);
nco_crcf_mix_down(stereoShifter, x, &y);
nco_crcf_step(stereoShifter);
firhilbf_c2r_execute(firStereoC2R, y, &demodStereoData[i]);
// 19khz pilot band-pass
iirfilt_crcf_execute(iirStereoPilot, x, &v);
nco_crcf_cexpf(stereoPilot, &w);
w.imag = -w.imag; // conjf(w)
// 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);
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]);
}
// 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);
@ -281,10 +311,10 @@ 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);
ati->data[i * 2] = l;
@ -411,11 +441,14 @@ 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()) {

13
src/demod/DemodulatorWorkerThread.cpp
View File

@ -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) {
@ -69,6 +69,15 @@ void DemodulatorWorkerThread::threadMain() {
result.firStereoLeft = firfilt_rrrf_create(h, h_len);
result.firStereoRight = firfilt_rrrf_create(h, h_len);
// stereo pilot filter
unsigned int order = 5; // filter order
float f0 = ((double) 19000 / (double) filterCommand.bandwidth);
float fc = ((double) 19500 / (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) {

3
src/demod/DemodulatorWorkerThread.h
View File

@ -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;