Merge pull request #87 from cjcliffe/stereo_pll

Improve FM Stereo

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

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);
@@ -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
+    double firStereoCutoff = ((double) 16000 / (double) params.audioSampleRate);
-    float ft = 0.05f;         // filter transition
+    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;

src/demod/DemodulatorPreThread.h

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

src/demod/DemodulatorThread.cpp

@@ -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];
+    liquid_float_complex u, v, w, x, y;
-    float rz[2];
 
     firhilbf firStereoR2C = firhilbf_create(5, 60.0f);
     firhilbf firStereoC2R = firhilbf_create(5, 60.0f);
 
-    nco_crcf stereoShifter = nco_crcf_create(LIQUID_NCO);
+    nco_crcf stereoPilot = nco_crcf_create(LIQUID_VCO);
-    double stereoShiftFrequency = 0;
+    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);
+            
-
+            float phase_error = 0;
-            if (stereoShiftFrequency != freq) {
-                nco_crcf_set_frequency(stereoShifter, freq);
-                stereoShiftFrequency = freq;
-            }
 
             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);
+                // 19khz pilot band-pass
-                firhilbf_c2r_execute(firStereoC2R, y, &demodStereoData[i]);
+                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()) {

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
+                double firStereoCutoff = ((double) 16000 / (double) filterCommand.audioSampleRate);
-                float ft = 0.05f;         // filter transition
+                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) {

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;