1
0
Fork 0

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

This commit is contained in:
Joris van Rantwijk 2013-12-31 00:07:14 +01:00
parent 620dba5d13
commit f119f2f72c
4 changed files with 237 additions and 41 deletions

View File

@ -1,4 +1,5 @@
#include <cassert>
#include <cmath> #include <cmath>
#include "FmDecode.h" #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. * This is a type-2, 4th order phase-locked loop.
* *
* Open-loop transfer function: * 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. // Set min/max locking frequencies.
@ -104,24 +106,26 @@ PilotPhaseLock::PilotPhaseLock(double freq, double bandwidth, double minsignal)
m_minsignal = minsignal; m_minsignal = minsignal;
m_lock_delay = int(10.0 / bandwidth); m_lock_delay = int(10.0 / bandwidth);
m_lock_cnt = 0; m_lock_cnt = 0;
m_pilot_level = 0;
// Create 2nd order filter for I/Q representation of phase error. // 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; m_phasor_b0 = 1 + m_phasor_a1 + m_phasor_a2;
// Create loop filter to stabilize the loop. // 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 // After the loop filter, the phase error is integrated to produce
// the frequency. Then the frequency is integrated to produce the phase. // 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_freq = freq * 2.0 * M_PI;
m_phase = 0; m_phase = 0;
@ -129,6 +133,7 @@ PilotPhaseLock::PilotPhaseLock(double freq, double bandwidth, double minsignal)
m_phasor_i2 = 0; m_phasor_i2 = 0;
m_phasor_q1 = 0; m_phasor_q1 = 0;
m_phasor_q2 = 0; m_phasor_q2 = 0;
m_loopfilter_x1 = 0;
} }
@ -140,17 +145,23 @@ void PilotPhaseLock::process(const SampleVector& samples_in,
samples_out.resize(n); samples_out.resize(n);
if (n > 0)
m_pilot_level = 1000.0;
for (unsigned int i = 0; i < n; i++) { for (unsigned int i = 0; i < n; i++) {
// Generate locked pilot tone. // Generate locked pilot tone.
Sample psin = sin(m_phase); Sample psin = sin(m_phase);
Sample pcos = cos(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. // Multiply locked tone with input.
Sample x = samples_in[i]; 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. // Run IQ phase error through low-pass filter.
phasor_i = m_phasor_b0 * phasor_i phasor_i = m_phasor_b0 * phasor_i
@ -171,22 +182,18 @@ void PilotPhaseLock::process(const SampleVector& samples_in,
// Use simple linear approximation of arctan. // Use simple linear approximation of arctan.
phase_err = phasor_q / phasor_i; phase_err = phasor_q / phasor_i;
} else if (phasor_q > 0) { } 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; phase_err = 1;
} else { } else {
// We are lagging more than 45 degrees behind the input. // We are more than 45 degrees ahead of the input.
phase_err = -1; 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. // 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_b1 * m_loopfilter_x1;
m_loopfilter_x1 = phase_err; m_loopfilter_x1 = phase_err;
@ -195,13 +202,17 @@ void PilotPhaseLock::process(const SampleVector& samples_in,
// Update locked phase. // Update locked phase.
m_phase += m_freq; 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_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 // Construct DownsampleFilter for baseband
, m_resample_baseband(8 * downsample, 0.4 / downsample, downsample, true) , 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 // 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 // Construct HighPassFilterIir
, m_dcblock_mono(30.0 / sample_rate_pcm) , 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_mean = 0.95 * m_baseband_mean + 0.05 * baseband_mean;
m_baseband_level = 0.95 * m_baseband_level + 0.05 * baseband_rms; m_baseband_level = 0.95 * m_baseband_level + 0.05 * baseband_rms;
// TODO : stereo decoding
// Extract mono audio signal. // Extract mono audio signal.
m_resample_mono.process(m_buf_baseband, m_buf_mono); 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_dcblock_mono.processInPlace(m_buf_mono);
m_deemph_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 // TODO : stereo mixing
audio = move(m_buf_mono); 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 */ /* end */

View File

@ -36,17 +36,22 @@ class PilotPhaseLock
{ {
public: public:
// TODO : pulse-per-second
/** /**
* Construct phase-locked loop. * 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) * (0.5 is Nyquist)
* bandwidth :: approximate bandwidth relative to sample frequency * bandwidth :: bandwidth relative to sample frequency
* minsignal :: minimum pilot amplitude * minsignal :: minimum pilot amplitude
*/ */
PilotPhaseLock(double freq, double bandwidth, double minsignal); 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); void process(const SampleVector& samples_in, SampleVector& samples_out);
/** Return true if the phase-locked loop is locked. */ /** Return true if the phase-locked loop is locked. */
@ -55,6 +60,12 @@ public:
return m_lock_cnt >= m_lock_delay; return m_lock_cnt >= m_lock_delay;
} }
/** Return detected amplitude of pilot signal. */
double get_pilot_level() const
{
return 2 * m_pilot_level;
}
private: private:
Sample m_minfreq, m_maxfreq; Sample m_minfreq, m_maxfreq;
Sample m_phasor_b0, m_phasor_a1, m_phasor_a2; Sample m_phasor_b0, m_phasor_a1, m_phasor_a2;
@ -63,6 +74,7 @@ private:
Sample m_loopfilter_x1; Sample m_loopfilter_x1;
Sample m_freq, m_phase; Sample m_freq, m_phase;
Sample m_minsignal; Sample m_minsignal;
Sample m_pilot_level;
int m_lock_delay; int m_lock_delay;
int m_lock_cnt; int m_lock_cnt;
}; };
@ -144,9 +156,18 @@ public:
return m_baseband_level; 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: 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 double m_sample_rate_if;
const int m_tuning_table_size; const int m_tuning_table_size;
const int m_tuning_shift; const int m_tuning_shift;
@ -162,12 +183,16 @@ private:
IQSampleVector m_buf_iffiltered; IQSampleVector m_buf_iffiltered;
SampleVector m_buf_baseband; SampleVector m_buf_baseband;
SampleVector m_buf_mono; SampleVector m_buf_mono;
SampleVector m_buf_rawstereo;
SampleVector m_buf_stereo;
FineTuner m_finetuner; FineTuner m_finetuner;
LowPassFilterFirIQ m_iffilter; LowPassFilterFirIQ m_iffilter;
PhaseDiscriminator m_phasedisc; PhaseDiscriminator m_phasedisc;
DownsampleFilter m_resample_baseband; DownsampleFilter m_resample_baseband;
PilotPhaseLock m_pilotpll;
DownsampleFilter m_resample_mono; DownsampleFilter m_resample_mono;
DownsampleFilter m_resample_stereo;
HighPassFilterIir m_dcblock_mono; HighPassFilterIir m_dcblock_mono;
LowPassFilterRC m_deemph_mono; LowPassFilterRC m_deemph_mono;
}; };

12
main.cc
View File

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

103
plltest.py Normal file
View File

@ -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()