Stereo decoding works. WHEEE!!

FmDecode.cc

@@ -274,10 +274,13 @@ FmDecoder::FmDecoder(double sample_rate_if,
 
     // Construct HighPassFilterIir
     , m_dcblock_mono(30.0 / sample_rate_pcm)
+    , m_dcblock_stereo(30.0 / sample_rate_pcm)
 
     // Construct LowwPassFilterRC
-    , m_deemph_mono((deemphasis == 0) ?
+    , m_deemph_mono(
-                    1.0 : (deemphasis * sample_rate_pcm * 1.0e-6))
+        (deemphasis == 0) ? 1.0 : (deemphasis * sample_rate_pcm * 1.0e-6))
+    , m_deemph_stereo(
+        (deemphasis == 0) ? 1.0 : (deemphasis * sample_rate_pcm * 1.0e-6))
 
 {
     // nothing more to do
@@ -325,22 +328,37 @@ void FmDecoder::process(const IQSampleVector& samples_in,
         m_pilotpll.process(m_buf_baseband, m_buf_rawstereo);
         m_stereo_detected = m_pilotpll.locked();
 
-        if (m_stereo_enabled) {
+        // Demodulate stereo signal.
+        demod_stereo(m_buf_baseband, m_buf_rawstereo);
 
-            // Demodulate stereo signal.
+        // Extract audio and downsample.
-            demod_stereo(m_buf_baseband, m_buf_rawstereo);
+        // NOTE: This MUST be done even if no stereo signal is detected yet,
+        // because the downsamplers for mono and stereo signal must be
+        // kept in sync.
+        m_resample_stereo.process(m_buf_rawstereo, m_buf_stereo);
 
-            // Extract audio and downsample.
+        // DC blocking and de-emphasis.
-            m_resample_stereo.process(m_buf_rawstereo, m_buf_stereo);
+        m_dcblock_stereo.process_inplace(m_buf_stereo);
+        m_deemph_stereo.process_inplace(m_buf_stereo);
 
-// TODO : filters
+        if (m_stereo_detected) {
+
+            // Extract left/right channels from mono/stereo signals.
+            stereo_to_left_right(m_buf_mono, m_buf_stereo, audio);
+
+        } else {
+
+            // Duplicate mono signal in left/right channels.
+            mono_to_left_right(m_buf_mono, audio);
 
         }
+
+    } else {
+
+        // Just return mono channel.
+        audio = move(m_buf_mono);
+
     }
-
-
-// TODO : stereo mixing
-    audio = move(m_buf_mono);
 }
 
 
@@ -360,4 +378,37 @@ void FmDecoder::demod_stereo(const SampleVector& samples_baseband,
     }
 }
 
+
+// Duplicate mono signal in left/right channels.
+void FmDecoder::mono_to_left_right(const SampleVector& samples_mono,
+                                   SampleVector& audio)
+{
+    unsigned int n = samples_mono.size();
+
+    audio.resize(2*n);
+    for (unsigned int i = 0; i < n; i++) {
+        Sample m = samples_mono[i];
+        audio[2*i]   = m;
+        audio[2*i+1] = m;
+    }
+}
+
+
+// Extract left/right channels from mono/stereo signals.
+void FmDecoder::stereo_to_left_right(const SampleVector& samples_mono,
+                                     const SampleVector& samples_stereo,
+                                     SampleVector& audio)
+{
+    unsigned int n = samples_mono.size();
+    assert(n == samples_stereo.size());
+
+    audio.resize(2*n);
+    for (unsigned int i = 0; i < n; i++) {
+        Sample m = samples_mono[i];
+        Sample s = samples_stereo[i];
+        audio[2*i]   = m + s;
+        audio[2*i+1] = m - s;
+    }
+}
+
 /* end */

FmDecode.h

@@ -168,6 +168,15 @@ private:
     void demod_stereo(const SampleVector& samples_baseband,
                       SampleVector& samples_stereo);
 
+    /** Duplicate mono signal in left/right channels. */
+    void mono_to_left_right(const SampleVector& samples_mono,
+                            SampleVector& audio);
+
+    /** Extract left/right channels from mono/stereo signals. */
+    void stereo_to_left_right(const SampleVector& samples_mono,
+                              const SampleVector& samples_stereo,
+                              SampleVector& audio);
+
     // Data members.
     const double    m_sample_rate_if;
     const double    m_sample_rate_baseband;
@@ -196,7 +205,9 @@ private:
     DownsampleFilter    m_resample_mono;
     DownsampleFilter    m_resample_stereo;
     HighPassFilterIir   m_dcblock_mono;
+    HighPassFilterIir   m_dcblock_stereo;
     LowPassFilterRC     m_deemph_mono;
+    LowPassFilterRC     m_deemph_stereo;
 };
 
 #endif