diff --git a/include/dsp/phaselock.h b/include/dsp/phaselock.h
index fa2d44370..4776d139c 100644
--- a/include/dsp/phaselock.h
+++ b/include/dsp/phaselock.h
@@ -15,6 +15,7 @@
// along with this program. If not, see . //
///////////////////////////////////////////////////////////////////////////////////
+#include
#include "dsp/dsptypes.h"
/** Phase-locked loop mainly for broadcadt FM stereo pilot. */
@@ -43,6 +44,9 @@ public:
*/
PhaseLock(Real freq, Real bandwidth, Real minsignal);
+ virtual ~PhaseLock()
+ {}
+
/**
* Change phase locked loop parameters
*
@@ -61,12 +65,21 @@ public:
void process(const std::vector& samples_in, std::vector& samples_out);
/**
- * Process samples and extract 19 kHz pilot tone.
- * Generate phase-locked 38 kHz tone with unit amplitude.
+ * Process samples and extract pilot tone. Generate phase-locked twice
+ * the frequency tone with unit amplitude. Mostly useful for 19 kHz stereo
+ * pilot tone on broadcast FM.
* In flow version
*/
void process(const Real& sample_in, Real& sample_out);
+ /**
+ * Process samples and track a pilot tone. Generate samples for multiple phase-locked
+ * signals. Implement the processPhase virtual method to produce the output samples.
+ * In flow version. Ex: Use 19 kHz stereo pilot tone to generate 38 kHz (stereo) and 57 kHz
+ * pilots (see RDSPhaseLock class below).
+ */
+ void process(const Real& sample_in, std::vector& samples_out);
+
/** Return true if the phase-locked loop is locked. */
bool locked() const
{
@@ -79,13 +92,23 @@ public:
return 2 * m_pilot_level;
}
+protected:
+ Real m_phase;
+ Real m_psin;
+ Real m_pcos;
+ /**
+ * Callback method to produce multiple outputs from the current phase value in m_phase
+ * and/or the sin and cos values in m_psin and m_pcos
+ */
+ virtual void processPhase(std::vector& samples_out) const {};
+
private:
Real m_minfreq, m_maxfreq;
Real m_phasor_b0, m_phasor_a1, m_phasor_a2;
Real m_phasor_i1, m_phasor_i2, m_phasor_q1, m_phasor_q2;
Real m_loopfilter_b0, m_loopfilter_b1;
Real m_loopfilter_x1;
- Real m_freq, m_phase;
+ Real m_freq;
Real m_minsignal;
Real m_pilot_level;
int m_lock_delay;
@@ -97,3 +120,45 @@ private:
quint64 m_sample_cnt;
std::vector m_pps_events;
};
+
+class StereoPhaseLock : public PhaseLock
+{
+public:
+ StereoPhaseLock(Real freq, Real bandwidth, Real minsignal) :
+ PhaseLock(freq, bandwidth, minsignal)
+ {}
+
+ virtual ~StereoPhaseLock()
+ {}
+
+protected:
+ virtual void processPhase(std::vector& samples_out) const
+ {
+ samples_out[0] = m_psin; // f Pilot
+ // Generate double-frequency output.
+ // sin(2*x) = 2 * sin(x) * cos(x)
+ samples_out[1] = 2.0 * m_psin * m_pcos; // 2f Pilot
+ }
+};
+
+
+class RDSPhaseLock : public PhaseLock
+{
+public:
+ RDSPhaseLock(Real freq, Real bandwidth, Real minsignal) :
+ PhaseLock(freq, bandwidth, minsignal)
+ {}
+
+ virtual ~RDSPhaseLock()
+ {}
+
+protected:
+ virtual void processPhase(std::vector& samples_out) const
+ {
+ samples_out[0] = m_psin; // f Pilot
+ // Generate double-frequency output.
+ // sin(2*x) = 2 * sin(x) * cos(x)
+ samples_out[1] = 2.0 * m_psin * m_pcos; // 2f Pilot
+ samples_out[2] = sin(3.0 * m_phase); // 3f pilot
+ }
+};
diff --git a/sdrbase/dsp/phaselock.cpp b/sdrbase/dsp/phaselock.cpp
index 875df9a98..b0fdff8c7 100644
--- a/sdrbase/dsp/phaselock.cpp
+++ b/sdrbase/dsp/phaselock.cpp
@@ -45,6 +45,8 @@ PhaseLock::PhaseLock(Real freq, Real bandwidth, Real minsignal)
m_lock_cnt = 0;
m_unlock_cnt = 0;
m_pilot_level = 0;
+ m_psin = 0.0;
+ m_pcos = 1.0;
// Create 2nd order filter for I/Q representation of phase error.
// Filter has two poles, unit DC gain.
@@ -377,3 +379,108 @@ void PhaseLock::process(const Real& sample_in, Real& sample_out)
// Update sample counter.
m_sample_cnt += 1; // n
}
+
+// Process samples. Multiple output
+void PhaseLock::process(const Real& sample_in, std::vector& samples_out)
+{
+ bool was_locked = (m_lock_cnt >= m_lock_delay);
+ m_pps_events.clear();
+
+ // Generate locked pilot tone.
+ m_psin = sin(m_phase);
+ m_pcos = cos(m_phase);
+
+ // Generate output
+ processPhase(samples_out);
+
+ // Multiply locked tone with input.
+ Real x = sample_in;
+ Real phasor_i = m_psin * x;
+ Real phasor_q = m_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;
+ }
+ }
+
+ // 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
+}