diff --git a/include/dsp/phaselock.h b/include/dsp/phaselock.h index 1bb389ae3..fa2d44370 100644 --- a/include/dsp/phaselock.h +++ b/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& samples_in, std::vector& 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 m_pps_events; }; diff --git a/plugins/channel/bfm/bfmdemod.cpp b/plugins/channel/bfm/bfmdemod.cpp index 0ac49a49e..b1d64bcbd 100644 --- a/plugins/channel/bfm/bfmdemod.cpp +++ b/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)) diff --git a/plugins/channel/bfm/bfmdemod.h b/plugins/channel/bfm/bfmdemod.h index 6419d3f7e..edf2d820c 100644 --- a/plugins/channel/bfm/bfmdemod.h +++ b/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(); }; diff --git a/plugins/channel/bfm/bfmdemodgui.cpp b/plugins/channel/bfm/bfmdemodgui.cpp index 95de9835c..7c3aff37c 100644 --- a/plugins/channel/bfm/bfmdemodgui.cpp +++ b/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 } diff --git a/sdrbase/dsp/phaselock.cpp b/sdrbase/dsp/phaselock.cpp index 72cf5b63d..875df9a98 100644 --- a/sdrbase/dsp/phaselock.cpp +++ b/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); - Real p2 = exp(-5.331 * bandwidth * 2.0 * M_PI); + 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. - 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& samples_in, std::vector& samples_out) { unsigned int n = samples_in.size(); @@ -81,6 +148,7 @@ void PhaseLock::process(const std::vector& samples_in, std::vector& 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& samples_in, std::vector& 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& samples_in, std::vector& 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 }