From f119f2f72c59327a81a0d7db80958ceff540c015 Mon Sep 17 00:00:00 2001 From: Joris van Rantwijk Date: Tue, 31 Dec 2013 00:07:14 +0100 Subject: [PATCH] Stereo pilot PLL now working. (Stereo decoding not yet implemented.) --- FmDecode.cc | 130 ++++++++++++++++++++++++++++++++++++++-------------- FmDecode.h | 33 +++++++++++-- main.cc | 12 ++++- plltest.py | 103 +++++++++++++++++++++++++++++++++++++++++ 4 files changed, 237 insertions(+), 41 deletions(-) create mode 100644 plltest.py diff --git a/FmDecode.cc b/FmDecode.cc index 0eb9ff9..75b389b 100644 --- a/FmDecode.cc +++ b/FmDecode.cc @@ -1,4 +1,5 @@ +#include #include #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)) - * K = 3.125 * (bandwidth * 2 * PI)**3 - * Qz = exp(-0.2 * bandwidth * 2*PI) - * Qp = exp(-2.5 * bandwidth * 2*PI) + * 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 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). - double t = exp(-2.5 * bandwidth * 2.0 * M_PI); - m_phasor_a1 = -2.0 * t; - m_phasor_a2 = t * t; + // 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. - // Filter has one Zero at z = exp(-0.2 * bandwidth * 2*PI). - m_loopfilter_b0 = 0.5 * bandwidth * 2.0 * M_PI; - m_loopfilter_b1 = - m_loopfilter_b0 * exp(-0.2 * 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; // 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_q = psin * x; + Sample phasor_i = 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. - if (phasor_i > m_minsignal) { - m_lock_cnt++; - } else { - m_lock_cnt = 0; - } + // Detect pilot level (conservative). + m_pilot_level = min(m_pilot_level, phasor_i); // 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) - m_phase += 2.0 * M_PI; - else if (m_phase > 2.0 * M_PI) + 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), - bandwidth_pcm * downsample / sample_rate_if, - sample_rate_if / downsample / sample_rate_pcm, false) + , m_resample_mono( + 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 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 */ diff --git a/FmDecode.h b/FmDecode.h index 062d9b4..8087732 100644 --- a/FmDecode.h +++ b/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; }; diff --git a/main.cc b/main.cc index 0e3c081..9199385 100644 --- a/main.cc +++ b/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) { diff --git a/plltest.py b/plltest.py new file mode 100644 index 0000000..ecfd3d5 --- /dev/null +++ b/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, '