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

First pass at FM Stereo improve via pilot tone PLL

This commit is contained in:
Charles J. Cliffe 2015-06-05 00:28:32 -04:00
parent e7297beaa3
commit 0e80e1009e
6 changed files with 76 additions and 5 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) {
}

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

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;

50
src/demod/DemodulatorThread.cpp
View File

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

9
src/demod/DemodulatorWorkerThread.cpp
View File

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

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;