First pass at FM Stereo improve via pilot tone PLL

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

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

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

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;