Merge pull request #20 from cjcliffe/fm-stereo

FM Stereo now supported, toggle by hovering and pressing SPACE for now

src/audio/AudioThread.cpp

@@ -112,7 +112,7 @@ static int audioCallback(void *outputBuffer, void *inputBuffer, unsigned int nBu
                 srcmix->audio_queue_ptr++;
             }
         } else {
-            for (int i = 0, iMax = src->currentInput->channels * nBufferFrames; i < iMax; i++) {
+            for (int i = 0, iMax = srcmix->currentInput->channels * nBufferFrames; i < iMax; i++) {
                 if (srcmix->audio_queue_ptr >= srcmix->currentInput->data.size()) {
                     if (srcmix->currentInput) {
                         srcmix->currentInput->decRefCount();

src/demod/DemodDefs.h

@@ -70,13 +70,15 @@ public:
 class DemodulatorThreadPostIQData: public ReferenceCounter {
 public:
     std::vector<liquid_float_complex> data;
+    int bandwidth;
     float audio_resample_ratio;
     msresamp_rrrf audio_resampler;
+    msresamp_rrrf stereo_resampler;
     float resample_ratio;
     msresamp_crcf resampler;
 
     DemodulatorThreadPostIQData() :
-            audio_resample_ratio(0), audio_resampler(NULL), resample_ratio(0), resampler(NULL) {
+            bandwidth(0), audio_resample_ratio(0), audio_resampler(NULL), stereo_resampler(NULL), resample_ratio(0), resampler(NULL) {
 
     }
 

src/demod/DemodulatorInstance.cpp

@@ -146,6 +146,16 @@ void DemodulatorInstance::setActive(bool state) {
     audioThread->setActive(state);
 }
 
+bool DemodulatorInstance::isStereo() {
+    return stereo;
+}
+
+void DemodulatorInstance::setStereo(bool state) {
+    stereo = state;
+    demodulatorThread->setStereo(state);
+}
+
+
 void DemodulatorInstance::squelchAuto() {
     DemodulatorThreadControlCommand command;
     command.cmd = DemodulatorThreadControlCommand::DEMOD_THREAD_CMD_CTL_SQUELCH_AUTO;

src/demod/DemodulatorInstance.h

@@ -49,6 +49,9 @@ public:
     bool isActive();
     void setActive(bool state);
 
+    bool isStereo();
+    void setStereo(bool state);
+
     void squelchAuto();bool isSquelchEnabled();
     void setSquelchEnabled(bool state);
 
@@ -56,5 +59,6 @@ private:
     std::atomic<std::string *> label;bool terminated;bool demodTerminated;bool audioTerminated;bool preDemodTerminated;
     std::atomic<bool> active;
     std::atomic<bool> squelch;
+    std::atomic<bool> stereo;
 };
 

src/demod/DemodulatorPreThread.cpp

@@ -9,7 +9,7 @@
 
 DemodulatorPreThread::DemodulatorPreThread(DemodulatorThreadInputQueue* pQueueIn, DemodulatorThreadPostInputQueue* pQueueOut,
         DemodulatorThreadControlCommandQueue *threadQueueControl, DemodulatorThreadCommandQueue* threadQueueNotify) :
-        inputQueue(pQueueIn), postInputQueue(pQueueOut), terminated(false), initialized(false), audio_resampler(NULL), resample_ratio(1), audio_resample_ratio(
+        inputQueue(pQueueIn), postInputQueue(pQueueOut), terminated(false), initialized(false), audio_resampler(NULL), stereo_resampler(NULL), resample_ratio(1), audio_resample_ratio(
                 1), resampler(NULL), commandQueue(NULL), fir_filter(NULL), audioInputQueue(NULL), threadQueueNotify(threadQueueNotify), threadQueueControl(
                 threadQueueControl) {
 
@@ -71,6 +71,12 @@ void DemodulatorPreThread::initialize() {
     audio_resampler = msresamp_rrrf_create(audio_resample_ratio, As);
 //    msresamp_crcf_print(audio_resampler);
 
+    if (stereo_resampler) {
+        msresamp_rrrf_destroy(stereo_resampler);
+    }
+    stereo_resampler = msresamp_rrrf_create(audio_resample_ratio, As);
+
+
     initialized = true;
 //    std::cout << "inputResampleRate " << params.bandwidth << std::endl;
 
@@ -217,8 +223,10 @@ void DemodulatorPreThread::threadMain() {
 
             resamp->audio_resample_ratio = audio_resample_ratio;
             resamp->audio_resampler = audio_resampler;
+            resamp->stereo_resampler = stereo_resampler;
             resamp->resample_ratio = resample_ratio;
             resamp->resampler = resampler;
+            resamp->bandwidth = params.bandwidth;
 
             postInputQueue->push(resamp);
         }
@@ -239,6 +247,7 @@ void DemodulatorPreThread::threadMain() {
                     fir_filter = result.fir_filter;
                     resampler = result.resampler;
                     audio_resampler = result.audio_resampler;
+                    stereo_resampler = result.stereo_resampler;
 
                     resample_ratio = result.resample_ratio;
                     audio_resample_ratio = result.audio_resample_ratio;

src/demod/DemodulatorPreThread.h

@@ -57,6 +57,7 @@ protected:
     float resample_ratio;
 
     msresamp_rrrf audio_resampler;
+    msresamp_rrrf stereo_resampler;
     float audio_resample_ratio;
 
     DemodulatorThreadParameters params;

src/demod/DemodulatorThread.cpp

@@ -8,8 +8,8 @@
 
 DemodulatorThread::DemodulatorThread(DemodulatorThreadPostInputQueue* pQueue, DemodulatorThreadControlCommandQueue *threadQueueControl,
         DemodulatorThreadCommandQueue* threadQueueNotify) :
-        postInputQueue(pQueue), visOutQueue(NULL), audioInputQueue(NULL), agc(NULL), terminated(false), threadQueueNotify(threadQueueNotify), threadQueueControl(
+        postInputQueue(pQueue), visOutQueue(NULL), audioInputQueue(NULL), agc(NULL), stereo(false), terminated(false), threadQueueNotify(
-                threadQueueControl), squelch_level(0), squelch_tolerance(0), squelch_enabled(false) {
+                threadQueueNotify), threadQueueControl(threadQueueControl), squelch_level(0), squelch_tolerance(0), squelch_enabled(false) {
 
     float kf = 0.5;         // modulation factor
     fdem = freqdem_create(kf);
@@ -31,6 +31,7 @@ void DemodulatorThread::threadMain() {
 #endif
 
     msresamp_rrrf audio_resampler = NULL;
+    msresamp_rrrf stereo_resampler = NULL;
     msresamp_crcf resampler = NULL;
 
     agc = agc_crcf_create();
@@ -44,7 +45,11 @@ void DemodulatorThread::threadMain() {
     std::vector<liquid_float_complex> resampled_data;
     std::vector<liquid_float_complex> agc_data;
     std::vector<float> demod_output;
+    std::vector<float> demod_output_stereo;
     std::vector<float> resampled_audio_output;
+    std::vector<float> resampled_audio_output_stereo;
+
+    double freq_index = 0;
 
     while (!terminated) {
         DemodulatorThreadPostIQData *inp;
@@ -61,11 +66,14 @@ void DemodulatorThread::threadMain() {
         if (resampler == NULL) {
             resampler = inp->resampler;
             audio_resampler = inp->audio_resampler;
+            stereo_resampler = inp->stereo_resampler;
         } else if (resampler != inp->resampler) {
             msresamp_crcf_destroy(resampler);
             msresamp_rrrf_destroy(audio_resampler);
+            msresamp_rrrf_destroy(stereo_resampler);
             resampler = inp->resampler;
             audio_resampler = inp->audio_resampler;
+            stereo_resampler = inp->stereo_resampler;
         }
 
         int out_size = ceil((float) (bufSize) * inp->resample_ratio);
@@ -89,24 +97,49 @@ void DemodulatorThread::threadMain() {
         if (demod_output.size() != num_written) {
             if (demod_output.capacity() < num_written) {
                 demod_output.reserve(num_written);
+                demod_output_stereo.reserve(num_written);
             }
             demod_output.resize(num_written);
+            demod_output_stereo.resize(num_written);
         }
 
         freqdem_demodulate_block(fdem, &agc_data[0], num_written, &demod_output[0]);
 
+        if (stereo) {
+            int shift_freq = 38000 - inp->bandwidth;
+            double freq = (2.0 * M_PI) * (((double) abs(shift_freq)) / ((double) inp->bandwidth));
+
+            for (int i = 0; i < num_written; i++) {
+                freq_index += freq;
+
+                demod_output_stereo[i] = demod_output[i] * sin(freq_index); // + demod_output[i] * cos(freq_index);
+                while (freq_index > (M_PI * 2.0)) {
+                    freq_index -= (M_PI * 2.0);
+                }
+                while (freq_index < (M_PI * 2.0)) {
+                    freq_index += (M_PI * 2.0);
+                }
+            }
+        }
+
         int audio_out_size = ceil((float) (num_written) * audio_resample_ratio);
 
         if (audio_out_size != resampled_audio_output.size()) {
             if (resampled_audio_output.capacity() < audio_out_size) {
                 resampled_audio_output.reserve(audio_out_size);
+                resampled_audio_output_stereo.reserve(audio_out_size);
             }
             resampled_audio_output.resize(audio_out_size);
+            resampled_audio_output_stereo.resize(audio_out_size);
         }
 
         unsigned int num_audio_written;
         msresamp_rrrf_execute(audio_resampler, &demod_output[0], num_written, &resampled_audio_output[0], &num_audio_written);
 
+        if (stereo) {
+            msresamp_rrrf_execute(stereo_resampler, &demod_output_stereo[0], num_written, &resampled_audio_output_stereo[0], &num_audio_written);
+        }
+
         if (audioInputQueue != NULL) {
             if (!squelch_enabled || ((agc_crcf_get_signal_level(agc)) >= 0.1)) {
                 AudioThreadInput *ati = NULL;
@@ -124,8 +157,18 @@ void DemodulatorThread::threadMain() {
                 }
 
                 ati->setRefCount(1);
+
+                if (stereo) {
+                    ati->channels = 2;
+                    ati->data.resize(num_audio_written * 2);
+                    for (int i = 0; i < num_audio_written; i++) {
+                        ati->data[i * 2] = (resampled_audio_output[i] - (resampled_audio_output_stereo[i]));
+                        ati->data[i * 2 + 1] = (resampled_audio_output[i] + (resampled_audio_output_stereo[i]));
+                    }
+                } else {
                     ati->channels = 1;
                     ati->data.assign(resampled_audio_output.begin(), resampled_audio_output.begin() + num_audio_written);
+                }
 
                 audioInputQueue->push(ati);
             }
@@ -136,7 +179,21 @@ void DemodulatorThread::threadMain() {
             ati_vis->channels = 1;
 
             int num_vis = DEMOD_VIS_SIZE;
+            if (stereo) {
+
+                int stereoSize = resampled_audio_output.size();
+                if (stereoSize > DEMOD_VIS_SIZE) {
+                    stereoSize = DEMOD_VIS_SIZE;
+                }
+                ati_vis->data.resize(stereoSize);
+
+                for (int i = 0; i < stereoSize / 2; i++) {
+                    ati_vis->data[i] = (resampled_audio_output[i] - (resampled_audio_output_stereo[i]));
+                    ati_vis->data[i + stereoSize / 2] = (resampled_audio_output[i] + (resampled_audio_output_stereo[i]));
+                }
+            } else {
                 if (num_audio_written > num_written) {
+
                     if (num_vis > num_audio_written) {
                         num_vis = num_audio_written;
                     }
@@ -148,10 +205,11 @@ void DemodulatorThread::threadMain() {
                     ati_vis->data.assign(demod_output.begin(), demod_output.begin() + num_vis);
                 }
 
-            visOutQueue->push(ati_vis);
 //            std::cout << "Signal: " << agc_crcf_get_signal_level(agc) << " -- " << agc_crcf_get_rssi(agc) << "dB " << std::endl;
             }
 
+            visOutQueue->push(ati_vis);
+        }
         if (!threadQueueControl->empty()) {
             while (!threadQueueControl->empty()) {
                 DemodulatorThreadControlCommand command;
@@ -183,6 +241,9 @@ void DemodulatorThread::threadMain() {
     if (audio_resampler != NULL) {
         msresamp_rrrf_destroy(audio_resampler);
     }
+    if (stereo_resampler != NULL) {
+        msresamp_rrrf_destroy(stereo_resampler);
+    }
 
     agc_crcf_destroy(agc);
 
@@ -204,3 +265,12 @@ void DemodulatorThread::terminate() {
     DemodulatorThreadPostIQData *inp = new DemodulatorThreadPostIQData;    // push dummy to nudge queue
     postInputQueue->push(inp);
 }
+
+void DemodulatorThread::setStereo(bool state) {
+    stereo = state;
+    std::cout << "Stereo " << (state ? "Enabled" : "Disabled") << std::endl;
+}
+
+bool DemodulatorThread::isStereo() {
+    return stereo;
+}

src/demod/DemodulatorThread.h

@@ -35,6 +35,9 @@ public:
 
     void terminate();
 
+    void setStereo(bool state);
+    bool isStereo();
+
 #ifdef __APPLE__
     static void *pthread_helper(void *context) {
         return ((DemodulatorThread *) context)->threadMain();
@@ -49,6 +52,7 @@ protected:
     freqdem fdem;
     agc_crcf agc;
 
+    std::atomic<bool> stereo;
     std::atomic<bool> terminated;
 
     DemodulatorThreadCommandQueue* threadQueueNotify;

src/demod/DemodulatorWorkerThread.cpp

@@ -59,6 +59,7 @@ void DemodulatorWorkerThread::threadMain() {
             result.fir_filter = firfilt_crcf_create(h, h_len);
             result.resampler = msresamp_crcf_create(result.resample_ratio, As);
             result.audio_resampler = msresamp_rrrf_create(result.audio_resample_ratio, As);
+            result.stereo_resampler = msresamp_rrrf_create(result.audio_resample_ratio, As);
 
             result.audioSampleRate = filterCommand.audioSampleRate;
             result.bandwidth = filterCommand.bandwidth;

src/demod/DemodulatorWorkerThread.h

@@ -22,13 +22,13 @@ public:
     };
 
     DemodulatorWorkerThreadResult() :
-            cmd(DEMOD_WORKER_THREAD_RESULT_NULL), fir_filter(NULL), resampler(NULL), resample_ratio(0), audio_resampler(NULL), audio_resample_ratio(
+            cmd(DEMOD_WORKER_THREAD_RESULT_NULL), fir_filter(NULL), resampler(NULL), resample_ratio(0), audio_resampler(NULL), stereo_resampler(NULL), audio_resample_ratio(
                     0), inputRate(0), bandwidth(0), audioSampleRate(0) {
 
     }
 
     DemodulatorWorkerThreadResult(DemodulatorThreadResultEnum cmd) :
-            cmd(cmd), fir_filter(NULL), resampler(NULL), resample_ratio(0), audio_resampler(NULL), audio_resample_ratio(0), inputRate(0), bandwidth(
+            cmd(cmd), fir_filter(NULL), resampler(NULL), resample_ratio(0), audio_resampler(NULL), stereo_resampler(NULL), audio_resample_ratio(0), inputRate(0), bandwidth(
                     0), audioSampleRate(0) {
 
     }
@@ -39,6 +39,7 @@ public:
     msresamp_crcf resampler;
     float resample_ratio;
     msresamp_rrrf audio_resampler;
+    msresamp_rrrf stereo_resampler;
     float audio_resample_ratio;
 
     unsigned int inputRate;

src/visual/WaterfallCanvas.cpp

@@ -200,6 +200,16 @@ void WaterfallCanvas::OnKeyDown(wxKeyEvent& event) {
             activeDemod->squelchAuto();
         }
         break;
+    case WXK_SPACE:
+          if (!activeDemod) {
+              break;
+          }
+          if (activeDemod->isStereo()) {
+              activeDemod->setStereo(false);
+          } else {
+              activeDemod->setStereo(true);
+          }
+          break;
     default:
         event.Skip();
         return;