Stereo pilot PLL now working. (Stereo decoding not yet implemented.)

FmDecode.cc

@@ -1,4 +1,5 @@
 
+#include <cassert>
 #include <cmath>
 
 #include "FmDecode.h"
@@ -88,12 +89,13 @@ PilotPhaseLock::PilotPhaseLock(double freq, double bandwidth, double minsignal)
      * This is a type-2, 4th order phase-locked loop.
      *
      * Open-loop transfer function:
-     *   G(z) = K * (z - Qz) / ((z - Qp) * (z - Qp) * (z - 1) * (z - 1))
+     *   G(z) = K * (z - q1) / ((z - p1) * (z - p2) * (z - 1) * (z - 1))
-     *   K  = 3.125 * (bandwidth * 2 * PI)**3
+     *   K  = 3.788 * (bandwidth * 2 * Pi)**3
-     *   Qz = exp(-0.2 * bandwidth * 2*PI)
+     *   q1 = exp(-0.1153 * bandwidth * 2*Pi)
-     *   Qp = exp(-2.5 * bandwidth * 2*PI)
+     *   p1 = exp(-1.146 * bandwidth * 2*Pi)
+     *   p2 = exp(-5.331 * bandwidth * 2*Pi)
      *
-     * I don't understand what I'm doing; hopefully it just works.
+     * I don't understand what I'm doing; hopefully it will work.
      */
 
     // Set min/max locking frequencies.
@@ -104,24 +106,26 @@ PilotPhaseLock::PilotPhaseLock(double freq, double bandwidth, double minsignal)
     m_minsignal  = minsignal;
     m_lock_delay = int(10.0 / bandwidth);
     m_lock_cnt   = 0;
+    m_pilot_level = 0;
 
     // Create 2nd order filter for I/Q representation of phase error.
-    // Filter has both poles at z = exp(-2.5 * bandwidth * 2*PI).
+    // Filter has two poles, unit DC gain.
-    double t = exp(-2.5 * bandwidth * 2.0 * M_PI);
+    double p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
-    m_phasor_a1 = -2.0 * t;
+    double p2 = exp(-5.331 * bandwidth * 2.0 * M_PI);
-    m_phasor_a2 = t * t;
+    m_phasor_a1 = - p1 - p2;
+    m_phasor_a2 = p1 * p2;
     m_phasor_b0 = 1 + m_phasor_a1 + m_phasor_a2;
 
     // Create loop filter to stabilize the loop.
-    // Filter has one Zero at z = exp(-0.2 * bandwidth * 2*PI).
+    double q1 = exp(-0.1153 * bandwidth * 2.0 * M_PI);
-    m_loopfilter_b0 = 0.5 * bandwidth * 2.0 * M_PI;
+    m_loopfilter_b0 = 0.62 * bandwidth * 2.0 * M_PI;
-    m_loopfilter_b1 = - m_loopfilter_b0 * exp(-0.2 * bandwidth * 2.0 * M_PI);
+    m_loopfilter_b1 = - m_loopfilter_b0 * q1;
 
     // After the loop filter, the phase error is integrated to produce
     // the frequency. Then the frequency is integrated to produce the phase.
-    // These two integrators form the two remaining poles, both at z = 1.
+    // These integrators form the two remaining poles, both at z = 1.
 
-    // Reset frequency and phase.
+    // Initialize frequency and phase.
     m_freq  = freq * 2.0 * M_PI;
     m_phase = 0;
 
@@ -129,6 +133,7 @@ PilotPhaseLock::PilotPhaseLock(double freq, double bandwidth, double minsignal)
     m_phasor_i2 = 0;
     m_phasor_q1 = 0;
     m_phasor_q2 = 0;
+    m_loopfilter_x1 = 0;
 }
 
 
@@ -140,17 +145,23 @@ void PilotPhaseLock::process(const SampleVector& samples_in,
 
     samples_out.resize(n);
 
+    if (n > 0)
+        m_pilot_level = 1000.0;
+
     for (unsigned int i = 0; i < n; i++) {
 
         // Generate locked pilot tone.
         Sample psin = sin(m_phase);
         Sample pcos = cos(m_phase);
-        samples_out[i] = pcos;
+
+        // Generate double-frequency output.
+        // sin(2*x) = 2 * sin(x) * cos(x)
+        samples_out[i] = 2 * psin * pcos;
 
         // Multiply locked tone with input.
         Sample x = samples_in[i];
-        Sample phasor_i = pcos * x;
+        Sample phasor_i = psin * x;
-        Sample phasor_q = psin * x;
+        Sample phasor_q = pcos * x;
 
         // Run IQ phase error through low-pass filter.
         phasor_i = m_phasor_b0 * phasor_i
@@ -171,22 +182,18 @@ void PilotPhaseLock::process(const SampleVector& samples_in,
             // Use simple linear approximation of arctan.
             phase_err = phasor_q / phasor_i;
         } else if (phasor_q > 0) {
-            // We are more than 45 degrees ahead of the input.
+            // We are lagging more than 45 degrees behind the input.
             phase_err = 1;
         } else {
-            // We are lagging more than 45 degrees behind the input.
+            // We are more than 45 degrees ahead of the input.
             phase_err = -1;
         }
 
-        // Detect signal threshold.
+        // Detect pilot level (conservative).
-        if (phasor_i > m_minsignal) {
+        m_pilot_level = min(m_pilot_level, phasor_i);
-            m_lock_cnt++;
-        } else {
-            m_lock_cnt = 0;
-        }
 
         // Run phase error through loop filter and update frequency estimate.
-        m_freq -= m_loopfilter_b0 * phase_err
+        m_freq += m_loopfilter_b0 * phase_err
                   + m_loopfilter_b1 * m_loopfilter_x1;
         m_loopfilter_x1 = phase_err;
 
@@ -195,13 +202,17 @@ void PilotPhaseLock::process(const SampleVector& samples_in,
 
         // Update locked phase.
         m_phase += m_freq;
-        if (m_phase < -2.0 * M_PI)
+        if (m_phase > 2.0 * M_PI)
-            m_phase += 2.0 * M_PI;
-        else if (m_phase > 2.0 * M_PI)
             m_phase -= 2.0 * M_PI;
     }
 
-    m_lock_cnt = min(m_lock_delay, m_lock_cnt);
+    // Update lock status.
+    if (2 * m_pilot_level > m_minsignal) {
+        if (m_lock_cnt < m_lock_delay)
+            m_lock_cnt += n;
+    } else {
+        m_lock_cnt = 0;
+    }
 }
 
 
@@ -241,10 +252,24 @@ FmDecoder::FmDecoder(double sample_rate_if,
     // Construct DownsampleFilter for baseband
     , m_resample_baseband(8 * downsample, 0.4 / downsample, downsample, true)
 
+    // Construct PilotPhaseLock
+    , m_pilotpll(19000 * downsample / sample_rate_if,       // freq
+                 50 * downsample / sample_rate_if,          // bandwidth
+                 0.04)                                      // minsignal
+
     // Construct DownsampleFilter for mono channel
-    , m_resample_mono(int(sample_rate_if / downsample / 1000.0),
+    , m_resample_mono(
-                      bandwidth_pcm * downsample / sample_rate_if,
+        int(sample_rate_if / downsample / 1000.0),          // filter_order
-                      sample_rate_if / downsample / sample_rate_pcm, false)
+        bandwidth_pcm * downsample / sample_rate_if,        // cutoff
+        sample_rate_if / downsample / sample_rate_pcm,      // downsample
+        false)                                              // integer_factor
+
+    // Construct DownsampleFilter for stereo channel
+    , m_resample_stereo(
+        int(sample_rate_if / downsample / 1000.0),          // filter_order
+        bandwidth_pcm * downsample / sample_rate_if,        // cutoff
+        sample_rate_if / downsample / sample_rate_pcm,      // downsample
+        false)                                              // integer_factor
 
     // Construct HighPassFilterIir
     , m_dcblock_mono(30.0 / sample_rate_pcm)
@@ -285,8 +310,6 @@ void FmDecoder::process(const IQSampleVector& samples_in,
     m_baseband_mean  = 0.95 * m_baseband_mean + 0.05 * baseband_mean;
     m_baseband_level = 0.95 * m_baseband_level + 0.05 * baseband_rms;
 
-// TODO : stereo decoding
-
     // Extract mono audio signal.
     m_resample_mono.process(m_buf_baseband, m_buf_mono);
 
@@ -294,8 +317,45 @@ void FmDecoder::process(const IQSampleVector& samples_in,
     m_dcblock_mono.processInPlace(m_buf_mono);
     m_deemph_mono.processInPlace(m_buf_mono);
 
+    if (m_stereo_enabled) {
+
+        // Lock on stereo pilot.
+        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);
+
+            // Extract audio and downsample.
+            m_resample_stereo.process(m_buf_rawstereo, m_buf_stereo);
+
+// TODO : filters
+
+        }
+    }
+
+
 // TODO : stereo mixing
     audio = move(m_buf_mono);
 }
 
+
+// Demodulate stereo L-R signal.
+void FmDecoder::demod_stereo(const SampleVector& samples_baseband,
+                             SampleVector& samples_rawstereo)
+{
+    // Just multiply the baseband signal with the double-frequency pilot.
+    // And multiply by two to get the full amplitude.
+    // That's all.
+
+    unsigned int n = samples_baseband.size();
+    assert(n == samples_rawstereo.size());
+
+    for (unsigned int i = 0; i < n; i++) {
+        samples_rawstereo[i] *= 2 * samples_baseband[i];
+    }
+}
+
 /* end */

FmDecode.h

@@ -36,17 +36,22 @@ class PilotPhaseLock
 {
 public:
 
+// TODO : pulse-per-second
+
     /**
      * Construct phase-locked loop.
      *
-     * freq       :: center frequency of capture range relative to sample freq
+     * freq       :: 19 kHz center frequency relative to sample freq
      *               (0.5 is Nyquist)
-     * bandwidth  :: approximate bandwidth relative to sample frequency
+     * bandwidth  :: bandwidth relative to sample frequency
      * minsignal  :: minimum pilot amplitude
      */
     PilotPhaseLock(double freq, double bandwidth, double minsignal);
 
-    /** Process samples and extract pilot tone at unit amplitude. */
+    /**
+     * Process samples and extract 19 kHz pilot tone.
+     * Generate phase-locked 38 kHz tone with unit amplitude.
+     */
     void process(const SampleVector& samples_in, SampleVector& samples_out);
 
     /** Return true if the phase-locked loop is locked. */
@@ -55,6 +60,12 @@ public:
         return m_lock_cnt >= m_lock_delay;
     }
 
+    /** Return detected amplitude of pilot signal. */
+    double get_pilot_level() const
+    {
+        return 2 * m_pilot_level;
+    }
+
 private:
     Sample  m_minfreq, m_maxfreq;
     Sample  m_phasor_b0, m_phasor_a1, m_phasor_a2;
@@ -63,6 +74,7 @@ private:
     Sample  m_loopfilter_x1;
     Sample  m_freq, m_phase;
     Sample  m_minsignal;
+    Sample  m_pilot_level;
     int     m_lock_delay;
     int     m_lock_cnt;
 };
@@ -144,9 +156,18 @@ public:
         return m_baseband_level;
     }
 
-// TODO : stuff for stereo pilot locking
+    /** Return amplitude of stereo pilot (nominal level is 0.1). */
+    double get_pilot_level() const
+    {
+        return m_pilotpll.get_pilot_level();
+    }
 
 private:
+    /** Demodulate stereo L-R signal. */
+    void demod_stereo(const SampleVector& samples_baseband,
+                      SampleVector& samples_stereo);
+
+    // Data members.
     const double    m_sample_rate_if;
     const int       m_tuning_table_size;
     const int       m_tuning_shift;
@@ -162,12 +183,16 @@ private:
     IQSampleVector  m_buf_iffiltered;
     SampleVector    m_buf_baseband;
     SampleVector    m_buf_mono;
+    SampleVector    m_buf_rawstereo;
+    SampleVector    m_buf_stereo;
 
     FineTuner           m_finetuner;
     LowPassFilterFirIQ  m_iffilter;
     PhaseDiscriminator  m_phasedisc;
     DownsampleFilter    m_resample_baseband;
+    PilotPhaseLock      m_pilotpll;
     DownsampleFilter    m_resample_mono;
+    DownsampleFilter    m_resample_stereo;
     HighPassFilterIir   m_dcblock_mono;
     LowPassFilterRC     m_deemph_mono;
 };

main.cc

@@ -451,6 +451,7 @@ int main(int argc, char **argv)
     SampleVector audiosamples;
     bool inbuf_length_warning = false;
     double audio_level = 0;
+    bool got_stereo = false;
 
     // Main loop.
     for (unsigned int block = 0; !stop_flag.load(); block++) {
@@ -480,8 +481,6 @@ int main(int argc, char **argv)
 
 // TODO : investigate I/Q imbalance to fix Radio4 noise
 
-// TODO : show mono/stereo (switching)
-
         fprintf(stderr,
                 "\rblk=%6d  freq=%8.4fMHz  IF=%+5.1fdB  BB=%+5.1fdB  audio=%+5.1fdB ",
                 block,
@@ -498,6 +497,15 @@ int main(int argc, char **argv)
         }
         fflush(stderr);
 
+        if (fm.stereo_detected() != got_stereo) {
+            got_stereo = fm.stereo_detected();
+            if (got_stereo)
+                fprintf(stderr, "\ngot stereo signal (pilot level = %f)\n",
+                        fm.get_pilot_level());
+            else
+                fprintf(stderr, "\nlost stereo signal\n");
+        }
+
         // Throw away first block. It is noisy because IF filters
         // are still starting up.
         if (block > 0) {

plltest.py

@@ -0,0 +1,103 @@
+"""
+Test PLL algorithm.
+
+Usage: testpll.py baseband.dat centerfreq bandwidth > output.dat
+
+  baseband.dat   Raw 16-bit signed little-endian sample stream
+  centerfreq     Center frequency relative to sample frequency (0.5 = Nyquist)
+  bandwidth      Approximate bandwidth of PLL relative to sample frequency
+  output.dat     ASCII file with space-separated data
+"""
+
+import sys
+import numpy
+
+
+def pll(d, centerfreq, bandwidth):
+
+    minfreq = (centerfreq - bandwidth) * 2 * numpy.pi
+    maxfreq = (centerfreq + bandwidth) * 2 * numpy.pi
+
+    w = bandwidth * 2 * numpy.pi
+    phasor_a = numpy.poly([ numpy.exp(-1.146*w), numpy.exp(-5.331*w) ])
+    phasor_b = numpy.array([ sum(phasor_a) ])
+
+    loopfilter_b = numpy.poly([ numpy.exp(-0.1153*w) ])
+    loopfilter_b *= 0.62 * w
+
+    n = len(d)
+    y = numpy.zeros(n)
+    phasei = numpy.zeros(n)
+    phaseq = numpy.zeros(n)
+    phaseerr = numpy.zeros(n)
+    freq = numpy.zeros(n)
+    phase = numpy.zeros(n)
+    freq[0] = centerfreq * 2 * numpy.pi
+
+    phasor_i1 = phasor_i2 = 0
+    phasor_q1 = phasor_q2 = 0
+    loopfilter_x1 = 0
+
+    for i in xrange(n):
+
+        psin = numpy.sin(phase[i])
+        pcos = numpy.cos(phase[i])
+        y[i] = pcos
+
+        pi = pcos * d[i]
+        pq = psin * d[i]
+
+        pi = phasor_b[0] * pi - phasor_a[1] * phasor_i1 - phasor_a[2] * phasor_i2
+        pq = phasor_b[0] * pq - phasor_a[1] * phasor_q1 - phasor_a[2] * phasor_q2
+        phasor_i2 = phasor_i1
+        phasor_i1 = pi
+        phasor_q2 = phasor_q1
+        phasor_q1 = pq
+
+        phasei[i] = pi
+        phaseq[i] = pq
+
+        if pi > abs(pq):
+            perr = pq / pi
+        elif pq > 0:
+            perr = 1
+        else:
+            perr = -1
+        phaseerr[i] = perr
+
+        dfreq = loopfilter_b[0] * perr + loopfilter_b[1] * loopfilter_x1
+        loopfilter_x1 = perr
+
+        if i + 1 < n:
+            freq[i+1] = min(maxfreq, max(minfreq, freq[i] - dfreq))
+            p = phase[i] + freq[i+1]
+            if p > 2 * numpy.pi:  p -= 2 * numpy.pi
+            if p < -2 * numpy.pi: p += 2 * numpy.pi
+            phase[i+1] = p
+
+    return y, phasei, phaseq, phaseerr, freq, phase
+
+
+def main():
+
+    if len(sys.argv) != 4:
+        print >>sys.stderr, __doc__
+        sys.exit(1)
+
+    infile      = sys.argv[1]
+    centerfreq  = float(sys.argv[2])
+    bandwidth   = float(sys.argv[3])
+
+    d = numpy.fromfile(infile, '<i2')
+    d = d.astype(numpy.float64) / 32767.0
+
+    (y, phasei, phaseq, phaseerr, freq, phase) = pll(d, centerfreq, bandwidth)
+
+    print '#output phasei, phaseq, phaseerr freq phase'
+    for i in xrange(len(y)):
+        print y[i], phasei[i], phaseq[i], phaseerr[i], freq[i], phase[i]
+
+
+if __name__ == '__main__':
+    main()
+