BFM demod: PLL lock pilot carrier in flow

include/dsp/phaselock.h

@@ -25,6 +25,14 @@ public:
     /** Expected pilot frequency (used for PPS events). */
     static const int pilot_frequency = 19000;
 
+    /** Timestamp event produced once every 19000 pilot periods. */
+    struct PpsEvent
+    {
+        quint64   pps_index;
+        quint64   sample_index;
+        double    block_position;
+    };
+
     /**
      * Construct phase-locked loop.
      *
@@ -35,12 +43,30 @@ public:
      */
     PhaseLock(Real freq, Real bandwidth, Real minsignal);
 
+    /**
+     * Change phase locked loop parameters
+     *
+     * freq       :: 19 kHz center frequency relative to sample freq
+     *               (0.5 is Nyquist)
+     * bandwidth  :: bandwidth relative to sample frequency
+     * minsignal  :: minimum pilot amplitude
+     */
+    void configure(Real freq, Real bandwidth, Real minsignal);
+
     /**
      * Process samples and extract 19 kHz pilot tone.
      * Generate phase-locked 38 kHz tone with unit amplitude.
+     * Bufferized version with input and output vectors
      */
     void process(const std::vector<Real>& samples_in, std::vector<Real>& samples_out);
 
+    /**
+     * Process samples and extract 19 kHz pilot tone.
+     * Generate phase-locked 38 kHz tone with unit amplitude.
+     * In flow version
+     */
+    void process(const Real& sample_in, Real& sample_out);
+
     /** Return true if the phase-locked loop is locked. */
     bool locked() const
     {
@@ -64,4 +90,10 @@ private:
     Real    m_pilot_level;
     int     m_lock_delay;
     int     m_lock_cnt;
+    int     m_unlock_cnt;
+    int     m_unlock_delay;
+    int     m_pilot_periods;
+    quint64 m_pps_cnt;
+    quint64 m_sample_cnt;
+    std::vector<PpsEvent> m_pps_events;
 };

plugins/channel/bfm/bfmdemod.cpp

@@ -31,7 +31,8 @@ MESSAGE_CLASS_DEFINITION(BFMDemod::MsgConfigureBFMDemod, Message)
 BFMDemod::BFMDemod(SampleSink* sampleSink) :
 	m_sampleSink(sampleSink),
 	m_audioFifo(4, 250000),
-	m_settingsMutex(QMutex::Recursive)
+	m_settingsMutex(QMutex::Recursive),
+	m_pilotPLL(19000/384000, 50/384000, 0.01)
 {
 	setObjectName("BFMDemod");
 
@@ -118,7 +119,10 @@ void BFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
 			m_m2Sample = m_m1Sample;
 			m_m1Sample = rf[i];
 
-			m_sampleBuffer.push_back(Sample(demod * (1<<15), 0.0));
+			Real pilotSample;
+			m_pilotPLL.process(demod, pilotSample);
+			//m_sampleBuffer.push_back(Sample(demod * (1<<15), 0.0));
+			m_sampleBuffer.push_back(Sample(pilotSample * (1<<15), 0.0));
 			Complex e(demod, 0);
 
 			if(m_interpolator.interpolate(&m_interpolatorDistanceRemain, e, &ci))
@@ -230,6 +234,10 @@ bool BFMDemod::handleMessage(const Message& cmd)
 
 void BFMDemod::apply()
 {
+	if (m_config.m_inputSampleRate != m_running.m_inputSampleRate)
+	{
+		m_pilotPLL.configure(19000.0/m_config.m_inputSampleRate, 50.0/m_config.m_inputSampleRate, 0.01);
+	}
 
 	if((m_config.m_inputFrequencyOffset != m_running.m_inputFrequencyOffset) ||
 		(m_config.m_inputSampleRate != m_running.m_inputSampleRate))

plugins/channel/bfm/bfmdemod.h

@@ -26,6 +26,7 @@
 #include "dsp/lowpass.h"
 #include "dsp/movingaverage.h"
 #include "dsp/fftfilt.h"
+#include "dsp/phaselock.h"
 #include "audio/audiofifo.h"
 #include "util/message.h"
 
@@ -45,6 +46,8 @@ public:
 	virtual bool handleMessage(const Message& cmd);
 
 	Real getMagSq() const { return m_movingAverage.average(); }
+	bool getPilotLock() const { return m_pilotPLL.locked(); }
+	Real getPilotLevel() const { return m_pilotPLL.get_pilot_level(); }
 
 private:
 	class MsgConfigureBFMDemod : public Message {
@@ -133,6 +136,8 @@ private:
 	SampleVector m_sampleBuffer;
 	QMutex m_settingsMutex;
 
+	PhaseLock m_pilotPLL;
+
 	void apply();
 };
 

plugins/channel/bfm/bfmdemodgui.cpp

@@ -334,4 +334,5 @@ void BFMDemodGUI::tick()
 	Real powDb = CalcDb::dbPower(m_bfmDemod->getMagSq());
 	m_channelPowerDbAvg.feed(powDb);
 	ui->channelPower->setText(QString::number(m_channelPowerDbAvg.average(), 'f', 1));
+	//qDebug() << "Pilot lock: " << m_bfmDemod->getPilotLock() << ":" << m_bfmDemod->getPilotLevel(); TODO: update a GUI item with status
 }

sdrbase/dsp/phaselock.cpp

@@ -41,19 +41,21 @@ PhaseLock::PhaseLock(Real freq, Real bandwidth, Real minsignal)
     // Set valid signal threshold.
     m_minsignal  = minsignal;
     m_lock_delay = int(20.0 / bandwidth);
+    m_unlock_delay = int(10.0 / bandwidth);
     m_lock_cnt   = 0;
+    m_unlock_cnt = 0;
     m_pilot_level = 0;
 
     // Create 2nd order filter for I/Q representation of phase error.
     // Filter has two poles, unit DC gain.
-    Real p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
+    double p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
-    Real p2 = exp(-5.331 * bandwidth * 2.0 * M_PI);
+    double p2 = exp(-5.331 * bandwidth * 2.0 * M_PI);
     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.
-    Real q1 = exp(-0.1153 * bandwidth * 2.0 * M_PI);
+    double q1 = exp(-0.1153 * bandwidth * 2.0 * M_PI);
     m_loopfilter_b0 = 0.62 * bandwidth * 2.0 * M_PI;
     m_loopfilter_b1 = - m_loopfilter_b0 * q1;
 
@@ -70,10 +72,75 @@ PhaseLock::PhaseLock(Real freq, Real bandwidth, Real minsignal)
     m_phasor_q1 = 0;
     m_phasor_q2 = 0;
     m_loopfilter_x1 = 0;
+
+    // Initialize PPS generator.
+    m_pilot_periods = 0;
+    m_pps_cnt       = 0;
+    m_sample_cnt    = 0;
 }
 
+void PhaseLock::configure(Real freq, Real bandwidth, Real minsignal)
+{
+	qDebug("PhaseLock::configure: freq: %f bandwidth: %f minsignal: %f", freq, bandwidth, minsignal);
+    /*
+     * This is a type-2, 4th order phase-locked loop.
+     *
+     * Open-loop transfer function:
+     *   G(z) = K * (z - q1) / ((z - p1) * (z - p2) * (z - 1) * (z - 1))
+     *   K  = 3.788 * (bandwidth * 2 * Pi)**3
+     *   q1 = exp(-0.1153 * 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 will work.
+     */
 
-// Process samples.
+    // Set min/max locking frequencies.
+    m_minfreq = (freq - bandwidth) * 2.0 * M_PI;
+    m_maxfreq = (freq + bandwidth) * 2.0 * M_PI;
+
+    // Set valid signal threshold.
+    m_minsignal  = minsignal;
+    m_lock_delay = int(20.0 / bandwidth);
+    m_unlock_delay = int(10.0 / bandwidth);
+    m_lock_cnt   = 0;
+    m_unlock_cnt = 0;
+    m_pilot_level = 0;
+
+    // Create 2nd order filter for I/Q representation of phase error.
+    // Filter has two poles, unit DC gain.
+    double p1 = exp(-1.146 * bandwidth * 2.0 * M_PI);
+    double p2 = exp(-5.331 * bandwidth * 2.0 * M_PI);
+    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.
+    double q1 = exp(-0.1153 * bandwidth * 2.0 * M_PI);
+    m_loopfilter_b0 = 0.62 * 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 integrators form the two remaining poles, both at z = 1.
+
+    // Initialize frequency and phase.
+    m_freq  = freq * 2.0 * M_PI;
+    m_phase = 0;
+
+    m_phasor_i1 = 0;
+    m_phasor_i2 = 0;
+    m_phasor_q1 = 0;
+    m_phasor_q2 = 0;
+    m_loopfilter_x1 = 0;
+
+    // Initialize PPS generator.
+    m_pilot_periods = 0;
+    m_pps_cnt       = 0;
+    m_sample_cnt    = 0;
+}
+
+// Process samples. Bufferized version
 void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>& samples_out)
 {
     unsigned int n = samples_in.size();
@@ -81,6 +148,7 @@ void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>&
     samples_out.resize(n);
 
     bool was_locked = (m_lock_cnt >= m_lock_delay);
+    m_pps_events.clear();
 
     if (n > 0)
         m_pilot_level = 1000.0;
@@ -141,6 +209,20 @@ void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>&
         m_phase += m_freq;
         if (m_phase > 2.0 * M_PI) {
             m_phase -= 2.0 * M_PI;
+            m_pilot_periods++;
+
+            // Generate pulse-per-second.
+            if (m_pilot_periods == pilot_frequency) {
+                m_pilot_periods = 0;
+                if (was_locked) {
+                    struct PpsEvent ev;
+                    ev.pps_index      = m_pps_cnt;
+                    ev.sample_index   = m_sample_cnt + i;
+                    ev.block_position = double(i) / double(n);
+                    m_pps_events.push_back(ev);
+                    m_pps_cnt++;
+                }
+            }
         }
     }
 
@@ -152,4 +234,146 @@ void PhaseLock::process(const std::vector<Real>& samples_in, std::vector<Real>&
         m_lock_cnt = 0;
     }
 
+    // Drop PPS events when pilot not locked.
+    if (m_lock_cnt < m_lock_delay) {
+        m_pilot_periods = 0;
+        m_pps_cnt = 0;
+        m_pps_events.clear();
+    }
+
+    // Update sample counter.
+    m_sample_cnt += n;
+}
+
+/*
+void PhaseLock::process(const Real& sample_in, Real& sample_out)
+{
+	m_phase += m_freq;
+
+	if (m_phase > 2.0 * M_PI) {
+		m_phase -= 2.0 * M_PI;
+	}
+
+	Real psin = sin(m_phase);
+	Real pcos = cos(m_phase);
+
+	sample_out = 2 * psin * pcos;
+}*/
+
+
+// Process samples.
+void PhaseLock::process(const Real& sample_in, Real& sample_out)
+{
+    bool was_locked = (m_lock_cnt >= m_lock_delay);
+    m_pps_events.clear();
+
+    //if (n > 0) m_pilot_level = 1000.0;
+
+    {
+
+        // Generate locked pilot tone.
+    	Real psin = sin(m_phase);
+    	Real pcos = cos(m_phase);
+
+        // Generate double-frequency output.
+        // sin(2*x) = 2 * sin(x) * cos(x)
+        sample_out = 2 * psin * pcos;
+
+        // Multiply locked tone with input.
+        Real x = sample_in;
+        Real phasor_i = psin * x;
+        Real phasor_q = pcos * x;
+
+        // Run IQ phase error through low-pass filter.
+        phasor_i = m_phasor_b0 * phasor_i
+                   - m_phasor_a1 * m_phasor_i1
+                   - m_phasor_a2 * m_phasor_i2;
+        phasor_q = m_phasor_b0 * phasor_q
+                   - m_phasor_a1 * m_phasor_q1
+                   - m_phasor_a2 * m_phasor_q2;
+        m_phasor_i2 = m_phasor_i1;
+        m_phasor_i1 = phasor_i;
+        m_phasor_q2 = m_phasor_q1;
+        m_phasor_q1 = phasor_q;
+
+        // Convert I/Q ratio to estimate of phase error.
+        Real phase_err;
+        if (phasor_i > abs(phasor_q)) {
+            // We are within +/- 45 degrees from lock.
+            // Use simple linear approximation of arctan.
+            phase_err = phasor_q / phasor_i;
+        } else if (phasor_q > 0) {
+            // We are lagging more than 45 degrees behind the input.
+            phase_err = 1;
+        } else {
+            // We are more than 45 degrees ahead of the input.
+            phase_err = -1;
+        }
+
+        // Detect pilot level (conservative).
+        // m_pilot_level = std::min(m_pilot_level, phasor_i);
+        m_pilot_level = phasor_i;
+
+        // Run phase error through loop filter and update frequency estimate.
+        m_freq += m_loopfilter_b0 * phase_err
+                  + m_loopfilter_b1 * m_loopfilter_x1;
+        m_loopfilter_x1 = phase_err;
+
+        // Limit frequency to allowable range.
+        m_freq = std::max(m_minfreq, std::min(m_maxfreq, m_freq));
+
+        // Update locked phase.
+        m_phase += m_freq;
+        if (m_phase > 2.0 * M_PI) {
+            m_phase -= 2.0 * M_PI;
+            m_pilot_periods++;
+
+            // Generate pulse-per-second.
+            if (m_pilot_periods == pilot_frequency) {
+                m_pilot_periods = 0;
+                //if (was_locked) {
+                //    struct PpsEvent ev;
+                //    ev.pps_index      = m_pps_cnt;
+                //    ev.sample_index   = m_sample_cnt + i;
+                //    ev.block_position = double(i) / double(n);
+                //    m_pps_events.push_back(ev);
+                //    m_pps_cnt++;
+                //}
+            }
+        }
+    }
+
+    // Update lock status.
+    if (2 * m_pilot_level > m_minsignal)
+    {
+        if (m_lock_cnt < m_lock_delay)
+        {
+            m_lock_cnt += 1; // n
+        }
+        else
+        {
+        	m_unlock_cnt = 0;
+        }
+    }
+    else
+    {
+    	if (m_unlock_cnt < m_unlock_delay)
+    	{
+    		m_unlock_cnt += 1;
+    	}
+    	else
+    	{
+    		m_lock_cnt = 0;
+    	}
+    }
+
+    // Drop PPS events when pilot not locked.
+    if (m_lock_cnt < m_lock_delay) {
+        m_pilot_periods = 0;
+        m_pps_cnt = 0;
+        m_pps_events.clear();
+    }
+
+    // Update sample counter.
+    m_sample_cnt += 1; // n
 }